AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-16 06:30:05 +00:00
Code Issues Releases Wiki Activity

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
Erwin Coumans 2018-10-05 20:01:37 -07:00
commit f90fbed54e
1830 changed files with 1094089 additions and 477295 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
CMakeLists.txt
View File

@ -369,6 +369,13 @@ IF(BUILD_PYBULLET)
ENDIF(BUILD_PYBULLET)
IF(NOT WIN32 AND NOT APPLE)
OPTION(BUILD_EGL "Build OpenGL/EGL" ON)
IF(BUILD_EGL)
ADD_DEFINITIONS(-DBT_USE_EGL)
ENDIF(BUILD_EGL)
ENDIF()
IF(BUILD_BULLET3)
IF(APPLE)
MESSAGE("Mac OSX Version is ${_CURRENT_OSX_VERSION}")

205
Extras/ConvexDecomposition/ConvexBuilder.cpp
View File

@ -8,34 +8,32 @@
#include "fitsphere.h"
#include "bestfitobb.h"
unsigned int MAXDEPTH = 8 ;
float CONCAVE_PERCENT = 1.0f ;
float MERGE_PERCENT = 2.0f ;
unsigned int MAXDEPTH = 8;
float CONCAVE_PERCENT = 1.0f;
float MERGE_PERCENT = 2.0f;
CHull::CHull(const ConvexDecomposition::ConvexResult &result)
{
mResult = new ConvexDecomposition::ConvexResult(result);
mVolume = computeMeshVolume( result.mHullVertices, result.mHullTcount, result.mHullIndices );
mVolume = computeMeshVolume(result.mHullVertices, result.mHullTcount, result.mHullIndices);
mDiagonal = getBoundingRegion( result.mHullVcount, result.mHullVertices, sizeof(float)*3, mMin, mMax );
mDiagonal = getBoundingRegion(result.mHullVcount, result.mHullVertices, sizeof(float) * 3, mMin, mMax);
float dx = mMax[0] - mMin[0];
float dy = mMax[1] - mMin[1];
float dz = mMax[2] - mMin[2];
dx*=0.1f; // inflate 1/10th on each edge
dy*=0.1f; // inflate 1/10th on each edge
dz*=0.1f; // inflate 1/10th on each edge
mMin[0]-=dx;
mMin[1]-=dy;
mMin[2]-=dz;
mMax[0]+=dx;
mMax[1]+=dy;
mMax[2]+=dz;
dx *= 0.1f; // inflate 1/10th on each edge
dy *= 0.1f; // inflate 1/10th on each edge
dz *= 0.1f; // inflate 1/10th on each edge
mMin[0] -= dx;
mMin[1] -= dy;
mMin[2] -= dz;
mMax[0] += dx;
mMax[1] += dy;
mMax[2] += dz;
}
CHull::~CHull(void)
@ -45,12 +43,9 @@ CHull::~CHull(void)
bool CHull::overlap(const CHull &h) const
{
return overlapAABB(mMin,mMax, h.mMin, h.mMax );
return overlapAABB(mMin, mMax, h.mMin, h.mMax);
}
ConvexBuilder::ConvexBuilder(ConvexDecompInterface *callback)
{
mCallback = callback;
@ -59,45 +54,45 @@ ConvexBuilder::ConvexBuilder(ConvexDecompInterface *callback)
ConvexBuilder::~ConvexBuilder(void)
{
int i;
for (i=0;i<mChulls.size();i++)
for (i = 0; i < mChulls.size(); i++)
{
CHull *cr = mChulls[i];
delete cr;
}
}
bool ConvexBuilder::isDuplicate(unsigned int i1,unsigned int i2,unsigned int i3,
unsigned int ci1,unsigned int ci2,unsigned int ci3)
bool ConvexBuilder::isDuplicate(unsigned int i1, unsigned int i2, unsigned int i3,
unsigned int ci1, unsigned int ci2, unsigned int ci3)
{
unsigned int dcount = 0;
assert( i1 != i2 && i1 != i3 && i2 != i3 );
assert( ci1 != ci2 && ci1 != ci3 && ci2 != ci3 );
assert(i1 != i2 && i1 != i3 && i2 != i3);
assert(ci1 != ci2 && ci1 != ci3 && ci2 != ci3);
if ( i1 == ci1 || i1 == ci2 || i1 == ci3 ) dcount++;
if ( i2 == ci1 || i2 == ci2 || i2 == ci3 ) dcount++;
if ( i3 == ci1 || i3 == ci2 || i3 == ci3 ) dcount++;
if (i1 == ci1 || i1 == ci2 || i1 == ci3) dcount++;
if (i2 == ci1 || i2 == ci2 || i2 == ci3) dcount++;
if (i3 == ci1 || i3 == ci2 || i3 == ci3) dcount++;
return dcount == 3;
}
void ConvexBuilder::getMesh(const ConvexDecomposition::ConvexResult &cr,VertexLookup vc,UintVector &indices)
void ConvexBuilder::getMesh(const ConvexDecomposition::ConvexResult &cr, VertexLookup vc, UintVector &indices)
{
unsigned int *src = cr.mHullIndices;
for (unsigned int i=0; i<cr.mHullTcount; i++)
for (unsigned int i = 0; i < cr.mHullTcount; i++)
{
unsigned int i1 = *src++;
unsigned int i2 = *src++;
unsigned int i3 = *src++;
const float *p1 = &cr.mHullVertices[i1*3];
const float *p2 = &cr.mHullVertices[i2*3];
const float *p3 = &cr.mHullVertices[i3*3];
const float *p1 = &cr.mHullVertices[i1 * 3];
const float *p2 = &cr.mHullVertices[i2 * 3];
const float *p3 = &cr.mHullVertices[i3 * 3];
i1 = Vl_getIndex(vc,p1);
i2 = Vl_getIndex(vc,p2);
i3 = Vl_getIndex(vc,p3);
i1 = Vl_getIndex(vc, p1);
i2 = Vl_getIndex(vc, p2);
i3 = Vl_getIndex(vc, p3);
#if 0
bool duplicate = false;
@ -122,14 +117,12 @@ void ConvexBuilder::getMesh(const ConvexDecomposition::ConvexResult &cr,VertexLo
indices.push_back(i3);
}
#endif
}
}
CHull * ConvexBuilder::canMerge(CHull *a,CHull *b)
CHull *ConvexBuilder::canMerge(CHull *a, CHull *b)
{
if ( !a->overlap(*b) ) return 0; // if their AABB's (with a little slop) don't overlap, then return.
if (!a->overlap(*b)) return 0; // if their AABB's (with a little slop) don't overlap, then return.
CHull *ret = 0;
@ -140,47 +133,45 @@ CHull * ConvexBuilder::canMerge(CHull *a,CHull *b)
UintVector indices;
getMesh( *a->mResult, vc, indices );
getMesh( *b->mResult, vc, indices );
getMesh(*a->mResult, vc, indices);
getMesh(*b->mResult, vc, indices);
unsigned int vcount = Vl_getVcount(vc);
const float *vertices = Vl_getVertices(vc);
unsigned int tcount = indices.size()/3;
unsigned int tcount = indices.size() / 3;
//don't do anything if hull is empty
if (!tcount)
{
Vl_releaseVertexLookup (vc);
Vl_releaseVertexLookup(vc);
return 0;
}
ConvexDecomposition::HullResult hresult;
ConvexDecomposition::HullLibrary hl;
ConvexDecomposition::HullDesc desc;
ConvexDecomposition::HullDesc desc;
desc.SetHullFlag(ConvexDecomposition::QF_TRIANGLES);
desc.mVcount = vcount;
desc.mVertices = vertices;
desc.mVertexStride = sizeof(float)*3;
desc.mVcount = vcount;
desc.mVertices = vertices;
desc.mVertexStride = sizeof(float) * 3;
ConvexDecomposition::HullError hret = hl.CreateConvexHull(desc,hresult);
ConvexDecomposition::HullError hret = hl.CreateConvexHull(desc, hresult);
if ( hret == ConvexDecomposition::QE_OK )
if (hret == ConvexDecomposition::QE_OK)
{
float combineVolume = computeMeshVolume(hresult.mOutputVertices, hresult.mNumFaces, hresult.mIndices);
float sumVolume = a->mVolume + b->mVolume;
float combineVolume = computeMeshVolume( hresult.mOutputVertices, hresult.mNumFaces, hresult.mIndices );
float sumVolume = a->mVolume + b->mVolume;
float percent = (sumVolume*100) / combineVolume;
if ( percent >= (100.0f-MERGE_PERCENT) )
float percent = (sumVolume * 100) / combineVolume;
if (percent >= (100.0f - MERGE_PERCENT))
{
ConvexDecomposition::ConvexResult cr(hresult.mNumOutputVertices, hresult.mOutputVertices, hresult.mNumFaces, hresult.mIndices);
ret = new CHull(cr);
}
}
Vl_releaseVertexLookup(vc);
return ret;
@ -188,41 +179,36 @@ CHull * ConvexBuilder::canMerge(CHull *a,CHull *b)
bool ConvexBuilder::combineHulls(void)
{
bool combine = false;
sortChulls(mChulls); // sort the convex hulls, largest volume to least...
CHullVector output; // the output hulls...
sortChulls(mChulls); // sort the convex hulls, largest volume to least...
CHullVector output; // the output hulls...
int i;
for (i=0;i<mChulls.size() && !combine; ++i)
for (i = 0; i < mChulls.size() && !combine; ++i)
{
CHull *cr = mChulls[i];
int j;
for (j=0;j<mChulls.size();j++)
for (j = 0; j < mChulls.size(); j++)
{
CHull *match = mChulls[j];
if ( cr != match ) // don't try to merge a hull with itself, that be stoopid
if (cr != match) // don't try to merge a hull with itself, that be stoopid
{
CHull *merge = canMerge(cr, match); // if we can merge these two....
CHull *merge = canMerge(cr,match); // if we can merge these two....
if ( merge )
if (merge)
{
output.push_back(merge);
++i;
while ( i != mChulls.size() )
while (i != mChulls.size())
{
CHull *cr = mChulls[i];
if ( cr != match )
if (cr != match)
{
output.push_back(cr);
}
@ -237,7 +223,7 @@ bool ConvexBuilder::combineHulls(void)
}
}
if ( combine )
if (combine)
{
break;
}
@ -245,88 +231,82 @@ bool ConvexBuilder::combineHulls(void)
{
output.push_back(cr);
}
}
if ( combine )
if (combine)
{
mChulls.clear();
mChulls.copyFromArray(output);
output.clear();
}
return combine;
}
unsigned int ConvexBuilder::process(const ConvexDecomposition::DecompDesc &desc)
{
unsigned int ret = 0;
MAXDEPTH = desc.mDepth;
MAXDEPTH = desc.mDepth;
CONCAVE_PERCENT = desc.mCpercent;
MERGE_PERCENT = desc.mPpercent;
MERGE_PERCENT = desc.mPpercent;
calcConvexDecomposition(desc.mVcount, desc.mVertices, desc.mTcount, desc.mIndices, this, 0, 0);
calcConvexDecomposition(desc.mVcount, desc.mVertices, desc.mTcount, desc.mIndices,this,0,0);
while ( combineHulls() ); // keep combinging hulls until I can't combine any more...
while (combineHulls())
; // keep combinging hulls until I can't combine any more...
int i;
for (i=0;i<mChulls.size();i++)
for (i = 0; i < mChulls.size(); i++)
{
CHull *cr = mChulls[i];
// before we hand it back to the application, we need to regenerate the hull based on the
// limits given by the user.
const ConvexDecomposition::ConvexResult &c = *cr->mResult; // the high resolution hull...
const ConvexDecomposition::ConvexResult &c = *cr->mResult; // the high resolution hull...
ConvexDecomposition::HullResult result;
ConvexDecomposition::HullLibrary hl;
ConvexDecomposition::HullDesc hdesc;
ConvexDecomposition::HullDesc hdesc;
hdesc.SetHullFlag(ConvexDecomposition::QF_TRIANGLES);
hdesc.mVcount = c.mHullVcount;
hdesc.mVertices = c.mHullVertices;
hdesc.mVertexStride = sizeof(float)*3;
hdesc.mMaxVertices = desc.mMaxVertices; // maximum number of vertices allowed in the output
hdesc.mVcount = c.mHullVcount;
hdesc.mVertices = c.mHullVertices;
hdesc.mVertexStride = sizeof(float) * 3;
hdesc.mMaxVertices = desc.mMaxVertices; // maximum number of vertices allowed in the output
if ( desc.mSkinWidth )
if (desc.mSkinWidth)
{
hdesc.mSkinWidth = desc.mSkinWidth;
hdesc.SetHullFlag(ConvexDecomposition::QF_SKIN_WIDTH); // do skin width computation.
hdesc.SetHullFlag(ConvexDecomposition::QF_SKIN_WIDTH); // do skin width computation.
}
ConvexDecomposition::HullError ret = hl.CreateConvexHull(hdesc,result);
ConvexDecomposition::HullError ret = hl.CreateConvexHull(hdesc, result);
if ( ret == ConvexDecomposition::QE_OK )
if (ret == ConvexDecomposition::QE_OK)
{
ConvexDecomposition::ConvexResult r(result.mNumOutputVertices, result.mOutputVertices, result.mNumFaces, result.mIndices);
r.mHullVolume = computeMeshVolume( result.mOutputVertices, result.mNumFaces, result.mIndices ); // the volume of the hull.
r.mHullVolume = computeMeshVolume(result.mOutputVertices, result.mNumFaces, result.mIndices); // the volume of the hull.
// compute the best fit OBB
computeBestFitOBB( result.mNumOutputVertices, result.mOutputVertices, sizeof(float)*3, r.mOBBSides, r.mOBBTransform );
computeBestFitOBB(result.mNumOutputVertices, result.mOutputVertices, sizeof(float) * 3, r.mOBBSides, r.mOBBTransform);
r.mOBBVolume = r.mOBBSides[0] * r.mOBBSides[1] *r.mOBBSides[2]; // compute the OBB volume.
r.mOBBVolume = r.mOBBSides[0] * r.mOBBSides[1] * r.mOBBSides[2]; // compute the OBB volume.
fm_getTranslation( r.mOBBTransform, r.mOBBCenter ); // get the translation component of the 4x4 matrix.
fm_getTranslation(r.mOBBTransform, r.mOBBCenter); // get the translation component of the 4x4 matrix.
fm_matrixToQuat( r.mOBBTransform, r.mOBBOrientation ); // extract the orientation as a quaternion.
r.mSphereRadius = computeBoundingSphere( result.mNumOutputVertices, result.mOutputVertices, r.mSphereCenter );
r.mSphereVolume = fm_sphereVolume( r.mSphereRadius );
fm_matrixToQuat(r.mOBBTransform, r.mOBBOrientation); // extract the orientation as a quaternion.
r.mSphereRadius = computeBoundingSphere(result.mNumOutputVertices, result.mOutputVertices, r.mSphereCenter);
r.mSphereVolume = fm_sphereVolume(r.mSphereRadius);
mCallback->ConvexDecompResult(r);
}
hl.ReleaseResult (result);
hl.ReleaseResult(result);
delete cr;
}
@ -338,24 +318,23 @@ unsigned int ConvexBuilder::process(const ConvexDecomposition::DecompDesc &desc)
return ret;
}
void ConvexBuilder::ConvexDebugTri(const float *p1,const float *p2,const float *p3,unsigned int color)
void ConvexBuilder::ConvexDebugTri(const float *p1, const float *p2, const float *p3, unsigned int color)
{
mCallback->ConvexDebugTri(p1,p2,p3,color);
mCallback->ConvexDebugTri(p1, p2, p3, color);
}
void ConvexBuilder::ConvexDebugOBB(const float *sides, const float *matrix,unsigned int color)
void ConvexBuilder::ConvexDebugOBB(const float *sides, const float *matrix, unsigned int color)
{
mCallback->ConvexDebugOBB(sides,matrix,color);
mCallback->ConvexDebugOBB(sides, matrix, color);
}
void ConvexBuilder::ConvexDebugPoint(const float *p,float dist,unsigned int color)
void ConvexBuilder::ConvexDebugPoint(const float *p, float dist, unsigned int color)
{
mCallback->ConvexDebugPoint(p,dist,color);
mCallback->ConvexDebugPoint(p, dist, color);
}
void ConvexBuilder::ConvexDebugBound(const float *bmin,const float *bmax,unsigned int color)
void ConvexBuilder::ConvexDebugBound(const float *bmin, const float *bmax, unsigned int color)
{
mCallback->ConvexDebugBound(bmin,bmax,color);
mCallback->ConvexDebugBound(bmin, bmax, color);
}
void ConvexBuilder::ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
@ -369,5 +348,3 @@ void ConvexBuilder::sortChulls(CHullVector &hulls)
hulls.quickSort(CHullSort());
//hulls.heapSort(CHullSort());
}

41
Extras/ConvexDecomposition/ConvexBuilder.h
View File

@ -35,12 +35,10 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
// http://www.amillionpixels.us
//
#include "ConvexDecomposition.h"
#include "vlookup.h"
#include "LinearMath/btAlignedObjectArray.h"
class CHull
{
public:
@ -50,28 +48,24 @@ public:
bool overlap(const CHull &h) const;
float mMin[3];
float mMax[3];
float mVolume;
float mDiagonal; // long edge..
ConvexDecomposition::ConvexResult *mResult;
float mMin[3];
float mMax[3];
float mVolume;
float mDiagonal; // long edge..
ConvexDecomposition::ConvexResult *mResult;
};
// Usage: std::sort( list.begin(), list.end(), StringSortRef() );
class CHullSort
{
public:
inline bool operator()(const CHull *a,const CHull *b) const
inline bool operator()(const CHull *a, const CHull *b) const
{
return a->mVolume < b->mVolume;
}
};
typedef btAlignedObjectArray< CHull * > CHullVector;
typedef btAlignedObjectArray<CHull *> CHullVector;
class ConvexBuilder : public ConvexDecomposition::ConvexDecompInterface
{
@ -80,31 +74,30 @@ public:
virtual ~ConvexBuilder(void);
bool isDuplicate(unsigned int i1,unsigned int i2,unsigned int i3,
unsigned int ci1,unsigned int ci2,unsigned int ci3);
bool isDuplicate(unsigned int i1, unsigned int i2, unsigned int i3,
unsigned int ci1, unsigned int ci2, unsigned int ci3);
void getMesh(const ConvexDecomposition::ConvexResult &cr,VertexLookup vc,UintVector &indices);
void getMesh(const ConvexDecomposition::ConvexResult &cr, VertexLookup vc, UintVector &indices);
CHull * canMerge(CHull *a,CHull *b);
CHull *canMerge(CHull *a, CHull *b);
bool combineHulls(void);
unsigned int process(const ConvexDecomposition::DecompDesc &desc);
virtual void ConvexDebugTri(const float *p1,const float *p2,const float *p3,unsigned int color);
virtual void ConvexDebugTri(const float *p1, const float *p2, const float *p3, unsigned int color);
virtual void ConvexDebugOBB(const float *sides, const float *matrix,unsigned int color);
virtual void ConvexDebugPoint(const float *p,float dist,unsigned int color);
virtual void ConvexDebugOBB(const float *sides, const float *matrix, unsigned int color);
virtual void ConvexDebugPoint(const float *p, float dist, unsigned int color);
virtual void ConvexDebugBound(const float *bmin,const float *bmax,unsigned int color);
virtual void ConvexDebugBound(const float *bmin, const float *bmax, unsigned int color);
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result);
void sortChulls(CHullVector &hulls);
CHullVector mChulls;
CHullVector mChulls;
ConvexDecompInterface *mCallback;
};
#endif //CONVEX_BUILDER_H
#endif //CONVEX_BUILDER_H

401
Extras/ConvexDecomposition/ConvexDecomposition.cpp
View File

@ -5,7 +5,6 @@
#include <string.h>
#include <assert.h>
/*----------------------------------------------------------------------
Copyright (c) 2004 Open Dynamics Framework Group
www.physicstools.org
@ -56,320 +55,292 @@
#define SHOW_MESH 0
#define MAKE_MESH 1
using namespace ConvexDecomposition;
namespace ConvexDecomposition
{
class FaceTri
{
public:
FaceTri(void) { };
FaceTri(const float *vertices,unsigned int i1,unsigned int i2,unsigned int i3)
{
mP1.Set( &vertices[i1*3] );
mP2.Set( &vertices[i2*3] );
mP3.Set( &vertices[i3*3] );
}
Vector3d mP1;
Vector3d mP2;
Vector3d mP3;
Vector3d mNormal;
FaceTri(void){};
FaceTri(const float *vertices, unsigned int i1, unsigned int i2, unsigned int i3)
{
mP1.Set(&vertices[i1 * 3]);
mP2.Set(&vertices[i2 * 3]);
mP3.Set(&vertices[i3 * 3]);
}
Vector3d mP1;
Vector3d mP2;
Vector3d mP3;
Vector3d mNormal;
};
void addTri(VertexLookup vl,UintVector &list,const Vector3d &p1,const Vector3d &p2,const Vector3d &p3)
void addTri(VertexLookup vl, UintVector &list, const Vector3d &p1, const Vector3d &p2, const Vector3d &p3)
{
unsigned int i1 = Vl_getIndex(vl, p1.Ptr() );
unsigned int i2 = Vl_getIndex(vl, p2.Ptr() );
unsigned int i3 = Vl_getIndex(vl, p3.Ptr() );
unsigned int i1 = Vl_getIndex(vl, p1.Ptr());
unsigned int i2 = Vl_getIndex(vl, p2.Ptr());
unsigned int i3 = Vl_getIndex(vl, p3.Ptr());
// do *not* process degenerate triangles!
// do *not* process degenerate triangles!
if ( i1 != i2 && i1 != i3 && i2 != i3 )
{
list.push_back(i1);
list.push_back(i2);
list.push_back(i3);
}
if (i1 != i2 && i1 != i3 && i2 != i3)
{
list.push_back(i1);
list.push_back(i2);
list.push_back(i3);
}
}
void calcConvexDecomposition(unsigned int vcount,
const float *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
float masterVolume,
unsigned int depth)
void calcConvexDecomposition(unsigned int vcount,
const float *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
float masterVolume,
unsigned int depth)
{
float plane[4];
float plane[4];
bool split = false;
if ( depth < MAXDEPTH )
{
bool split = false;
if (depth < MAXDEPTH)
{
float volume;
float c = computeConcavity( vcount, vertices, tcount, indices, callback, plane, volume );
float c = computeConcavity(vcount, vertices, tcount, indices, callback, plane, volume);
if ( depth == 0 )
{
masterVolume = volume;
}
float percent = (c*100.0f)/masterVolume;
if ( percent > CONCAVE_PERCENT ) // if great than 5% of the total volume is concave, go ahead and keep splitting.
if (depth == 0)
{
split = true;
}
masterVolume = volume;
}
}
float percent = (c * 100.0f) / masterVolume;
if ( depth >= MAXDEPTH || !split )
{
if (percent > CONCAVE_PERCENT) // if great than 5% of the total volume is concave, go ahead and keep splitting.
{
split = true;
}
}
if (depth >= MAXDEPTH || !split)
{
#if 1
HullResult result;
HullLibrary hl;
HullDesc desc;
HullResult result;
HullLibrary hl;
HullDesc desc;
desc.SetHullFlag(QF_TRIANGLES);
desc.SetHullFlag(QF_TRIANGLES);
desc.mVcount = vcount;
desc.mVertices = vertices;
desc.mVertexStride = sizeof(float)*3;
desc.mVcount = vcount;
desc.mVertices = vertices;
desc.mVertexStride = sizeof(float) * 3;
HullError ret = hl.CreateConvexHull(desc,result);
if ( ret == QE_OK )
{
HullError ret = hl.CreateConvexHull(desc, result);
if (ret == QE_OK)
{
ConvexResult r(result.mNumOutputVertices, result.mOutputVertices, result.mNumFaces, result.mIndices);
callback->ConvexDecompResult(r);
}
}
#else
static unsigned int colors[8] =
{
0xFF0000,
0x00FF00,
0x0000FF,
0xFFFF00,
0x00FFFF,
0xFF00FF,
0xFFFFFF,
0xFF8040
};
{
0xFF0000,
0x00FF00,
0x0000FF,
0xFFFF00,
0x00FFFF,
0xFF00FF,
0xFFFFFF,
0xFF8040};
static int count = 0;
count++;
if ( count == 8 ) count = 0;
if (count == 8) count = 0;
assert( count >= 0 && count < 8 );
assert(count >= 0 && count < 8);
unsigned int color = colors[count];
const unsigned int *source = indices;
for (unsigned int i=0; i<tcount; i++)
{
unsigned int i1 = *source++;
unsigned int i2 = *source++;
unsigned int i3 = *source++;
FaceTri t(vertices, i1, i2, i3 );
callback->ConvexDebugTri( t.mP1.Ptr(), t.mP2.Ptr(), t.mP3.Ptr(), color );
}
#endif
hl.ReleaseResult (result);
return;
}
UintVector ifront;
UintVector iback;
VertexLookup vfront = Vl_createVertexLookup();
VertexLookup vback = Vl_createVertexLookup();
bool showmesh = false;
#if SHOW_MESH
showmesh = true;
#endif
if ( 0 )
{
showmesh = true;
for (float x=-1; x<1; x+=0.10f)
{
for (float y=0; y<1; y+=0.10f)
{
for (float z=-1; z<1; z+=0.04f)
{
float d = x*plane[0] + y*plane[1] + z*plane[2] + plane[3];
Vector3d p(x,y,z);
if ( d >= 0 )
callback->ConvexDebugPoint(p.Ptr(), 0.02f, 0x00FF00);
else
callback->ConvexDebugPoint(p.Ptr(), 0.02f, 0xFF0000);
}
}
}
}
if ( 1 )
{
// ok..now we are going to 'split' all of the input triangles against this plane!
const unsigned int *source = indices;
for (unsigned int i=0; i<tcount; i++)
for (unsigned int i = 0; i < tcount; i++)
{
unsigned int i1 = *source++;
unsigned int i2 = *source++;
unsigned int i3 = *source++;
FaceTri t(vertices, i1, i2, i3 );
FaceTri t(vertices, i1, i2, i3);
callback->ConvexDebugTri(t.mP1.Ptr(), t.mP2.Ptr(), t.mP3.Ptr(), color);
}
#endif
hl.ReleaseResult(result);
return;
}
UintVector ifront;
UintVector iback;
VertexLookup vfront = Vl_createVertexLookup();
VertexLookup vback = Vl_createVertexLookup();
bool showmesh = false;
#if SHOW_MESH
showmesh = true;
#endif
if (0)
{
showmesh = true;
for (float x = -1; x < 1; x += 0.10f)
{
for (float y = 0; y < 1; y += 0.10f)
{
for (float z = -1; z < 1; z += 0.04f)
{
float d = x * plane[0] + y * plane[1] + z * plane[2] + plane[3];
Vector3d p(x, y, z);
if (d >= 0)
callback->ConvexDebugPoint(p.Ptr(), 0.02f, 0x00FF00);
else
callback->ConvexDebugPoint(p.Ptr(), 0.02f, 0xFF0000);
}
}
}
}
if (1)
{
// ok..now we are going to 'split' all of the input triangles against this plane!
const unsigned int *source = indices;
for (unsigned int i = 0; i < tcount; i++)
{
unsigned int i1 = *source++;
unsigned int i2 = *source++;
unsigned int i3 = *source++;
FaceTri t(vertices, i1, i2, i3);
Vector3d front[4];
Vector3d back[4];
unsigned int fcount=0;
unsigned int bcount=0;
unsigned int fcount = 0;
unsigned int bcount = 0;
PlaneTriResult result;
result = planeTriIntersection(plane,t.mP1.Ptr(),sizeof(Vector3d),0.00001f,front[0].Ptr(),fcount,back[0].Ptr(),bcount );
result = planeTriIntersection(plane, t.mP1.Ptr(), sizeof(Vector3d), 0.00001f, front[0].Ptr(), fcount, back[0].Ptr(), bcount);
if( fcount > 4 || bcount > 4 )
if (fcount > 4 || bcount > 4)
{
result = planeTriIntersection(plane,t.mP1.Ptr(),sizeof(Vector3d),0.00001f,front[0].Ptr(),fcount,back[0].Ptr(),bcount );
result = planeTriIntersection(plane, t.mP1.Ptr(), sizeof(Vector3d), 0.00001f, front[0].Ptr(), fcount, back[0].Ptr(), bcount);
}
switch ( result )
switch (result)
{
case PTR_FRONT:
assert( fcount == 3 );
assert(fcount == 3);
if ( showmesh )
callback->ConvexDebugTri( front[0].Ptr(), front[1].Ptr(), front[2].Ptr(), 0x00FF00 );
if (showmesh)
callback->ConvexDebugTri(front[0].Ptr(), front[1].Ptr(), front[2].Ptr(), 0x00FF00);
#if MAKE_MESH
#if MAKE_MESH
addTri( vfront, ifront, front[0], front[1], front[2] );
addTri(vfront, ifront, front[0], front[1], front[2]);
#endif
#endif
break;
case PTR_BACK:
assert( bcount == 3 );
assert(bcount == 3);
if ( showmesh )
callback->ConvexDebugTri( back[0].Ptr(), back[1].Ptr(), back[2].Ptr(), 0xFFFF00 );
if (showmesh)
callback->ConvexDebugTri(back[0].Ptr(), back[1].Ptr(), back[2].Ptr(), 0xFFFF00);
#if MAKE_MESH
#if MAKE_MESH
addTri( vback, iback, back[0], back[1], back[2] );
addTri(vback, iback, back[0], back[1], back[2]);
#endif
#endif
break;
case PTR_SPLIT:
assert( fcount >= 3 && fcount <= 4);
assert( bcount >= 3 && bcount <= 4);
assert(fcount >= 3 && fcount <= 4);
assert(bcount >= 3 && bcount <= 4);
#if MAKE_MESH
#if MAKE_MESH
addTri( vfront, ifront, front[0], front[1], front[2] );
addTri( vback, iback, back[0], back[1], back[2] );
addTri(vfront, ifront, front[0], front[1], front[2]);
addTri(vback, iback, back[0], back[1], back[2]);
if (fcount == 4)
{
addTri(vfront, ifront, front[0], front[2], front[3]);
}
if ( fcount == 4 )
{
addTri( vfront, ifront, front[0], front[2], front[3] );
}
if (bcount == 4)
{
addTri(vback, iback, back[0], back[2], back[3]);
}
if ( bcount == 4 )
{
addTri( vback, iback, back[0], back[2], back[3] );
}
#endif
#endif
if (showmesh)
{
callback->ConvexDebugTri(front[0].Ptr(), front[1].Ptr(), front[2].Ptr(), 0x00D000);
callback->ConvexDebugTri(back[0].Ptr(), back[1].Ptr(), back[2].Ptr(), 0xD0D000);
if ( showmesh )
{
callback->ConvexDebugTri( front[0].Ptr(), front[1].Ptr(), front[2].Ptr(), 0x00D000 );
callback->ConvexDebugTri( back[0].Ptr(), back[1].Ptr(), back[2].Ptr(), 0xD0D000 );
if ( fcount == 4 )
{
callback->ConvexDebugTri( front[0].Ptr(), front[2].Ptr(), front[3].Ptr(), 0x00D000 );
}
if ( bcount == 4 )
{
callback->ConvexDebugTri( back[0].Ptr(), back[2].Ptr(), back[3].Ptr(), 0xD0D000 );
}
}
if (fcount == 4)
{
callback->ConvexDebugTri(front[0].Ptr(), front[2].Ptr(), front[3].Ptr(), 0x00D000);
}
if (bcount == 4)
{
callback->ConvexDebugTri(back[0].Ptr(), back[2].Ptr(), back[3].Ptr(), 0xD0D000);
}
}
break;
}
}
// ok... here we recursively call
if ( ifront.size() )
{
unsigned int vcount = Vl_getVcount(vfront);
const float *vertices = Vl_getVertices(vfront);
unsigned int tcount = ifront.size()/3;
// ok... here we recursively call
if (ifront.size())
{
unsigned int vcount = Vl_getVcount(vfront);
const float *vertices = Vl_getVertices(vfront);
unsigned int tcount = ifront.size() / 3;
calcConvexDecomposition(vcount, vertices, tcount, &ifront[0], callback, masterVolume, depth+1);
calcConvexDecomposition(vcount, vertices, tcount, &ifront[0], callback, masterVolume, depth + 1);
}
}
ifront.clear();
ifront.clear();
Vl_releaseVertexLookup(vfront);
Vl_releaseVertexLookup(vfront);
if (iback.size())
{
unsigned int vcount = Vl_getVcount(vback);
const float *vertices = Vl_getVertices(vback);
unsigned int tcount = iback.size() / 3;
if ( iback.size() )
{
unsigned int vcount = Vl_getVcount(vback);
const float *vertices = Vl_getVertices(vback);
unsigned int tcount = iback.size()/3;
calcConvexDecomposition(vcount, vertices, tcount, &iback[0], callback, masterVolume, depth+1);
}
iback.clear();
Vl_releaseVertexLookup(vback);
calcConvexDecomposition(vcount, vertices, tcount, &iback[0], callback, masterVolume, depth + 1);
}
iback.clear();
Vl_releaseVertexLookup(vback);
}
}
}
} // namespace ConvexDecomposition

282
Extras/ConvexDecomposition/ConvexDecomposition.h
View File

@ -36,185 +36,167 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
// http://www.amillionpixels.us
//
#ifdef _WIN32
#include <memory.h> //memcpy
#include <memory.h> //memcpy
#endif
#include <string.h>
#include <stdio.h>
#include "LinearMath/btAlignedObjectArray.h"
extern unsigned int MAXDEPTH;
extern float CONCAVE_PERCENT;
extern float MERGE_PERCENT;
extern unsigned int MAXDEPTH ;
extern float CONCAVE_PERCENT ;
extern float MERGE_PERCENT ;
typedef btAlignedObjectArray< unsigned int > UintVector;
typedef btAlignedObjectArray<unsigned int> UintVector;
namespace ConvexDecomposition
{
class ConvexResult
class ConvexResult
{
public:
ConvexResult(void)
{
public:
ConvexResult(void)
mHullVcount = 0;
mHullVertices = 0;
mHullTcount = 0;
mHullIndices = 0;
}
ConvexResult(unsigned int hvcount, const float *hvertices, unsigned int htcount, const unsigned int *hindices)
{
mHullVcount = hvcount;
if (mHullVcount)
{
mHullVertices = new float[mHullVcount * sizeof(float) * 3];
memcpy(mHullVertices, hvertices, sizeof(float) * 3 * mHullVcount);
}
else
{
mHullVcount = 0;
mHullVertices = 0;
mHullTcount = 0;
}
mHullTcount = htcount;
if (mHullTcount)
{
mHullIndices = new unsigned int[sizeof(unsigned int) * mHullTcount * 3];
memcpy(mHullIndices, hindices, sizeof(unsigned int) * mHullTcount * 3);
}
else
{
mHullIndices = 0;
}
}
ConvexResult(unsigned int hvcount,const float *hvertices,unsigned int htcount,const unsigned int *hindices)
{
mHullVcount = hvcount;
if ( mHullVcount )
{
mHullVertices = new float[mHullVcount*sizeof(float)*3];
memcpy(mHullVertices, hvertices, sizeof(float)*3*mHullVcount );
}
else
{
mHullVertices = 0;
}
mHullTcount = htcount;
if ( mHullTcount )
{
mHullIndices = new unsigned int[sizeof(unsigned int)*mHullTcount*3];
memcpy(mHullIndices,hindices, sizeof(unsigned int)*mHullTcount*3 );
}
else
{
mHullIndices = 0;
}
}
ConvexResult(const ConvexResult &r)
{
mHullVcount = r.mHullVcount;
if ( mHullVcount )
{
mHullVertices = new float[mHullVcount*sizeof(float)*3];
memcpy(mHullVertices, r.mHullVertices, sizeof(float)*3*mHullVcount );
}
else
{
mHullVertices = 0;
}
mHullTcount = r.mHullTcount;
if ( mHullTcount )
{
mHullIndices = new unsigned int[sizeof(unsigned int)*mHullTcount*3];
memcpy(mHullIndices, r.mHullIndices, sizeof(unsigned int)*mHullTcount*3 );
}
else
{
mHullIndices = 0;
}
}
~ConvexResult(void)
{
delete [] mHullVertices;
delete [] mHullIndices;
}
// the convex hull.
unsigned int mHullVcount;
float * mHullVertices;
unsigned int mHullTcount;
unsigned int *mHullIndices;
float mHullVolume; // the volume of the convex hull.
float mOBBSides[3]; // the width, height and breadth of the best fit OBB
float mOBBCenter[3]; // the center of the OBB
float mOBBOrientation[4]; // the quaternion rotation of the OBB.
float mOBBTransform[16]; // the 4x4 transform of the OBB.
float mOBBVolume; // the volume of the OBB
float mSphereRadius; // radius and center of best fit sphere
float mSphereCenter[3];
float mSphereVolume; // volume of the best fit sphere
};
class ConvexDecompInterface
ConvexResult(const ConvexResult &r)
{
public:
virtual ~ConvexDecompInterface() {};
virtual void ConvexDebugTri(const float *p1,const float *p2,const float *p3,unsigned int color) { };
virtual void ConvexDebugPoint(const float *p,float dist,unsigned int color) { };
virtual void ConvexDebugBound(const float *bmin,const float *bmax,unsigned int color) { };
virtual void ConvexDebugOBB(const float *sides, const float *matrix,unsigned int color) { };
virtual void ConvexDecompResult(ConvexResult &result) = 0;
};
// just to avoid passing a zillion parameters to the method the
// options are packed into this descriptor.
class DecompDesc
{
public:
DecompDesc(void)
mHullVcount = r.mHullVcount;
if (mHullVcount)
{
mVcount = 0;
mVertices = 0;
mTcount = 0;
mIndices = 0;
mDepth = 5;
mCpercent = 5;
mPpercent = 5;
mMaxVertices = 32;
mSkinWidth = 0;
mCallback = 0;
mHullVertices = new float[mHullVcount * sizeof(float) * 3];
memcpy(mHullVertices, r.mHullVertices, sizeof(float) * 3 * mHullVcount);
}
else
{
mHullVertices = 0;
}
mHullTcount = r.mHullTcount;
if (mHullTcount)
{
mHullIndices = new unsigned int[sizeof(unsigned int) * mHullTcount * 3];
memcpy(mHullIndices, r.mHullIndices, sizeof(unsigned int) * mHullTcount * 3);
}
else
{
mHullIndices = 0;
}
}
// describes the input triangle.
unsigned int mVcount; // the number of vertices in the source mesh.
const float *mVertices; // start of the vertex position array. Assumes a stride of 3 floats.
unsigned int mTcount; // the number of triangles in the source mesh.
unsigned int *mIndices; // the indexed triangle list array (zero index based)
~ConvexResult(void)
{
delete[] mHullVertices;
delete[] mHullIndices;
}
// options
unsigned int mDepth; // depth to split, a maximum of 10, generally not over 7.
float mCpercent; // the concavity threshold percentage. 0=20 is reasonable.
float mPpercent; // the percentage volume conservation threshold to collapse hulls. 0-30 is reasonable.
// the convex hull.
unsigned int mHullVcount;
float *mHullVertices;
unsigned int mHullTcount;
unsigned int *mHullIndices;
// hull output limits.
unsigned int mMaxVertices; // maximum number of vertices in the output hull. Recommended 32 or less.
float mSkinWidth; // a skin width to apply to the output hulls.
float mHullVolume; // the volume of the convex hull.
ConvexDecompInterface *mCallback; // the interface to receive back the results.
float mOBBSides[3]; // the width, height and breadth of the best fit OBB
float mOBBCenter[3]; // the center of the OBB
float mOBBOrientation[4]; // the quaternion rotation of the OBB.
float mOBBTransform[16]; // the 4x4 transform of the OBB.
float mOBBVolume; // the volume of the OBB
};
float mSphereRadius; // radius and center of best fit sphere
float mSphereCenter[3];
float mSphereVolume; // volume of the best fit sphere
};
// perform approximate convex decomposition on a mesh.
unsigned int performConvexDecomposition(const DecompDesc &desc); // returns the number of hulls produced.
class ConvexDecompInterface
{
public:
virtual ~ConvexDecompInterface(){};
virtual void ConvexDebugTri(const float *p1, const float *p2, const float *p3, unsigned int color){};
virtual void ConvexDebugPoint(const float *p, float dist, unsigned int color){};
virtual void ConvexDebugBound(const float *bmin, const float *bmax, unsigned int color){};
virtual void ConvexDebugOBB(const float *sides, const float *matrix, unsigned int color){};
virtual void ConvexDecompResult(ConvexResult &result) = 0;
};
void calcConvexDecomposition(unsigned int vcount,
const float *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
float masterVolume,
unsigned int depth);
// just to avoid passing a zillion parameters to the method the
// options are packed into this descriptor.
class DecompDesc
{
public:
DecompDesc(void)
{
mVcount = 0;
mVertices = 0;
mTcount = 0;
mIndices = 0;
mDepth = 5;
mCpercent = 5;
mPpercent = 5;
mMaxVertices = 32;
mSkinWidth = 0;
mCallback = 0;
}
// describes the input triangle.
unsigned int mVcount; // the number of vertices in the source mesh.
const float *mVertices; // start of the vertex position array. Assumes a stride of 3 floats.
unsigned int mTcount; // the number of triangles in the source mesh.
unsigned int *mIndices; // the indexed triangle list array (zero index based)
}
// options
unsigned int mDepth; // depth to split, a maximum of 10, generally not over 7.
float mCpercent; // the concavity threshold percentage. 0=20 is reasonable.
float mPpercent; // the percentage volume conservation threshold to collapse hulls. 0-30 is reasonable.
// hull output limits.
unsigned int mMaxVertices; // maximum number of vertices in the output hull. Recommended 32 or less.
float mSkinWidth; // a skin width to apply to the output hulls.
ConvexDecompInterface *mCallback; // the interface to receive back the results.
};
// perform approximate convex decomposition on a mesh.
unsigned int performConvexDecomposition(const DecompDesc &desc); // returns the number of hulls produced.
void calcConvexDecomposition(unsigned int vcount,
const float *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
float masterVolume,
unsigned int depth);
} // namespace ConvexDecomposition
#endif

644
Extras/ConvexDecomposition/bestfit.cpp
View File

@ -52,369 +52,358 @@
namespace BestFit
{
class Vec3
{
public:
Vec3(void) { };
Vec3(float _x,float _y,float _z) { x = _x; y = _y; z = _z; };
Vec3(void){};
Vec3(float _x, float _y, float _z)
{
x = _x;
y = _y;
z = _z;
};
float dot(const Vec3 &v)
{
return x * v.x + y * v.y + z * v.z; // the dot product
}
float dot(const Vec3 &v)
{
return x*v.x + y*v.y + z*v.z; // the dot product
}
float x;
float y;
float z;
float x;
float y;
float z;
};
class Eigen
{
public:
void DecrSortEigenStuff(void)
{
Tridiagonal(); //diagonalize the matrix.
QLAlgorithm(); //
DecreasingSort();
GuaranteeRotation();
}
void Tridiagonal(void)
{
float fM00 = mElement[0][0];
float fM01 = mElement[0][1];
float fM02 = mElement[0][2];
float fM11 = mElement[1][1];
float fM12 = mElement[1][2];
float fM22 = mElement[2][2];
void DecrSortEigenStuff(void)
{
Tridiagonal(); //diagonalize the matrix.
QLAlgorithm(); //
DecreasingSort();
GuaranteeRotation();
}
m_afDiag[0] = fM00;
m_afSubd[2] = 0;
if (fM02 != (float)0.0)
{
float fLength = sqrtf(fM01 * fM01 + fM02 * fM02);
float fInvLength = ((float)1.0) / fLength;
fM01 *= fInvLength;
fM02 *= fInvLength;
float fQ = ((float)2.0) * fM01 * fM12 + fM02 * (fM22 - fM11);
m_afDiag[1] = fM11 + fM02 * fQ;
m_afDiag[2] = fM22 - fM02 * fQ;
m_afSubd[0] = fLength;
m_afSubd[1] = fM12 - fM01 * fQ;
mElement[0][0] = (float)1.0;
mElement[0][1] = (float)0.0;
mElement[0][2] = (float)0.0;
mElement[1][0] = (float)0.0;
mElement[1][1] = fM01;
mElement[1][2] = fM02;
mElement[2][0] = (float)0.0;
mElement[2][1] = fM02;
mElement[2][2] = -fM01;
m_bIsRotation = false;
}
else
{
m_afDiag[1] = fM11;
m_afDiag[2] = fM22;
m_afSubd[0] = fM01;
m_afSubd[1] = fM12;
mElement[0][0] = (float)1.0;
mElement[0][1] = (float)0.0;
mElement[0][2] = (float)0.0;
mElement[1][0] = (float)0.0;
mElement[1][1] = (float)1.0;
mElement[1][2] = (float)0.0;
mElement[2][0] = (float)0.0;
mElement[2][1] = (float)0.0;
mElement[2][2] = (float)1.0;
m_bIsRotation = true;
}
}
void Tridiagonal(void)
{
float fM00 = mElement[0][0];
float fM01 = mElement[0][1];
float fM02 = mElement[0][2];
float fM11 = mElement[1][1];
float fM12 = mElement[1][2];
float fM22 = mElement[2][2];
bool QLAlgorithm(void)
{
const int iMaxIter = 32;
m_afDiag[0] = fM00;
m_afSubd[2] = 0;
if (fM02 != (float)0.0)
{
float fLength = sqrtf(fM01*fM01+fM02*fM02);
float fInvLength = ((float)1.0)/fLength;
fM01 *= fInvLength;
fM02 *= fInvLength;
float fQ = ((float)2.0)*fM01*fM12+fM02*(fM22-fM11);
m_afDiag[1] = fM11+fM02*fQ;
m_afDiag[2] = fM22-fM02*fQ;
m_afSubd[0] = fLength;
m_afSubd[1] = fM12-fM01*fQ;
mElement[0][0] = (float)1.0;
mElement[0][1] = (float)0.0;
mElement[0][2] = (float)0.0;
mElement[1][0] = (float)0.0;
mElement[1][1] = fM01;
mElement[1][2] = fM02;
mElement[2][0] = (float)0.0;
mElement[2][1] = fM02;
mElement[2][2] = -fM01;
m_bIsRotation = false;
}
else
{
m_afDiag[1] = fM11;
m_afDiag[2] = fM22;
m_afSubd[0] = fM01;
m_afSubd[1] = fM12;
mElement[0][0] = (float)1.0;
mElement[0][1] = (float)0.0;
mElement[0][2] = (float)0.0;
mElement[1][0] = (float)0.0;
mElement[1][1] = (float)1.0;
mElement[1][2] = (float)0.0;
mElement[2][0] = (float)0.0;
mElement[2][1] = (float)0.0;
mElement[2][2] = (float)1.0;
m_bIsRotation = true;
}
}
for (int i0 = 0; i0 < 3; i0++)
{
int i1;
for (i1 = 0; i1 < iMaxIter; i1++)
{
int i2;
for (i2 = i0; i2 <= (3 - 2); i2++)
{
float fTmp = fabsf(m_afDiag[i2]) + fabsf(m_afDiag[i2 + 1]);
if (fabsf(m_afSubd[i2]) + fTmp == fTmp)
break;
}
if (i2 == i0)
{
break;
}
bool QLAlgorithm(void)
{
const int iMaxIter = 32;
float fG = (m_afDiag[i0 + 1] - m_afDiag[i0]) / (((float)2.0) * m_afSubd[i0]);
float fR = sqrtf(fG * fG + (float)1.0);
if (fG < (float)0.0)
{
fG = m_afDiag[i2] - m_afDiag[i0] + m_afSubd[i0] / (fG - fR);
}
else
{
fG = m_afDiag[i2] - m_afDiag[i0] + m_afSubd[i0] / (fG + fR);
}
float fSin = (float)1.0, fCos = (float)1.0, fP = (float)0.0;
for (int i3 = i2 - 1; i3 >= i0; i3--)
{
float fF = fSin * m_afSubd[i3];
float fB = fCos * m_afSubd[i3];
if (fabsf(fF) >= fabsf(fG))
{
fCos = fG / fF;
fR = sqrtf(fCos * fCos + (float)1.0);
m_afSubd[i3 + 1] = fF * fR;
fSin = ((float)1.0) / fR;
fCos *= fSin;
}
else
{
fSin = fF / fG;
fR = sqrtf(fSin * fSin + (float)1.0);
m_afSubd[i3 + 1] = fG * fR;
fCos = ((float)1.0) / fR;
fSin *= fCos;
}
fG = m_afDiag[i3 + 1] - fP;
fR = (m_afDiag[i3] - fG) * fSin + ((float)2.0) * fB * fCos;
fP = fSin * fR;
m_afDiag[i3 + 1] = fG + fP;
fG = fCos * fR - fB;
for (int i4 = 0; i4 < 3; i4++)
{
fF = mElement[i4][i3 + 1];
mElement[i4][i3 + 1] = fSin * mElement[i4][i3] + fCos * fF;
mElement[i4][i3] = fCos * mElement[i4][i3] - fSin * fF;
}
}
m_afDiag[i0] -= fP;
m_afSubd[i0] = fG;
m_afSubd[i2] = (float)0.0;
}
if (i1 == iMaxIter)
{
return false;
}
}
return true;
}
for (int i0 = 0; i0 <3; i0++)
{
int i1;
for (i1 = 0; i1 < iMaxIter; i1++)
{
int i2;
for (i2 = i0; i2 <= (3-2); i2++)
{
float fTmp = fabsf(m_afDiag[i2]) + fabsf(m_afDiag[i2+1]);
if ( fabsf(m_afSubd[i2]) + fTmp == fTmp )
break;
}
if (i2 == i0)
{
break;
}
void DecreasingSort(void)
{
//sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
for (int i0 = 0, i1; i0 <= 3 - 2; i0++)
{
// locate maximum eigenvalue
i1 = i0;
float fMax = m_afDiag[i1];
int i2;
for (i2 = i0 + 1; i2 < 3; i2++)
{
if (m_afDiag[i2] > fMax)
{
i1 = i2;
fMax = m_afDiag[i1];
}
}
float fG = (m_afDiag[i0+1] - m_afDiag[i0])/(((float)2.0) * m_afSubd[i0]);
float fR = sqrtf(fG*fG+(float)1.0);
if (fG < (float)0.0)
{
fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR);
}
else
{
fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR);
}
float fSin = (float)1.0, fCos = (float)1.0, fP = (float)0.0;
for (int i3 = i2-1; i3 >= i0; i3--)
{
float fF = fSin*m_afSubd[i3];
float fB = fCos*m_afSubd[i3];
if (fabsf(fF) >= fabsf(fG))
{
fCos = fG/fF;
fR = sqrtf(fCos*fCos+(float)1.0);
m_afSubd[i3+1] = fF*fR;
fSin = ((float)1.0)/fR;
fCos *= fSin;
}
else
{
fSin = fF/fG;
fR = sqrtf(fSin*fSin+(float)1.0);
m_afSubd[i3+1] = fG*fR;
fCos = ((float)1.0)/fR;
fSin *= fCos;
}
fG = m_afDiag[i3+1]-fP;
fR = (m_afDiag[i3]-fG)*fSin+((float)2.0)*fB*fCos;
fP = fSin*fR;
m_afDiag[i3+1] = fG+fP;
fG = fCos*fR-fB;
for (int i4 = 0; i4 < 3; i4++)
{
fF = mElement[i4][i3+1];
mElement[i4][i3+1] = fSin*mElement[i4][i3]+fCos*fF;
mElement[i4][i3] = fCos*mElement[i4][i3]-fSin*fF;
}
}
m_afDiag[i0] -= fP;
m_afSubd[i0] = fG;
m_afSubd[i2] = (float)0.0;
}
if (i1 == iMaxIter)
{
return false;
}
}
return true;
}
if (i1 != i0)
{
// swap eigenvalues
m_afDiag[i1] = m_afDiag[i0];
m_afDiag[i0] = fMax;
// swap eigenvectors
for (i2 = 0; i2 < 3; i2++)
{
float fTmp = mElement[i2][i0];
mElement[i2][i0] = mElement[i2][i1];
mElement[i2][i1] = fTmp;
m_bIsRotation = !m_bIsRotation;
}
}
}
}
void DecreasingSort(void)
{
//sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
for (int i0 = 0, i1; i0 <= 3-2; i0++)
{
// locate maximum eigenvalue
i1 = i0;
float fMax = m_afDiag[i1];
int i2;
for (i2 = i0+1; i2 < 3; i2++)
{
if (m_afDiag[i2] > fMax)
{
i1 = i2;
fMax = m_afDiag[i1];
}
}
void GuaranteeRotation(void)
{
if (!m_bIsRotation)
{
// change sign on the first column
for (int iRow = 0; iRow < 3; iRow++)
{
mElement[iRow][0] = -mElement[iRow][0];
}
}
}
if (i1 != i0)
{
// swap eigenvalues
m_afDiag[i1] = m_afDiag[i0];
m_afDiag[i0] = fMax;
// swap eigenvectors
for (i2 = 0; i2 < 3; i2++)
{
float fTmp = mElement[i2][i0];
mElement[i2][i0] = mElement[i2][i1];
mElement[i2][i1] = fTmp;
m_bIsRotation = !m_bIsRotation;
}
}
}
}
void GuaranteeRotation(void)
{
if (!m_bIsRotation)
{
// change sign on the first column
for (int iRow = 0; iRow <3; iRow++)
{
mElement[iRow][0] = -mElement[iRow][0];
}
}
}
float mElement[3][3];
float m_afDiag[3];
float m_afSubd[3];
bool m_bIsRotation;
float mElement[3][3];
float m_afDiag[3];
float m_afSubd[3];
bool m_bIsRotation;
};
}
} // namespace BestFit
using namespace BestFit;
bool getBestFitPlane(unsigned int vcount,
const float *points,
unsigned int vstride,
const float *weights,
unsigned int wstride,
float *plane)
const float *points,
unsigned int vstride,
const float *weights,
unsigned int wstride,
float *plane)
{
bool ret = false;
bool ret = false;
Vec3 kOrigin(0,0,0);
Vec3 kOrigin(0, 0, 0);
float wtotal = 0;
float wtotal = 0;
if ( 1 )
{
const char *source = (const char *) points;
const char *wsource = (const char *) weights;
if (1)
{
const char *source = (const char *)points;
const char *wsource = (const char *)weights;
for (unsigned int i=0; i<vcount; i++)
{
for (unsigned int i = 0; i < vcount; i++)
{
const float *p = (const float *)source;
const float *p = (const float *) source;
float w = 1;
float w = 1;
if (wsource)
{
const float *ws = (const float *)wsource;
w = *ws; //
wsource += wstride;
}
if ( wsource )
{
const float *ws = (const float *) wsource;
w = *ws; //
wsource+=wstride;
}
kOrigin.x += p[0] * w;
kOrigin.y += p[1] * w;
kOrigin.z += p[2] * w;
kOrigin.x+=p[0]*w;
kOrigin.y+=p[1]*w;
kOrigin.z+=p[2]*w;
wtotal += w;
wtotal+=w;
source += vstride;
}
}
source+=vstride;
}
}
float recip = 1.0f / wtotal; // reciprocol of total weighting
float recip = 1.0f / wtotal; // reciprocol of total weighting
kOrigin.x *= recip;
kOrigin.y *= recip;
kOrigin.z *= recip;
kOrigin.x*=recip;
kOrigin.y*=recip;
kOrigin.z*=recip;
float fSumXX = 0;
float fSumXY = 0;
float fSumXZ = 0;
float fSumYY = 0;
float fSumYZ = 0;
float fSumZZ = 0;
float fSumXX=0;
float fSumXY=0;
float fSumXZ=0;
if (1)
{
const char *source = (const char *)points;
const char *wsource = (const char *)weights;
float fSumYY=0;
float fSumYZ=0;
float fSumZZ=0;
for (unsigned int i = 0; i < vcount; i++)
{
const float *p = (const float *)source;
float w = 1;
if ( 1 )
{
const char *source = (const char *) points;
const char *wsource = (const char *) weights;
if (wsource)
{
const float *ws = (const float *)wsource;
w = *ws; //
wsource += wstride;
}
for (unsigned int i=0; i<vcount; i++)
{
Vec3 kDiff;
const float *p = (const float *) source;
kDiff.x = w * (p[0] - kOrigin.x); // apply vertex weighting!
kDiff.y = w * (p[1] - kOrigin.y);
kDiff.z = w * (p[2] - kOrigin.z);
float w = 1;
fSumXX += kDiff.x * kDiff.x; // sume of the squares of the differences.
fSumXY += kDiff.x * kDiff.y; // sume of the squares of the differences.
fSumXZ += kDiff.x * kDiff.z; // sume of the squares of the differences.
if ( wsource )
{
const float *ws = (const float *) wsource;
w = *ws; //
wsource+=wstride;
}
fSumYY += kDiff.y * kDiff.y;
fSumYZ += kDiff.y * kDiff.z;
fSumZZ += kDiff.z * kDiff.z;
Vec3 kDiff;
source += vstride;
}
}
kDiff.x = w*(p[0] - kOrigin.x); // apply vertex weighting!
kDiff.y = w*(p[1] - kOrigin.y);
kDiff.z = w*(p[2] - kOrigin.z);
fSumXX *= recip;
fSumXY *= recip;
fSumXZ *= recip;
fSumYY *= recip;
fSumYZ *= recip;
fSumZZ *= recip;
fSumXX+= kDiff.x * kDiff.x; // sume of the squares of the differences.
fSumXY+= kDiff.x * kDiff.y; // sume of the squares of the differences.
fSumXZ+= kDiff.x * kDiff.z; // sume of the squares of the differences.
// setup the eigensolver
Eigen kES;
fSumYY+= kDiff.y * kDiff.y;
fSumYZ+= kDiff.y * kDiff.z;
fSumZZ+= kDiff.z * kDiff.z;
kES.mElement[0][0] = fSumXX;
kES.mElement[0][1] = fSumXY;
kES.mElement[0][2] = fSumXZ;
kES.mElement[1][0] = fSumXY;
kES.mElement[1][1] = fSumYY;
kES.mElement[1][2] = fSumYZ;
source+=vstride;
}
}
kES.mElement[2][0] = fSumXZ;
kES.mElement[2][1] = fSumYZ;
kES.mElement[2][2] = fSumZZ;
fSumXX *= recip;
fSumXY *= recip;
fSumXZ *= recip;
fSumYY *= recip;
fSumYZ *= recip;
fSumZZ *= recip;
// compute eigenstuff, smallest eigenvalue is in last position
kES.DecrSortEigenStuff();
// setup the eigensolver
Eigen kES;
Vec3 kNormal;
kES.mElement[0][0] = fSumXX;
kES.mElement[0][1] = fSumXY;
kES.mElement[0][2] = fSumXZ;
kNormal.x = kES.mElement[0][2];
kNormal.y = kES.mElement[1][2];
kNormal.z = kES.mElement[2][2];
kES.mElement[1][0] = fSumXY;
kES.mElement[1][1] = fSumYY;
kES.mElement[1][2] = fSumYZ;
// the minimum energy
plane[0] = kNormal.x;
plane[1] = kNormal.y;
plane[2] = kNormal.z;
kES.mElement[2][0] = fSumXZ;
kES.mElement[2][1] = fSumYZ;
kES.mElement[2][2] = fSumZZ;
plane[3] = 0 - kNormal.dot(kOrigin);
// compute eigenstuff, smallest eigenvalue is in last position
kES.DecrSortEigenStuff();
Vec3 kNormal;
kNormal.x = kES.mElement[0][2];
kNormal.y = kES.mElement[1][2];
kNormal.z = kES.mElement[2][2];
// the minimum energy
plane[0] = kNormal.x;
plane[1] = kNormal.y;
plane[2] = kNormal.z;
plane[3] = 0 - kNormal.dot(kOrigin);
return ret;
return ret;
}
float getBoundingRegion(unsigned int vcount,const float *points,unsigned int pstride,float *bmin,float *bmax) // returns the diagonal distance
float getBoundingRegion(unsigned int vcount, const float *points, unsigned int pstride, float *bmin, float *bmax) // returns the diagonal distance
{
const unsigned char *source = (const unsigned char *) points;
const unsigned char *source = (const unsigned char *)points;
bmin[0] = points[0];
bmin[1] = points[1];
@ -424,43 +413,36 @@ float getBoundingRegion(unsigned int vcount,const float *points,unsigned int pst
bmax[1] = points[1];
bmax[2] = points[2];
for (unsigned int i = 1; i < vcount; i++)
{
source += pstride;
const float *p = (const float *)source;
for (unsigned int i=1; i<vcount; i++)
{
source+=pstride;
const float *p = (const float *) source;
if (p[0] < bmin[0]) bmin[0] = p[0];
if (p[1] < bmin[1]) bmin[1] = p[1];
if (p[2] < bmin[2]) bmin[2] = p[2];
if ( p[0] < bmin[0] ) bmin[0] = p[0];
if ( p[1] < bmin[1] ) bmin[1] = p[1];
if ( p[2] < bmin[2] ) bmin[2] = p[2];
if (p[0] > bmax[0]) bmax[0] = p[0];
if (p[1] > bmax[1]) bmax[1] = p[1];
if (p[2] > bmax[2]) bmax[2] = p[2];
}
if ( p[0] > bmax[0] ) bmax[0] = p[0];
if ( p[1] > bmax[1] ) bmax[1] = p[1];
if ( p[2] > bmax[2] ) bmax[2] = p[2];
}
float dx = bmax[0] - bmin[0];
float dy = bmax[1] - bmin[1];
float dz = bmax[2] - bmin[2];
return sqrtf( dx*dx + dy*dy + dz*dz );
float dx = bmax[0] - bmin[0];
float dy = bmax[1] - bmin[1];
float dz = bmax[2] - bmin[2];
return sqrtf(dx * dx + dy * dy + dz * dz);
}
bool overlapAABB(const float *bmin1,const float *bmax1,const float *bmin2,const float *bmax2) // return true if the two AABB's overlap.
bool overlapAABB(const float *bmin1, const float *bmax1, const float *bmin2, const float *bmax2) // return true if the two AABB's overlap.
{
if ( bmax2[0] < bmin1[0] ) return false; // if the maximum is less than our minimum on any axis
if ( bmax2[1] < bmin1[1] ) return false;
if ( bmax2[2] < bmin1[2] ) return false;
if (bmax2[0] < bmin1[0]) return false; // if the maximum is less than our minimum on any axis
if (bmax2[1] < bmin1[1]) return false;
if (bmax2[2] < bmin1[2]) return false;
if ( bmin2[0] > bmax1[0] ) return false; // if the minimum is greater than our maximum on any axis
if ( bmin2[1] > bmax1[1] ) return false; // if the minimum is greater than our maximum on any axis
if ( bmin2[2] > bmax1[2] ) return false; // if the minimum is greater than our maximum on any axis
if (bmin2[0] > bmax1[0]) return false; // if the minimum is greater than our maximum on any axis
if (bmin2[1] > bmax1[1]) return false; // if the minimum is greater than our maximum on any axis
if (bmin2[2] > bmax1[2]) return false; // if the minimum is greater than our maximum on any axis
return true; // the extents overlap
return true; // the extents overlap
}

18
Extras/ConvexDecomposition/bestfit.h
View File

@ -36,7 +36,6 @@
// http://www.amillionpixels.us
//
// This routine was released in 'snippet' form
// by John W. Ratcliff mailto:jratcliff@infiniplex.net
// on March 22, 2006.
@ -51,15 +50,14 @@
// computes the best fit plane to a collection of data points.
// returns the plane equation as A,B,C,D format. (Ax+By+Cz+D)
bool getBestFitPlane(unsigned int vcount, // number of input data points
const float *points, // starting address of points array.
unsigned int vstride, // stride between input points.
const float *weights, // *optional point weighting values.
unsigned int wstride, // weight stride for each vertex.
float *plane);
bool getBestFitPlane(unsigned int vcount, // number of input data points
const float *points, // starting address of points array.
unsigned int vstride, // stride between input points.
const float *weights, // *optional point weighting values.
unsigned int wstride, // weight stride for each vertex.
float *plane);
float getBoundingRegion(unsigned int vcount,const float *points,unsigned int pstride,float *bmin,float *bmax); // returns the diagonal distance
bool overlapAABB(const float *bmin1,const float *bmax1,const float *bmin2,const float *bmax2); // return true if the two AABB's overlap.
float getBoundingRegion(unsigned int vcount, const float *points, unsigned int pstride, float *bmin, float *bmax); // returns the diagonal distance
bool overlapAABB(const float *bmin1, const float *bmax1, const float *bmin2, const float *bmax2); // return true if the two AABB's overlap.
#endif

179
Extras/ConvexDecomposition/bestfitobb.cpp
View File

@ -44,130 +44,123 @@
#include "float_math.h"
// computes the OBB for this set of points relative to this transform matrix.
void computeOBB(unsigned int vcount,const float *points,unsigned int pstride,float *sides,const float *matrix)
void computeOBB(unsigned int vcount, const float *points, unsigned int pstride, float *sides, const float *matrix)
{
const char *src = (const char *) points;
const char *src = (const char *)points;
float bmin[3] = { 1e9, 1e9, 1e9 };
float bmax[3] = { -1e9, -1e9, -1e9 };
float bmin[3] = {1e9, 1e9, 1e9};
float bmax[3] = {-1e9, -1e9, -1e9};
for (unsigned int i=0; i<vcount; i++)
{
const float *p = (const float *) src;
float t[3];
for (unsigned int i = 0; i < vcount; i++)
{
const float *p = (const float *)src;
float t[3];
fm_inverseRT(matrix, p, t ); // inverse rotate translate
fm_inverseRT(matrix, p, t); // inverse rotate translate
if ( t[0] < bmin[0] ) bmin[0] = t[0];
if ( t[1] < bmin[1] ) bmin[1] = t[1];
if ( t[2] < bmin[2] ) bmin[2] = t[2];
if (t[0] < bmin[0]) bmin[0] = t[0];
if (t[1] < bmin[1]) bmin[1] = t[1];
if (t[2] < bmin[2]) bmin[2] = t[2];
if ( t[0] > bmax[0] ) bmax[0] = t[0];
if ( t[1] > bmax[1] ) bmax[1] = t[1];
if ( t[2] > bmax[2] ) bmax[2] = t[2];
if (t[0] > bmax[0]) bmax[0] = t[0];
if (t[1] > bmax[1]) bmax[1] = t[1];
if (t[2] > bmax[2]) bmax[2] = t[2];
src+=pstride;
}
src += pstride;
}
sides[0] = bmax[0];
sides[1] = bmax[1];
sides[2] = bmax[2];
sides[0] = bmax[0];
sides[1] = bmax[1];
sides[2] = bmax[2];
if ( fabsf(bmin[0]) > sides[0] ) sides[0] = fabsf(bmin[0]);
if ( fabsf(bmin[1]) > sides[1] ) sides[1] = fabsf(bmin[1]);
if ( fabsf(bmin[2]) > sides[2] ) sides[2] = fabsf(bmin[2]);
sides[0]*=2.0f;
sides[1]*=2.0f;
sides[2]*=2.0f;
if (fabsf(bmin[0]) > sides[0]) sides[0] = fabsf(bmin[0]);
if (fabsf(bmin[1]) > sides[1]) sides[1] = fabsf(bmin[1]);
if (fabsf(bmin[2]) > sides[2]) sides[2] = fabsf(bmin[2]);
sides[0] *= 2.0f;
sides[1] *= 2.0f;
sides[2] *= 2.0f;
}
void computeBestFitOBB(unsigned int vcount,const float *points,unsigned int pstride,float *sides,float *matrix)
void computeBestFitOBB(unsigned int vcount, const float *points, unsigned int pstride, float *sides, float *matrix)
{
float bmin[3];
float bmax[3];
float bmin[3];
float bmax[3];
fm_getAABB(vcount, points, pstride, bmin, bmax);
fm_getAABB(vcount,points,pstride,bmin,bmax);
float center[3];
float center[3];
center[0] = (bmax[0] - bmin[0]) * 0.5f + bmin[0];
center[1] = (bmax[1] - bmin[1]) * 0.5f + bmin[1];
center[2] = (bmax[2] - bmin[2]) * 0.5f + bmin[2];
center[0] = (bmax[0]-bmin[0])*0.5f + bmin[0];
center[1] = (bmax[1]-bmin[1])*0.5f + bmin[1];
center[2] = (bmax[2]-bmin[2])*0.5f + bmin[2];
float ax = 0;
float ay = 0;
float az = 0;
float ax = 0;
float ay = 0;
float az = 0;
float sweep = 45.0f; // 180 degree sweep on all three axes.
float steps = 8.0f; // 16 steps on each axis.
float sweep = 45.0f; // 180 degree sweep on all three axes.
float steps = 8.0f; // 16 steps on each axis.
float bestVolume = 1e9;
float angle[3] = {0.f, 0.f, 0.f};
float bestVolume = 1e9;
float angle[3]={0.f,0.f,0.f};
while (sweep >= 1)
{
bool found = false;
while ( sweep >= 1 )
{
float stepsize = sweep / steps;
bool found = false;
for (float x = ax - sweep; x <= ax + sweep; x += stepsize)
{
for (float y = ay - sweep; y <= ay + sweep; y += stepsize)
{
for (float z = az - sweep; z <= az + sweep; z += stepsize)
{
float pmatrix[16];
float stepsize = sweep / steps;
fm_eulerMatrix(x * FM_DEG_TO_RAD, y * FM_DEG_TO_RAD, z * FM_DEG_TO_RAD, pmatrix);
for (float x=ax-sweep; x<=ax+sweep; x+=stepsize)
{
for (float y=ay-sweep; y<=ay+sweep; y+=stepsize)
{
for (float z=az-sweep; z<=az+sweep; z+=stepsize)
{
float pmatrix[16];
pmatrix[3 * 4 + 0] = center[0];
pmatrix[3 * 4 + 1] = center[1];
pmatrix[3 * 4 + 2] = center[2];
fm_eulerMatrix( x*FM_DEG_TO_RAD, y*FM_DEG_TO_RAD, z*FM_DEG_TO_RAD, pmatrix );
float psides[3];
pmatrix[3*4+0] = center[0];
pmatrix[3*4+1] = center[1];
pmatrix[3*4+2] = center[2];
computeOBB(vcount, points, pstride, psides, pmatrix);
float psides[3];
float volume = psides[0] * psides[1] * psides[2]; // the volume of the cube
computeOBB( vcount, points, pstride, psides, pmatrix );
if (volume <= bestVolume)
{
bestVolume = volume;
float volume = psides[0]*psides[1]*psides[2]; // the volume of the cube
sides[0] = psides[0];
sides[1] = psides[1];
sides[2] = psides[2];
if ( volume <= bestVolume )
{
bestVolume = volume;
angle[0] = ax;
angle[1] = ay;
angle[2] = az;
sides[0] = psides[0];
sides[1] = psides[1];
sides[2] = psides[2];
memcpy(matrix, pmatrix, sizeof(float) * 16);
found = true; // yes, we found an improvement.
}
}
}
}
angle[0] = ax;
angle[1] = ay;
angle[2] = az;
memcpy(matrix,pmatrix,sizeof(float)*16);
found = true; // yes, we found an improvement.
}
}
}
}
if ( found )
{
ax = angle[0];
ay = angle[1];
az = angle[2];
sweep*=0.5f; // sweep 1/2 the distance as the last time.
}
else
{
break; // no improvement, so just
}
}
if (found)
{
ax = angle[0];
ay = angle[1];
az = angle[2];
sweep *= 0.5f; // sweep 1/2 the distance as the last time.
}
else
{
break; // no improvement, so just
}
}
}

4
Extras/ConvexDecomposition/bestfitobb.h
View File

@ -36,8 +36,6 @@
// http://www.amillionpixels.us
//
void computeBestFitOBB(unsigned int vcount,const float *points,unsigned int pstride,float *sides,float *matrix);
void computeBestFitOBB(unsigned int vcount, const float *points, unsigned int pstride, float *sides, float *matrix);
#endif

3403
Extras/ConvexDecomposition/cd_hull.cpp
View File

File diff suppressed because it is too large Load Diff

115
Extras/ConvexDecomposition/cd_hull.h
View File

@ -31,7 +31,6 @@
namespace ConvexDecomposition
{
class HullResult
{
public:
@ -44,110 +43,106 @@ public:
mNumIndices = 0;
mIndices = 0;
}
bool mPolygons; // true if indices represents polygons, false indices are triangles
unsigned int mNumOutputVertices; // number of vertices in the output hull
float *mOutputVertices; // array of vertices, 3 floats each x,y,z
unsigned int mNumFaces; // the number of faces produced
unsigned int mNumIndices; // the total number of indices
unsigned int *mIndices; // pointer to indices.
bool mPolygons; // true if indices represents polygons, false indices are triangles
unsigned int mNumOutputVertices; // number of vertices in the output hull
float *mOutputVertices; // array of vertices, 3 floats each x,y,z
unsigned int mNumFaces; // the number of faces produced
unsigned int mNumIndices; // the total number of indices
unsigned int *mIndices; // pointer to indices.
// If triangles, then indices are array indexes into the vertex list.
// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc..
// If triangles, then indices are array indexes into the vertex list.
// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc..
};
enum HullFlag
{
QF_TRIANGLES = (1<<0), // report results as triangles, not polygons.
QF_REVERSE_ORDER = (1<<1), // reverse order of the triangle indices.
QF_SKIN_WIDTH = (1<<2), // extrude hull based on this skin width
QF_DEFAULT = 0
QF_TRIANGLES = (1 << 0), // report results as triangles, not polygons.
QF_REVERSE_ORDER = (1 << 1), // reverse order of the triangle indices.
QF_SKIN_WIDTH = (1 << 2), // extrude hull based on this skin width
QF_DEFAULT = 0
};
class HullDesc
{
public:
HullDesc(void)
{
mFlags = QF_DEFAULT;
mVcount = 0;
mVertices = 0;
mVertexStride = sizeof(float)*3;
mNormalEpsilon = 0.001f;
mMaxVertices = 4096; // maximum number of points to be considered for a convex hull.
mMaxFaces = 4096;
mSkinWidth = 0.01f; // default is one centimeter
mFlags = QF_DEFAULT;
mVcount = 0;
mVertices = 0;
mVertexStride = sizeof(float) * 3;
mNormalEpsilon = 0.001f;
mMaxVertices = 4096; // maximum number of points to be considered for a convex hull.
mMaxFaces = 4096;
mSkinWidth = 0.01f; // default is one centimeter
};
HullDesc(HullFlag flag,
unsigned int vcount,
const float *vertices,
unsigned int stride)
unsigned int vcount,
const float *vertices,
unsigned int stride)
{
mFlags = flag;
mVcount = vcount;
mVertices = vertices;
mVertexStride = stride;
mNormalEpsilon = 0.001f;
mMaxVertices = 4096;
mSkinWidth = 0.01f; // default is one centimeter
mFlags = flag;
mVcount = vcount;
mVertices = vertices;
mVertexStride = stride;
mNormalEpsilon = 0.001f;
mMaxVertices = 4096;
mSkinWidth = 0.01f; // default is one centimeter
}
bool HasHullFlag(HullFlag flag) const
{
if ( mFlags & flag ) return true;
if (mFlags & flag) return true;
return false;
}
void SetHullFlag(HullFlag flag)
{
mFlags|=flag;
mFlags |= flag;
}
void ClearHullFlag(HullFlag flag)
{
mFlags&=~flag;
mFlags &= ~flag;
}
unsigned int mFlags; // flags to use when generating the convex hull.
unsigned int mVcount; // number of vertices in the input point cloud
const float *mVertices; // the array of vertices.
unsigned int mVertexStride; // the stride of each vertex, in bytes.
float mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on.
float mSkinWidth;
unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull!
unsigned int mMaxFaces;
unsigned int mFlags; // flags to use when generating the convex hull.
unsigned int mVcount; // number of vertices in the input point cloud
const float *mVertices; // the array of vertices.
unsigned int mVertexStride; // the stride of each vertex, in bytes.
float mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on.
float mSkinWidth;
unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull!
unsigned int mMaxFaces;
};
enum HullError
{
QE_OK, // success!
QE_FAIL // failed.
QE_OK, // success!
QE_FAIL // failed.
};
class HullLibrary
{
public:
HullError CreateConvexHull(const HullDesc &desc, // describes the input request
HullResult &result); // contains the resulst
HullError CreateConvexHull(const HullDesc &desc, // describes the input request
HullResult &result); // contains the resulst
HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it.
HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it.
private:
void BringOutYourDead(const float *verts, unsigned int vcount, float *overts, unsigned int &ocount, unsigned int *indices, unsigned indexcount);
void BringOutYourDead(const float *verts,unsigned int vcount, float *overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount);
bool CleanupVertices(unsigned int svcount,
const float *svertices,
unsigned int stride,
unsigned int &vcount, // output number of vertices
float *vertices, // location to store the results.
float normalepsilon,
float *scale);
bool CleanupVertices(unsigned int svcount,
const float *svertices,
unsigned int stride,
unsigned int &vcount, // output number of vertices
float *vertices, // location to store the results.
float normalepsilon,
float *scale);
};
}
} // namespace ConvexDecomposition
#endif

745
Extras/ConvexDecomposition/cd_vector.h
View File

File diff suppressed because it is too large Load Diff

686
Extras/ConvexDecomposition/cd_wavefront.cpp
View File

File diff suppressed because it is too large Load Diff

21
Extras/ConvexDecomposition/cd_wavefront.h
View File

@ -1,9 +1,7 @@
#ifndef CD_WAVEFRONT_OBJ_H
#define CD_WAVEFRONT_OBJ_H
/*----------------------------------------------------------------------
Copyright (c) 2004 Open Dynamics Framework Group
www.physicstools.org
@ -38,25 +36,22 @@
// http://www.amillionpixels.us
//
namespace ConvexDecomposition
{
class WavefrontObj
{
public:
WavefrontObj(void);
~WavefrontObj(void);
WavefrontObj(void);
~WavefrontObj(void);
unsigned int loadObj(const char *fname); // load a wavefront obj returns number of triangles that were loaded. Data is persists until the class is destructed.
unsigned int loadObj(const char *fname); // load a wavefront obj returns number of triangles that were loaded. Data is persists until the class is destructed.
int mVertexCount;
int mTriCount;
int *mIndices;
float *mVertices;
int mVertexCount;
int mTriCount;
int *mIndices;
float *mVertices;
};
}
} // namespace ConvexDecomposition
#endif

696
Extras/ConvexDecomposition/concavity.cpp
View File

@ -49,165 +49,156 @@
#include "splitplane.h"
#include "ConvexDecomposition.h"
#define WSCALE 4
#define CONCAVE_THRESH 0.05f
namespace ConvexDecomposition
{
unsigned int getDebugColor(void)
{
static unsigned int colors[8] =
{
0xFF0000,
0x00FF00,
0x0000FF,
0xFFFF00,
0x00FFFF,
0xFF00FF,
0xFFFFFF,
0xFF8040
};
{
0xFF0000,
0x00FF00,
0x0000FF,
0xFFFF00,
0x00FFFF,
0xFF00FF,
0xFFFFFF,
0xFF8040};
static int count = 0;
count++;
if ( count == 8 ) count = 0;
if (count == 8) count = 0;
assert( count >= 0 && count < 8 );
assert(count >= 0 && count < 8);
unsigned int color = colors[count];
return color;
return color;
}
class Wpoint
{
public:
Wpoint(const Vector3d &p,float w)
{
mPoint = p;
mWeight = w;
}
Wpoint(const Vector3d &p, float w)
{
mPoint = p;
mWeight = w;
}
Vector3d mPoint;
float mWeight;
Vector3d mPoint;
float mWeight;
};
typedef std::vector< Wpoint > WpointVector;
typedef std::vector<Wpoint> WpointVector;
static inline float DistToPt(const float *p,const float *plane)
static inline float DistToPt(const float *p, const float *plane)
{
float x = p[0];
float y = p[1];
float z = p[2];
float d = x*plane[0] + y*plane[1] + z*plane[2] + plane[3];
float d = x * plane[0] + y * plane[1] + z * plane[2] + plane[3];
return d;
}
static void intersect(const float *p1,const float *p2,float *split,const float *plane)
static void intersect(const float *p1, const float *p2, float *split, const float *plane)
{
float dp1 = DistToPt(p1, plane);
float dp1 = DistToPt(p1,plane);
float dir[3];
float dir[3];
dir[0] = p2[0] - p1[0];
dir[1] = p2[1] - p1[1];
dir[2] = p2[2] - p1[2];
dir[0] = p2[0] - p1[0];
dir[1] = p2[1] - p1[1];
dir[2] = p2[2] - p1[2];
float dot1 = dir[0] * plane[0] + dir[1] * plane[1] + dir[2] * plane[2];
float dot2 = dp1 - plane[3];
float dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2];
float dot2 = dp1 - plane[3];
float t = -(plane[3] + dot2) / dot1;
float t = -(plane[3] + dot2 ) / dot1;
split[0] = (dir[0]*t)+p1[0];
split[1] = (dir[1]*t)+p1[1];
split[2] = (dir[2]*t)+p1[2];
split[0] = (dir[0] * t) + p1[0];
split[1] = (dir[1] * t) + p1[1];
split[2] = (dir[2] * t) + p1[2];
}
class CTri
{
public:
CTri(void) { };
CTri(void){};
CTri(const float *p1,const float *p2,const float *p3,unsigned int i1,unsigned int i2,unsigned int i3)
{
mProcessed = 0;
mI1 = i1;
mI2 = i2;
mI3 = i3;
CTri(const float *p1, const float *p2, const float *p3, unsigned int i1, unsigned int i2, unsigned int i3)
{
mProcessed = 0;
mI1 = i1;
mI2 = i2;
mI3 = i3;
mP1.Set(p1);
mP2.Set(p2);
mP3.Set(p3);
mP1.Set(p1);
mP2.Set(p2);
mP3.Set(p3);
mPlaneD = mNormal.ComputePlane(mP1,mP2,mP3);
mPlaneD = mNormal.ComputePlane(mP1, mP2, mP3);
}
float Facing(const CTri &t)
{
float Facing(const CTri &t)
{
float d = mNormal.Dot(t.mNormal);
return d;
}
}
// clip this line segment against this triangle.
bool clip(const Vector3d &start,Vector3d &end) const
{
Vector3d sect;
bool hit = lineIntersectsTriangle(start.Ptr(), end.Ptr(), mP1.Ptr(), mP2.Ptr(), mP3.Ptr(), sect.Ptr() );
if ( hit )
{
end = sect;
}
return hit;
}
bool Concave(const Vector3d &p,float &distance,Vector3d &n) const
// clip this line segment against this triangle.
bool clip(const Vector3d &start, Vector3d &end) const
{
n.NearestPointInTriangle(p,mP1,mP2,mP3);
Vector3d sect;
bool hit = lineIntersectsTriangle(start.Ptr(), end.Ptr(), mP1.Ptr(), mP2.Ptr(), mP3.Ptr(), sect.Ptr());
if (hit)
{
end = sect;
}
return hit;
}
bool Concave(const Vector3d &p, float &distance, Vector3d &n) const
{
n.NearestPointInTriangle(p, mP1, mP2, mP3);
distance = p.Distance(n);
return true;
}
void addTri(unsigned int *indices,unsigned int i1,unsigned int i2,unsigned int i3,unsigned int &tcount) const
void addTri(unsigned int *indices, unsigned int i1, unsigned int i2, unsigned int i3, unsigned int &tcount) const
{
indices[tcount*3+0] = i1;
indices[tcount*3+1] = i2;
indices[tcount*3+2] = i3;
indices[tcount * 3 + 0] = i1;
indices[tcount * 3 + 1] = i2;
indices[tcount * 3 + 2] = i3;
tcount++;
}
float getVolume(ConvexDecompInterface *callback) const
{
unsigned int indices[8*3];
unsigned int indices[8 * 3];
unsigned int tcount = 0;
unsigned int tcount = 0;
addTri(indices, 0, 1, 2, tcount);
addTri(indices, 3, 4, 5, tcount);
addTri(indices,0,1,2,tcount);
addTri(indices,3,4,5,tcount);
addTri(indices, 0, 3, 4, tcount);
addTri(indices, 0, 4, 1, tcount);
addTri(indices,0,3,4,tcount);
addTri(indices,0,4,1,tcount);
addTri(indices, 1, 4, 5, tcount);
addTri(indices, 1, 5, 2, tcount);
addTri(indices,1,4,5,tcount);
addTri(indices,1,5,2,tcount);
addTri(indices, 0, 3, 5, tcount);
addTri(indices, 0, 5, 2, tcount);
addTri(indices,0,3,5,tcount);
addTri(indices,0,5,2,tcount);
const float *vertices = mP1.Ptr();
const float *vertices = mP1.Ptr();
if ( callback )
if (callback)
{
unsigned int color = getDebugColor();
@ -228,139 +219,134 @@ public:
callback->ConvexDebugTri(mP3.Ptr(), d3.Ptr(), d3.Ptr(),0x00FF00);
#else
for (unsigned int i=0; i<tcount; i++)
for (unsigned int i = 0; i < tcount; i++)
{
unsigned int i1 = indices[i*3+0];
unsigned int i2 = indices[i*3+1];
unsigned int i3 = indices[i*3+2];
unsigned int i1 = indices[i * 3 + 0];
unsigned int i2 = indices[i * 3 + 1];
unsigned int i3 = indices[i * 3 + 2];
const float *p1 = &vertices[ i1*3 ];
const float *p2 = &vertices[ i2*3 ];
const float *p3 = &vertices[ i3*3 ];
callback->ConvexDebugTri(p1,p2,p3,color);
const float *p1 = &vertices[i1 * 3];
const float *p2 = &vertices[i2 * 3];
const float *p3 = &vertices[i3 * 3];
callback->ConvexDebugTri(p1, p2, p3, color);
}
#endif
}
float v = computeMeshVolume(mP1.Ptr(), tcount, indices );
float v = computeMeshVolume(mP1.Ptr(), tcount, indices);
return v;
}
float raySect(const Vector3d &p,const Vector3d &dir,Vector3d &sect) const
float raySect(const Vector3d &p, const Vector3d &dir, Vector3d &sect) const
{
float plane[4];
plane[0] = mNormal.x;
plane[1] = mNormal.y;
plane[2] = mNormal.z;
plane[3] = mPlaneD;
plane[0] = mNormal.x;
plane[1] = mNormal.y;
plane[2] = mNormal.z;
plane[3] = mPlaneD;
Vector3d dest = p+dir*100000;
Vector3d dest = p + dir * 100000;
intersect( p.Ptr(), dest.Ptr(), sect.Ptr(), plane );
return sect.Distance(p); // return the intersection distance.
intersect(p.Ptr(), dest.Ptr(), sect.Ptr(), plane);
return sect.Distance(p); // return the intersection distance.
}
float planeDistance(const Vector3d &p) const
{
float planeDistance(const Vector3d &p) const
{
float plane[4];
plane[0] = mNormal.x;
plane[1] = mNormal.y;
plane[2] = mNormal.z;
plane[3] = mPlaneD;
plane[0] = mNormal.x;
plane[1] = mNormal.y;
plane[2] = mNormal.z;
plane[3] = mPlaneD;
return DistToPt( p.Ptr(), plane );
}
return DistToPt(p.Ptr(), plane);
}
bool samePlane(const CTri &t) const
{
const float THRESH = 0.001f;
float dd = fabsf( t.mPlaneD - mPlaneD );
if ( dd > THRESH ) return false;
dd = fabsf( t.mNormal.x - mNormal.x );
if ( dd > THRESH ) return false;
dd = fabsf( t.mNormal.y - mNormal.y );
if ( dd > THRESH ) return false;
dd = fabsf( t.mNormal.z - mNormal.z );
if ( dd > THRESH ) return false;
return true;
float dd = fabsf(t.mPlaneD - mPlaneD);
if (dd > THRESH) return false;
dd = fabsf(t.mNormal.x - mNormal.x);
if (dd > THRESH) return false;
dd = fabsf(t.mNormal.y - mNormal.y);
if (dd > THRESH) return false;
dd = fabsf(t.mNormal.z - mNormal.z);
if (dd > THRESH) return false;
return true;
}
bool hasIndex(unsigned int i) const
{
if ( i == mI1 || i == mI2 || i == mI3 ) return true;
if (i == mI1 || i == mI2 || i == mI3) return true;
return false;
}
bool sharesEdge(const CTri &t) const
{
bool ret = false;
unsigned int count = 0;
bool sharesEdge(const CTri &t) const
{
bool ret = false;
unsigned int count = 0;
if ( t.hasIndex(mI1) ) count++;
if ( t.hasIndex(mI2) ) count++;
if ( t.hasIndex(mI3) ) count++;
if (t.hasIndex(mI1)) count++;
if (t.hasIndex(mI2)) count++;
if (t.hasIndex(mI3)) count++;
if ( count >= 2 ) ret = true;
if (count >= 2) ret = true;
return ret;
}
return ret;
}
void debug(unsigned int color,ConvexDecompInterface *callback)
{
callback->ConvexDebugTri( mP1.Ptr(), mP2.Ptr(), mP3.Ptr(), color );
callback->ConvexDebugTri( mP1.Ptr(), mP1.Ptr(), mNear1.Ptr(), 0xFF0000 );
callback->ConvexDebugTri( mP2.Ptr(), mP2.Ptr(), mNear2.Ptr(), 0xFF0000 );
callback->ConvexDebugTri( mP2.Ptr(), mP3.Ptr(), mNear3.Ptr(), 0xFF0000 );
callback->ConvexDebugPoint( mNear1.Ptr(), 0.01f, 0xFF0000 );
callback->ConvexDebugPoint( mNear2.Ptr(), 0.01f, 0xFF0000 );
callback->ConvexDebugPoint( mNear3.Ptr(), 0.01f, 0xFF0000 );
}
void debug(unsigned int color, ConvexDecompInterface *callback)
{
callback->ConvexDebugTri(mP1.Ptr(), mP2.Ptr(), mP3.Ptr(), color);
callback->ConvexDebugTri(mP1.Ptr(), mP1.Ptr(), mNear1.Ptr(), 0xFF0000);
callback->ConvexDebugTri(mP2.Ptr(), mP2.Ptr(), mNear2.Ptr(), 0xFF0000);
callback->ConvexDebugTri(mP2.Ptr(), mP3.Ptr(), mNear3.Ptr(), 0xFF0000);
callback->ConvexDebugPoint(mNear1.Ptr(), 0.01f, 0xFF0000);
callback->ConvexDebugPoint(mNear2.Ptr(), 0.01f, 0xFF0000);
callback->ConvexDebugPoint(mNear3.Ptr(), 0.01f, 0xFF0000);
}
float area(void)
{
float a = mConcavity*mP1.Area(mP2,mP3);
return a;
}
float area(void)
{
float a = mConcavity * mP1.Area(mP2, mP3);
return a;
}
void addWeighted(WpointVector &list,ConvexDecompInterface *callback)
{
Wpoint p1(mP1,mC1);
Wpoint p2(mP2,mC2);
Wpoint p3(mP3,mC3);
void addWeighted(WpointVector &list, ConvexDecompInterface *callback)
{
Wpoint p1(mP1, mC1);
Wpoint p2(mP2, mC2);
Wpoint p3(mP3, mC3);
Vector3d d1 = mNear1 - mP1;
Vector3d d2 = mNear2 - mP2;
Vector3d d3 = mNear3 - mP3;
d1*=WSCALE;
d2*=WSCALE;
d3*=WSCALE;
d1 *= WSCALE;
d2 *= WSCALE;
d3 *= WSCALE;
d1 = d1 + mP1;
d2 = d2 + mP2;
d3 = d3 + mP3;
d3 = d3 + mP3;
Wpoint p4(d1,mC1);
Wpoint p5(d2,mC2);
Wpoint p6(d3,mC3);
Wpoint p4(d1, mC1);
Wpoint p5(d2, mC2);
Wpoint p6(d3, mC3);
list.push_back(p1);
list.push_back(p2);
list.push_back(p3);
list.push_back(p1);
list.push_back(p2);
list.push_back(p3);
list.push_back(p4);
list.push_back(p5);
list.push_back(p6);
list.push_back(p4);
list.push_back(p5);
list.push_back(p6);
#if 0
callback->ConvexDebugPoint(mP1.Ptr(),0.01f,0x00FF00);
@ -387,39 +373,35 @@ public:
callback->ConvexDebugPoint( np3.Ptr(), 0.01F, 0XFF00FF );
#endif
}
}
Vector3d mP1;
Vector3d mP2;
Vector3d mP3;
Vector3d mNear1;
Vector3d mNear2;
Vector3d mNear3;
Vector3d mNormal;
float mPlaneD;
float mConcavity;
float mC1;
float mC2;
float mC3;
unsigned int mI1;
unsigned int mI2;
unsigned int mI3;
int mProcessed; // already been added...
Vector3d mP1;
Vector3d mP2;
Vector3d mP3;
Vector3d mNear1;
Vector3d mNear2;
Vector3d mNear3;
Vector3d mNormal;
float mPlaneD;
float mConcavity;
float mC1;
float mC2;
float mC3;
unsigned int mI1;
unsigned int mI2;
unsigned int mI3;
int mProcessed; // already been added...
};
typedef std::vector< CTri > CTriVector;
typedef std::vector<CTri> CTriVector;
bool featureMatch(CTri &m,const CTriVector &tris,ConvexDecompInterface *callback,const CTriVector &input_mesh)
bool featureMatch(CTri &m, const CTriVector &tris, ConvexDecompInterface *callback, const CTriVector &input_mesh)
{
bool ret = false;
bool ret = false;
float neardot = 0.707f;
float neardot = 0.707f;
m.mConcavity = 0;
m.mConcavity = 0;
//gLog->Display("*********** FEATURE MATCH *************\r\n");
//gLog->Display("Plane: %0.4f,%0.4f,%0.4f %0.4f\r\n", m.mNormal.x, m.mNormal.y, m.mNormal.z, m.mPlaneD );
@ -429,87 +411,81 @@ bool featureMatch(CTri &m,const CTriVector &tris,ConvexDecompInterface *callback
CTri nearest;
for (i=tris.begin(); i!=tris.end(); ++i)
for (i = tris.begin(); i != tris.end(); ++i)
{
const CTri &t = (*i);
//gLog->Display(" HullPlane: %0.4f,%0.4f,%0.4f %0.4f\r\n", t.mNormal.x, t.mNormal.y, t.mNormal.z, t.mPlaneD );
//gLog->Display(" HullPlane: %0.4f,%0.4f,%0.4f %0.4f\r\n", t.mNormal.x, t.mNormal.y, t.mNormal.z, t.mPlaneD );
if ( t.samePlane(m) )
if (t.samePlane(m))
{
//gLog->Display("*** PLANE MATCH!!!\r\n");
ret = false;
break;
}
float dot = t.mNormal.Dot(m.mNormal);
float dot = t.mNormal.Dot(m.mNormal);
if ( dot > neardot )
{
if (dot > neardot)
{
float d1 = t.planeDistance(m.mP1);
float d2 = t.planeDistance(m.mP2);
float d3 = t.planeDistance(m.mP3);
float d1 = t.planeDistance( m.mP1 );
float d2 = t.planeDistance( m.mP2 );
float d3 = t.planeDistance( m.mP3 );
if (d1 > 0.001f || d2 > 0.001f || d3 > 0.001f) // can't be near coplaner!
{
neardot = dot;
if ( d1 > 0.001f || d2 > 0.001f || d3 > 0.001f ) // can't be near coplaner!
{
Vector3d n1, n2, n3;
neardot = dot;
Vector3d n1,n2,n3;
t.raySect( m.mP1, m.mNormal, m.mNear1 );
t.raySect( m.mP2, m.mNormal, m.mNear2 );
t.raySect( m.mP3, m.mNormal, m.mNear3 );
t.raySect(m.mP1, m.mNormal, m.mNear1);
t.raySect(m.mP2, m.mNormal, m.mNear2);
t.raySect(m.mP3, m.mNormal, m.mNear3);
nearest = t;
ret = true;
}
}
ret = true;
}
}
}
if ( ret )
if (ret)
{
if ( 0 )
{
CTriVector::const_iterator i;
for (i=input_mesh.begin(); i!=input_mesh.end(); ++i)
{
const CTri &c = (*i);
if ( c.mI1 != m.mI1 && c.mI2 != m.mI2 && c.mI3 != m.mI3 )
{
c.clip( m.mP1, m.mNear1 );
c.clip( m.mP2, m.mNear2 );
c.clip( m.mP3, m.mNear3 );
}
}
}
if (0)
{
CTriVector::const_iterator i;
for (i = input_mesh.begin(); i != input_mesh.end(); ++i)
{
const CTri &c = (*i);
if (c.mI1 != m.mI1 && c.mI2 != m.mI2 && c.mI3 != m.mI3)
{
c.clip(m.mP1, m.mNear1);
c.clip(m.mP2, m.mNear2);
c.clip(m.mP3, m.mNear3);
}
}
}
//gLog->Display("*********************************************\r\n");
//gLog->Display(" HullPlaneNearest: %0.4f,%0.4f,%0.4f %0.4f\r\n", nearest.mNormal.x, nearest.mNormal.y, nearest.mNormal.z, nearest.mPlaneD );
//gLog->Display("*********************************************\r\n");
//gLog->Display(" HullPlaneNearest: %0.4f,%0.4f,%0.4f %0.4f\r\n", nearest.mNormal.x, nearest.mNormal.y, nearest.mNormal.z, nearest.mPlaneD );
m.mC1 = m.mP1.Distance( m.mNear1 );
m.mC2 = m.mP2.Distance( m.mNear2 );
m.mC3 = m.mP3.Distance( m.mNear3 );
m.mC1 = m.mP1.Distance(m.mNear1);
m.mC2 = m.mP2.Distance(m.mNear2);
m.mC3 = m.mP3.Distance(m.mNear3);
m.mConcavity = m.mC1;
if ( m.mC2 > m.mConcavity ) m.mConcavity = m.mC2;
if ( m.mC3 > m.mConcavity ) m.mConcavity = m.mC3;
if (m.mC2 > m.mConcavity) m.mConcavity = m.mC2;
if (m.mC3 > m.mConcavity) m.mConcavity = m.mC3;
#if 0
#if 0
callback->ConvexDebugTri( m.mP1.Ptr(), m.mP2.Ptr(), m.mP3.Ptr(), 0x00FF00 );
callback->ConvexDebugTri( m.mNear1.Ptr(), m.mNear2.Ptr(), m.mNear3.Ptr(), 0xFF0000 );
callback->ConvexDebugTri( m.mP1.Ptr(), m.mP1.Ptr(), m.mNear1.Ptr(), 0xFFFF00 );
callback->ConvexDebugTri( m.mP2.Ptr(), m.mP2.Ptr(), m.mNear2.Ptr(), 0xFFFF00 );
callback->ConvexDebugTri( m.mP3.Ptr(), m.mP3.Ptr(), m.mNear3.Ptr(), 0xFFFF00 );
#endif
#endif
}
else
{
@ -520,83 +496,78 @@ bool featureMatch(CTri &m,const CTriVector &tris,ConvexDecompInterface *callback
return ret;
}
bool isFeatureTri(CTri &t,CTriVector &flist,float fc,ConvexDecompInterface *callback,unsigned int color)
bool isFeatureTri(CTri &t, CTriVector &flist, float fc, ConvexDecompInterface *callback, unsigned int color)
{
bool ret = false;
bool ret = false;
if ( t.mProcessed == 0 ) // if not already processed
{
if (t.mProcessed == 0) // if not already processed
{
float c = t.mConcavity / fc; // must be within 80% of the concavity of the parent.
float c = t.mConcavity / fc; // must be within 80% of the concavity of the parent.
if ( c > 0.85f )
{
// see if this triangle is a 'feature' triangle. Meaning it shares an
// edge with any existing feature triangle and is within roughly the same
// concavity of the parent.
if ( flist.size() )
if (c > 0.85f)
{
// see if this triangle is a 'feature' triangle. Meaning it shares an
// edge with any existing feature triangle and is within roughly the same
// concavity of the parent.
if (flist.size())
{
CTriVector::iterator i;
for (i=flist.begin(); i!=flist.end(); ++i)
{
CTri &ftri = (*i);
if ( ftri.sharesEdge(t) )
{
t.mProcessed = 2; // it is now part of a feature.
flist.push_back(t); // add it to the feature list.
// callback->ConvexDebugTri( t.mP1.Ptr(), t.mP2.Ptr(),t.mP3.Ptr(), color );
ret = true;
break;
}
CTriVector::iterator i;
for (i = flist.begin(); i != flist.end(); ++i)
{
CTri &ftri = (*i);
if (ftri.sharesEdge(t))
{
t.mProcessed = 2; // it is now part of a feature.
flist.push_back(t); // add it to the feature list.
// callback->ConvexDebugTri( t.mP1.Ptr(), t.mP2.Ptr(),t.mP3.Ptr(), color );
ret = true;
break;
}
}
}
else
{
t.mProcessed = 2;
flist.push_back(t); // add it to the feature list.
// callback->ConvexDebugTri( t.mP1.Ptr(), t.mP2.Ptr(),t.mP3.Ptr(), color );
flist.push_back(t); // add it to the feature list.
// callback->ConvexDebugTri( t.mP1.Ptr(), t.mP2.Ptr(),t.mP3.Ptr(), color );
ret = true;
}
}
else
{
t.mProcessed = 1; // eliminated for this feature, but might be valid for the next one..
}
}
return ret;
}
else
{
t.mProcessed = 1; // eliminated for this feature, but might be valid for the next one..
}
}
return ret;
}
float computeConcavity(unsigned int vcount,
const float *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
float *plane, // plane equation to split on
float &volume)
const float *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
float *plane, // plane equation to split on
float &volume)
{
float cret = 0;
volume = 1;
HullResult result;
HullLibrary hl;
HullDesc desc;
HullResult result;
HullLibrary hl;
HullDesc desc;
desc.mMaxFaces = 256;
desc.mMaxVertices = 256;
desc.SetHullFlag(QF_TRIANGLES);
desc.mVcount = vcount;
desc.mVertices = vertices;
desc.mVertexStride = sizeof(float) * 3;
desc.mVcount = vcount;
desc.mVertices = vertices;
desc.mVertexStride = sizeof(float)*3;
HullError ret = hl.CreateConvexHull(desc, result);
HullError ret = hl.CreateConvexHull(desc,result);
if ( ret == QE_OK )
{
if (ret == QE_OK)
{
#if 0
float bmin[3];
float bmax[3];
@ -612,7 +583,7 @@ float computeConcavity(unsigned int vcount,
center.z = bmin[2] + dz*0.5f;
#endif
volume = computeMeshVolume2( result.mOutputVertices, result.mNumFaces, result.mIndices );
volume = computeMeshVolume2(result.mOutputVertices, result.mNumFaces, result.mIndices);
#if 1
// ok..now..for each triangle on the original mesh..
@ -621,87 +592,81 @@ float computeConcavity(unsigned int vcount,
CTriVector tris;
for (unsigned int i=0; i<result.mNumFaces; i++)
{
unsigned int i1 = *source++;
unsigned int i2 = *source++;
unsigned int i3 = *source++;
for (unsigned int i = 0; i < result.mNumFaces; i++)
{
unsigned int i1 = *source++;
unsigned int i2 = *source++;
unsigned int i3 = *source++;
const float *p1 = &result.mOutputVertices[i1*3];
const float *p2 = &result.mOutputVertices[i2*3];
const float *p3 = &result.mOutputVertices[i3*3];
const float *p1 = &result.mOutputVertices[i1 * 3];
const float *p2 = &result.mOutputVertices[i2 * 3];
const float *p3 = &result.mOutputVertices[i3 * 3];
// callback->ConvexDebugTri(p1,p2,p3,0xFFFFFF);
// callback->ConvexDebugTri(p1,p2,p3,0xFFFFFF);
CTri t(p1,p2,p3,i1,i2,i3); //
CTri t(p1, p2, p3, i1, i2, i3); //
tris.push_back(t);
}
// we have not pre-computed the plane equation for each triangle in the convex hull..
// we have not pre-computed the plane equation for each triangle in the convex hull..
float totalVolume = 0;
CTriVector ftris; // 'feature' triangles.
CTriVector ftris; // 'feature' triangles.
const unsigned int *src = indices;
float maxc = 0;
float maxc=0;
if ( 1 )
if (1)
{
CTriVector input_mesh;
if ( 1 )
{
const unsigned int *src = indices;
for (unsigned int i=0; i<tcount; i++)
{
unsigned int i1 = *src++;
unsigned int i2 = *src++;
unsigned int i3 = *src++;
const float *p1 = &vertices[i1*3];
const float *p2 = &vertices[i2*3];
const float *p3 = &vertices[i3*3];
CTri t(p1,p2,p3,i1,i2,i3);
input_mesh.push_back(t);
}
}
CTri maxctri;
for (unsigned int i=0; i<tcount; i++)
CTriVector input_mesh;
if (1)
{
unsigned int i1 = *src++;
unsigned int i2 = *src++;
unsigned int i3 = *src++;
const float *p1 = &vertices[i1*3];
const float *p2 = &vertices[i2*3];
const float *p3 = &vertices[i3*3];
CTri t(p1,p2,p3,i1,i2,i3);
featureMatch(t, tris, callback, input_mesh );
if ( t.mConcavity > CONCAVE_THRESH )
const unsigned int *src = indices;
for (unsigned int i = 0; i < tcount; i++)
{
unsigned int i1 = *src++;
unsigned int i2 = *src++;
unsigned int i3 = *src++;
if ( t.mConcavity > maxc )
{
maxc = t.mConcavity;
maxctri = t;
}
const float *p1 = &vertices[i1 * 3];
const float *p2 = &vertices[i2 * 3];
const float *p3 = &vertices[i3 * 3];
float v = t.getVolume(0);
totalVolume+=v;
ftris.push_back(t);
}
CTri t(p1, p2, p3, i1, i2, i3);
input_mesh.push_back(t);
}
}
CTri maxctri;
for (unsigned int i = 0; i < tcount; i++)
{
unsigned int i1 = *src++;
unsigned int i2 = *src++;
unsigned int i3 = *src++;
const float *p1 = &vertices[i1 * 3];
const float *p2 = &vertices[i2 * 3];
const float *p3 = &vertices[i3 * 3];
CTri t(p1, p2, p3, i1, i2, i3);
featureMatch(t, tris, callback, input_mesh);
if (t.mConcavity > CONCAVE_THRESH)
{
if (t.mConcavity > maxc)
{
maxc = t.mConcavity;
maxctri = t;
}
float v = t.getVolume(0);
totalVolume += v;
ftris.push_back(t);
}
}
}
@ -786,12 +751,11 @@ float computeConcavity(unsigned int vcount,
cret = totalVolume;
hl.ReleaseResult(result);
}
hl.ReleaseResult(result);
}
#endif
return cret;
}
}
} // namespace ConvexDecomposition

19
Extras/ConvexDecomposition/concavity.h
View File

@ -36,25 +36,20 @@
// http://www.amillionpixels.us
//
namespace ConvexDecomposition
{
class ConvexDecompInterface;
// compute's how 'concave' this object is and returns the total volume of the
// convex hull as well as the volume of the 'concavity' which was found.
float computeConcavity(unsigned int vcount,
const float *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
float *plane,
float &volume);
}
const float *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
float *plane,
float &volume);
} // namespace ConvexDecomposition
#endif

173
Extras/ConvexDecomposition/fitsphere.cpp
View File

@ -6,7 +6,6 @@
#include "fitsphere.h"
/*----------------------------------------------------------------------
Copyright (c) 2004 Open Dynamics Framework Group
www.physicstools.org
@ -53,27 +52,26 @@ from "Graphics Gems", Academic Press, 1990
/* The abs() of all coordinates must be < BIGNUMBER */
/* Code written by Jack Ritter and Lyle Rains. */
#define BIGNUMBER 100000000.0 /* hundred million */
#define BIGNUMBER 100000000.0 /* hundred million */
static inline void Set(float *n,float x,float y,float z)
static inline void Set(float *n, float x, float y, float z)
{
n[0] = x;
n[1] = y;
n[2] = z;
}
static inline void Copy(float *dest,const float *source)
static inline void Copy(float *dest, const float *source)
{
dest[0] = source[0];
dest[1] = source[1];
dest[2] = source[2];
}
float computeBoundingSphere(unsigned int vcount,const float *points,float *center)
float computeBoundingSphere(unsigned int vcount, const float *points, float *center)
{
float mRadius;
float mRadius2;
float mRadius;
float mRadius2;
float xmin[3];
float xmax[3];
@ -84,119 +82,116 @@ float computeBoundingSphere(unsigned int vcount,const float *points,float *cente
float dia1[3];
float dia2[3];
/* FIRST PASS: find 6 minima/maxima points */
Set(xmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
Set(xmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
Set(ymin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
Set(ymax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
Set(zmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
Set(zmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
/* FIRST PASS: find 6 minima/maxima points */
Set(xmin, BIGNUMBER, BIGNUMBER, BIGNUMBER);
Set(xmax, -BIGNUMBER, -BIGNUMBER, -BIGNUMBER);
Set(ymin, BIGNUMBER, BIGNUMBER, BIGNUMBER);
Set(ymax, -BIGNUMBER, -BIGNUMBER, -BIGNUMBER);
Set(zmin, BIGNUMBER, BIGNUMBER, BIGNUMBER);
Set(zmax, -BIGNUMBER, -BIGNUMBER, -BIGNUMBER);
for (unsigned i=0; i<vcount; i++)
for (unsigned i = 0; i < vcount; i++)
{
const float *caller_p = &points[i*3];
const float *caller_p = &points[i * 3];
if (caller_p[0]<xmin[0])
Copy(xmin,caller_p); /* New xminimum point */
if (caller_p[0]>xmax[0])
Copy(xmax,caller_p);
if (caller_p[1]<ymin[1])
Copy(ymin,caller_p);
if (caller_p[1]>ymax[1])
Copy(ymax,caller_p);
if (caller_p[2]<zmin[2])
Copy(zmin,caller_p);
if (caller_p[2]>zmax[2])
Copy(zmax,caller_p);
if (caller_p[0] < xmin[0])
Copy(xmin, caller_p); /* New xminimum point */
if (caller_p[0] > xmax[0])
Copy(xmax, caller_p);
if (caller_p[1] < ymin[1])
Copy(ymin, caller_p);
if (caller_p[1] > ymax[1])
Copy(ymax, caller_p);
if (caller_p[2] < zmin[2])
Copy(zmin, caller_p);
if (caller_p[2] > zmax[2])
Copy(zmax, caller_p);
}
/* Set xspan = distance between the 2 points xmin & xmax (squared) */
float dx = xmax[0] - xmin[0];
float dy = xmax[1] - xmin[1];
float dz = xmax[2] - xmin[2];
float xspan = dx*dx + dy*dy + dz*dz;
/* Set xspan = distance between the 2 points xmin & xmax (squared) */
float dx = xmax[0] - xmin[0];
float dy = xmax[1] - xmin[1];
float dz = xmax[2] - xmin[2];
float xspan = dx * dx + dy * dy + dz * dz;
/* Same for y & z spans */
dx = ymax[0] - ymin[0];
dy = ymax[1] - ymin[1];
dz = ymax[2] - ymin[2];
float yspan = dx*dx + dy*dy + dz*dz;
/* Same for y & z spans */
dx = ymax[0] - ymin[0];
dy = ymax[1] - ymin[1];
dz = ymax[2] - ymin[2];
float yspan = dx * dx + dy * dy + dz * dz;
dx = zmax[0] - zmin[0];
dy = zmax[1] - zmin[1];
dz = zmax[2] - zmin[2];
float zspan = dx*dx + dy*dy + dz*dz;
dx = zmax[0] - zmin[0];
dy = zmax[1] - zmin[1];
dz = zmax[2] - zmin[2];
float zspan = dx * dx + dy * dy + dz * dz;
/* Set points dia1 & dia2 to the maximally separated pair */
Copy(dia1,xmin);
Copy(dia2,xmax); /* assume xspan biggest */
float maxspan = xspan;
/* Set points dia1 & dia2 to the maximally separated pair */
Copy(dia1, xmin);
Copy(dia2, xmax); /* assume xspan biggest */
float maxspan = xspan;
if (yspan>maxspan)
if (yspan > maxspan)
{
maxspan = yspan;
Copy(dia1,ymin);
Copy(dia2,ymax);
maxspan = yspan;
Copy(dia1, ymin);
Copy(dia2, ymax);
}
if (zspan>maxspan)
if (zspan > maxspan)
{
Copy(dia1,zmin);
Copy(dia2,zmax);
Copy(dia1, zmin);
Copy(dia2, zmax);
}
/* dia1,dia2 is a diameter of initial sphere */
/* calc initial center */
center[0] = (dia1[0] + dia2[0]) * 0.5f;
center[1] = (dia1[1] + dia2[1]) * 0.5f;
center[2] = (dia1[2] + dia2[2]) * 0.5f;
/* dia1,dia2 is a diameter of initial sphere */
/* calc initial center */
center[0] = (dia1[0]+dia2[0])*0.5f;
center[1] = (dia1[1]+dia2[1])*0.5f;
center[2] = (dia1[2]+dia2[2])*0.5f;
/* calculate initial radius**2 and radius */
/* calculate initial radius**2 and radius */
dx = dia2[0] - center[0]; /* x component of radius vector */
dy = dia2[1] - center[1]; /* y component of radius vector */
dz = dia2[2] - center[2]; /* z component of radius vector */
dx = dia2[0]-center[0]; /* x component of radius vector */
dy = dia2[1]-center[1]; /* y component of radius vector */
dz = dia2[2]-center[2]; /* z component of radius vector */
mRadius2 = dx * dx + dy * dy + dz * dz;
mRadius = float(sqrt(mRadius2));
mRadius2 = dx*dx + dy*dy + dz*dz;
mRadius = float(sqrt(mRadius2));
/* SECOND PASS: increment current sphere */
/* SECOND PASS: increment current sphere */
if ( 1 )
if (1)
{
for (unsigned i=0; i<vcount; i++)
for (unsigned i = 0; i < vcount; i++)
{
const float *caller_p = &points[i*3];
const float *caller_p = &points[i * 3];
dx = caller_p[0]-center[0];
dy = caller_p[1]-center[1];
dz = caller_p[2]-center[2];
dx = caller_p[0] - center[0];
dy = caller_p[1] - center[1];
dz = caller_p[2] - center[2];
float old_to_p_sq = dx*dx + dy*dy + dz*dz;
float old_to_p_sq = dx * dx + dy * dy + dz * dz;
if (old_to_p_sq > mRadius2) /* do r**2 test first */
{ /* this point is outside of current sphere */
float old_to_p = float(sqrt(old_to_p_sq));
if (old_to_p_sq > mRadius2) /* do r**2 test first */
{ /* this point is outside of current sphere */
float old_to_p = float(sqrt(old_to_p_sq));
/* calc radius of new sphere */
mRadius = (mRadius + old_to_p) * 0.5f;
mRadius2 = mRadius*mRadius; /* for next r**2 compare */
float old_to_new = old_to_p - mRadius;
mRadius = (mRadius + old_to_p) * 0.5f;
mRadius2 = mRadius * mRadius; /* for next r**2 compare */
float old_to_new = old_to_p - mRadius;
/* calc center of new sphere */
/* calc center of new sphere */
float recip = 1.0f /old_to_p;
float recip = 1.0f / old_to_p;
float cx = (mRadius*center[0] + old_to_new*caller_p[0]) * recip;
float cy = (mRadius*center[1] + old_to_new*caller_p[1]) * recip;
float cz = (mRadius*center[2] + old_to_new*caller_p[2]) * recip;
float cx = (mRadius * center[0] + old_to_new * caller_p[0]) * recip;
float cy = (mRadius * center[1] + old_to_new * caller_p[1]) * recip;
float cz = (mRadius * center[2] + old_to_new * caller_p[2]) * recip;
Set(center,cx,cy,cz);
Set(center, cx, cy, cz);
}
}
}
return mRadius;
return mRadius;
}

4
Extras/ConvexDecomposition/fitsphere.h
View File

@ -36,8 +36,6 @@
// http://www.amillionpixels.us
//
float computeBoundingSphere(unsigned int vcount,const float *points,float *center);
float computeBoundingSphere(unsigned int vcount, const float *points, float *center);
#endif

219
Extras/ConvexDecomposition/float_math.cpp
View File

@ -6,7 +6,6 @@
#include <assert.h>
#include <math.h>
/*----------------------------------------------------------------------
Copyright (c) 2004 Open Dynamics Framework Group
www.physicstools.org
@ -41,58 +40,53 @@
// http://www.amillionpixels.us
//
void fm_inverseRT(const float *matrix,const float *pos,float *t) // inverse rotate translate the point.
void fm_inverseRT(const float *matrix, const float *pos, float *t) // inverse rotate translate the point.
{
float _x = pos[0] - matrix[3*4+0];
float _y = pos[1] - matrix[3*4+1];
float _z = pos[2] - matrix[3*4+2];
float _x = pos[0] - matrix[3 * 4 + 0];
float _y = pos[1] - matrix[3 * 4 + 1];
float _z = pos[2] - matrix[3 * 4 + 2];
// Multiply inverse-translated source vector by inverted rotation transform
t[0] = (matrix[0*4+0] * _x) + (matrix[0*4+1] * _y) + (matrix[0*4+2] * _z);
t[1] = (matrix[1*4+0] * _x) + (matrix[1*4+1] * _y) + (matrix[1*4+2] * _z);
t[2] = (matrix[2*4+0] * _x) + (matrix[2*4+1] * _y) + (matrix[2*4+2] * _z);
t[0] = (matrix[0 * 4 + 0] * _x) + (matrix[0 * 4 + 1] * _y) + (matrix[0 * 4 + 2] * _z);
t[1] = (matrix[1 * 4 + 0] * _x) + (matrix[1 * 4 + 1] * _y) + (matrix[1 * 4 + 2] * _z);
t[2] = (matrix[2 * 4 + 0] * _x) + (matrix[2 * 4 + 1] * _y) + (matrix[2 * 4 + 2] * _z);
}
void fm_identity(float *matrix) // set 4x4 matrix to identity.
void fm_identity(float *matrix) // set 4x4 matrix to identity.
{
matrix[0*4+0] = 1;
matrix[1*4+1] = 1;
matrix[2*4+2] = 1;
matrix[3*4+3] = 1;
matrix[0 * 4 + 0] = 1;
matrix[1 * 4 + 1] = 1;
matrix[2 * 4 + 2] = 1;
matrix[3 * 4 + 3] = 1;
matrix[1*4+0] = 0;
matrix[2*4+0] = 0;
matrix[3*4+0] = 0;
matrix[1 * 4 + 0] = 0;
matrix[2 * 4 + 0] = 0;
matrix[3 * 4 + 0] = 0;
matrix[0*4+1] = 0;
matrix[2*4+1] = 0;
matrix[3*4+1] = 0;
matrix[0 * 4 + 1] = 0;
matrix[2 * 4 + 1] = 0;
matrix[3 * 4 + 1] = 0;
matrix[0*4+2] = 0;
matrix[1*4+2] = 0;
matrix[3*4+2] = 0;
matrix[0*4+3] = 0;
matrix[1*4+3] = 0;
matrix[2*4+3] = 0;
matrix[0 * 4 + 2] = 0;
matrix[1 * 4 + 2] = 0;
matrix[3 * 4 + 2] = 0;
matrix[0 * 4 + 3] = 0;
matrix[1 * 4 + 3] = 0;
matrix[2 * 4 + 3] = 0;
}
void fm_eulerMatrix(float ax,float ay,float az,float *matrix) // convert euler (in radians) to a dest 4x4 matrix (translation set to zero)
void fm_eulerMatrix(float ax, float ay, float az, float *matrix) // convert euler (in radians) to a dest 4x4 matrix (translation set to zero)
{
float quat[4];
fm_eulerToQuat(ax,ay,az,quat);
fm_quatToMatrix(quat,matrix);
float quat[4];
fm_eulerToQuat(ax, ay, az, quat);
fm_quatToMatrix(quat, matrix);
}
void fm_getAABB(unsigned int vcount,const float *points,unsigned int pstride,float *bmin,float *bmax)
void fm_getAABB(unsigned int vcount, const float *points, unsigned int pstride, float *bmin, float *bmax)
{
const unsigned char *source = (const unsigned char *) points;
const unsigned char *source = (const unsigned char *)points;
bmin[0] = points[0];
bmin[1] = points[1];
@ -102,29 +96,26 @@ void fm_getAABB(unsigned int vcount,const float *points,unsigned int pstride,flo
bmax[1] = points[1];
bmax[2] = points[2];
for (unsigned int i = 1; i < vcount; i++)
{
source += pstride;
const float *p = (const float *)source;
for (unsigned int i=1; i<vcount; i++)
{
source+=pstride;
const float *p = (const float *) source;
if (p[0] < bmin[0]) bmin[0] = p[0];
if (p[1] < bmin[1]) bmin[1] = p[1];
if (p[2] < bmin[2]) bmin[2] = p[2];
if ( p[0] < bmin[0] ) bmin[0] = p[0];
if ( p[1] < bmin[1] ) bmin[1] = p[1];
if ( p[2] < bmin[2] ) bmin[2] = p[2];
if ( p[0] > bmax[0] ) bmax[0] = p[0];
if ( p[1] > bmax[1] ) bmax[1] = p[1];
if ( p[2] > bmax[2] ) bmax[2] = p[2];
}
if (p[0] > bmax[0]) bmax[0] = p[0];
if (p[1] > bmax[1]) bmax[1] = p[1];
if (p[2] > bmax[2]) bmax[2] = p[2];
}
}
void fm_eulerToQuat(float roll,float pitch,float yaw,float *quat) // convert euler angles to quaternion.
void fm_eulerToQuat(float roll, float pitch, float yaw, float *quat) // convert euler angles to quaternion.
{
roll *= 0.5f;
roll *= 0.5f;
pitch *= 0.5f;
yaw *= 0.5f;
yaw *= 0.5f;
float cr = cosf(roll);
float cp = cosf(pitch);
@ -139,119 +130,109 @@ void fm_eulerToQuat(float roll,float pitch,float yaw,float *quat) // convert eul
float spcy = sp * cy;
float cpsy = cp * sy;
quat[0] = ( sr * cpcy - cr * spsy);
quat[1] = ( cr * spcy + sr * cpsy);
quat[2] = ( cr * cpsy - sr * spcy);
quat[3] = cr * cpcy + sr * spsy;
quat[0] = (sr * cpcy - cr * spsy);
quat[1] = (cr * spcy + sr * cpsy);
quat[2] = (cr * cpsy - sr * spcy);
quat[3] = cr * cpcy + sr * spsy;
}
void fm_quatToMatrix(const float *quat,float *matrix) // convert quaterinion rotation to matrix, zeros out the translation component.
void fm_quatToMatrix(const float *quat, float *matrix) // convert quaterinion rotation to matrix, zeros out the translation component.
{
float xx = quat[0] * quat[0];
float yy = quat[1] * quat[1];
float zz = quat[2] * quat[2];
float xy = quat[0] * quat[1];
float xz = quat[0] * quat[2];
float yz = quat[1] * quat[2];
float wx = quat[3] * quat[0];
float wy = quat[3] * quat[1];
float wz = quat[3] * quat[2];
float xx = quat[0]*quat[0];
float yy = quat[1]*quat[1];
float zz = quat[2]*quat[2];
float xy = quat[0]*quat[1];
float xz = quat[0]*quat[2];
float yz = quat[1]*quat[2];
float wx = quat[3]*quat[0];
float wy = quat[3]*quat[1];
float wz = quat[3]*quat[2];
matrix[0 * 4 + 0] = 1 - 2 * (yy + zz);
matrix[1 * 4 + 0] = 2 * (xy - wz);
matrix[2 * 4 + 0] = 2 * (xz + wy);
matrix[0*4+0] = 1 - 2 * ( yy + zz );
matrix[1*4+0] = 2 * ( xy - wz );
matrix[2*4+0] = 2 * ( xz + wy );
matrix[0 * 4 + 1] = 2 * (xy + wz);
matrix[1 * 4 + 1] = 1 - 2 * (xx + zz);
matrix[2 * 4 + 1] = 2 * (yz - wx);
matrix[0*4+1] = 2 * ( xy + wz );
matrix[1*4+1] = 1 - 2 * ( xx + zz );
matrix[2*4+1] = 2 * ( yz - wx );
matrix[0*4+2] = 2 * ( xz - wy );
matrix[1*4+2] = 2 * ( yz + wx );
matrix[2*4+2] = 1 - 2 * ( xx + yy );
matrix[3*4+0] = matrix[3*4+1] = matrix[3*4+2] = 0.0f;
matrix[0*4+3] = matrix[1*4+3] = matrix[2*4+3] = 0.0f;
matrix[3*4+3] = 1.0f;
matrix[0 * 4 + 2] = 2 * (xz - wy);
matrix[1 * 4 + 2] = 2 * (yz + wx);
matrix[2 * 4 + 2] = 1 - 2 * (xx + yy);
matrix[3 * 4 + 0] = matrix[3 * 4 + 1] = matrix[3 * 4 + 2] = 0.0f;
matrix[0 * 4 + 3] = matrix[1 * 4 + 3] = matrix[2 * 4 + 3] = 0.0f;
matrix[3 * 4 + 3] = 1.0f;
}
void fm_quatRotate(const float *quat,const float *v,float *r) // rotate a vector directly by a quaternion.
void fm_quatRotate(const float *quat, const float *v, float *r) // rotate a vector directly by a quaternion.
{
float left[4];
float left[4];
left[0] = quat[3]*v[0] + quat[1]*v[2] - v[1]*quat[2];
left[1] = quat[3]*v[1] + quat[2]*v[0] - v[2]*quat[0];
left[2] = quat[3]*v[2] + quat[0]*v[1] - v[0]*quat[1];
left[3] = - quat[0]*v[0] - quat[1]*v[1] - quat[2]*v[2];
r[0] = (left[3]*-quat[0]) + (quat[3]*left[0]) + (left[1]*-quat[2]) - (-quat[1]*left[2]);
r[1] = (left[3]*-quat[1]) + (quat[3]*left[1]) + (left[2]*-quat[0]) - (-quat[2]*left[0]);
r[2] = (left[3]*-quat[2]) + (quat[3]*left[2]) + (left[0]*-quat[1]) - (-quat[0]*left[1]);
left[0] = quat[3] * v[0] + quat[1] * v[2] - v[1] * quat[2];
left[1] = quat[3] * v[1] + quat[2] * v[0] - v[2] * quat[0];
left[2] = quat[3] * v[2] + quat[0] * v[1] - v[0] * quat[1];
left[3] = -quat[0] * v[0] - quat[1] * v[1] - quat[2] * v[2];
r[0] = (left[3] * -quat[0]) + (quat[3] * left[0]) + (left[1] * -quat[2]) - (-quat[1] * left[2]);
r[1] = (left[3] * -quat[1]) + (quat[3] * left[1]) + (left[2] * -quat[0]) - (-quat[2] * left[0]);
r[2] = (left[3] * -quat[2]) + (quat[3] * left[2]) + (left[0] * -quat[1]) - (-quat[0] * left[1]);
}
void fm_getTranslation(const float *matrix,float *t)
void fm_getTranslation(const float *matrix, float *t)
{
t[0] = matrix[3*4+0];
t[1] = matrix[3*4+1];
t[2] = matrix[3*4+2];
t[0] = matrix[3 * 4 + 0];
t[1] = matrix[3 * 4 + 1];
t[2] = matrix[3 * 4 + 2];
}
void fm_matrixToQuat(const float *matrix,float *quat) // convert the 3x3 portion of a 4x4 matrix into a quaterion as x,y,z,w
void fm_matrixToQuat(const float *matrix, float *quat) // convert the 3x3 portion of a 4x4 matrix into a quaterion as x,y,z,w
{
float tr = matrix[0*4+0] + matrix[1*4+1] + matrix[2*4+2];
float tr = matrix[0 * 4 + 0] + matrix[1 * 4 + 1] + matrix[2 * 4 + 2];
// check the diagonal
if (tr > 0.0f )
if (tr > 0.0f)
{
float s = (float) sqrt ( (double) (tr + 1.0f) );
float s = (float)sqrt((double)(tr + 1.0f));
quat[3] = s * 0.5f;
s = 0.5f / s;
quat[0] = (matrix[1*4+2] - matrix[2*4+1]) * s;
quat[1] = (matrix[2*4+0] - matrix[0*4+2]) * s;
quat[2] = (matrix[0*4+1] - matrix[1*4+0]) * s;
quat[0] = (matrix[1 * 4 + 2] - matrix[2 * 4 + 1]) * s;
quat[1] = (matrix[2 * 4 + 0] - matrix[0 * 4 + 2]) * s;
quat[2] = (matrix[0 * 4 + 1] - matrix[1 * 4 + 0]) * s;
}
else
{
// diagonal is negative
int nxt[3] = {1, 2, 0};
float qa[4];
float qa[4];
int i = 0;
if (matrix[1*4+1] > matrix[0*4+0]) i = 1;
if (matrix[2*4+2] > matrix[i*4+i]) i = 2;
if (matrix[1 * 4 + 1] > matrix[0 * 4 + 0]) i = 1;
if (matrix[2 * 4 + 2] > matrix[i * 4 + i]) i = 2;
int j = nxt[i];
int k = nxt[j];
float s = sqrtf ( ((matrix[i*4+i] - (matrix[j*4+j] + matrix[k*4+k])) + 1.0f) );
float s = sqrtf(((matrix[i * 4 + i] - (matrix[j * 4 + j] + matrix[k * 4 + k])) + 1.0f));
qa[i] = s * 0.5f;
if (s != 0.0f ) s = 0.5f / s;
if (s != 0.0f) s = 0.5f / s;
qa[3] = (matrix[j*4+k] - matrix[k*4+j]) * s;
qa[j] = (matrix[i*4+j] + matrix[j*4+i]) * s;
qa[k] = (matrix[i*4+k] + matrix[k*4+i]) * s;
qa[3] = (matrix[j * 4 + k] - matrix[k * 4 + j]) * s;
qa[j] = (matrix[i * 4 + j] + matrix[j * 4 + i]) * s;
qa[k] = (matrix[i * 4 + k] + matrix[k * 4 + i]) * s;
quat[0] = qa[0];
quat[1] = qa[1];
quat[2] = qa[2];
quat[3] = qa[3];
}
}
float fm_sphereVolume(float radius) // return's the volume of a sphere of this radius (4/3 PI * R cubed )
float fm_sphereVolume(float radius) // return's the volume of a sphere of this radius (4/3 PI * R cubed )
{
return (4.0f / 3.0f ) * FM_PI * radius * radius * radius;
return (4.0f / 3.0f) * FM_PI * radius * radius * radius;
}

33
Extras/ConvexDecomposition/float_math.h
View File

@ -3,12 +3,12 @@
#define FLOAT_MATH_H
#ifdef _WIN32
#pragma warning(disable : 4324) // disable padding warning
#pragma warning(disable : 4244) // disable padding warning
#pragma warning(disable : 4267) // possible loss of data
#pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
#pragma warning(disable:4996) //Turn off warnings about deprecated C routines
#pragma warning(disable:4786) // Disable the "debug name too long" warning
#pragma warning(disable : 4324) // disable padding warning
#pragma warning(disable : 4244) // disable padding warning
#pragma warning(disable : 4267) // possible loss of data
#pragma warning(disable : 4530) // Disable the exception disable but used in MSCV Stl warning.
#pragma warning(disable : 4996) //Turn off warnings about deprecated C routines
#pragma warning(disable : 4786) // Disable the "debug name too long" warning
#endif
/*----------------------------------------------------------------------
@ -45,7 +45,6 @@
// http://www.amillionpixels.us
//
// a set of routines that last you do common 3d math
// operations without any vector, matrix, or quaternion
// classes or templates.
@ -58,15 +57,15 @@ const float FM_PI = 3.141592654f;
const float FM_DEG_TO_RAD = ((2.0f * FM_PI) / 360.0f);
const float FM_RAD_TO_DEG = (360.0f / (2.0f * FM_PI));
void fm_identity(float *matrix); // set 4x4 matrix to identity.
void fm_inverseRT(const float *matrix,const float *pos,float *t); // inverse rotate translate the point.
void fm_eulerMatrix(float ax,float ay,float az,float *matrix); // convert euler (in radians) to a dest 4x4 matrix (translation set to zero)
void fm_getAABB(unsigned int vcount,const float *points,unsigned int pstride,float *bmin,float *bmax);
void fm_eulerToQuat(float roll,float pitch,float yaw,float *quat); // convert euler angles to quaternion.
void fm_quatToMatrix(const float *quat,float *matrix); // convert quaterinion rotation to matrix, translation set to zero.
void fm_quatRotate(const float *quat,const float *v,float *r); // rotate a vector directly by a quaternion.
void fm_getTranslation(const float *matrix,float *t);
void fm_matrixToQuat(const float *matrix,float *quat); // convert the 3x3 portion of a 4x4 matrix into a quaterion as x,y,z,w
float fm_sphereVolume(float radius); // return's the volume of a sphere of this radius (4/3 PI * R cubed )
void fm_identity(float *matrix); // set 4x4 matrix to identity.
void fm_inverseRT(const float *matrix, const float *pos, float *t); // inverse rotate translate the point.
void fm_eulerMatrix(float ax, float ay, float az, float *matrix); // convert euler (in radians) to a dest 4x4 matrix (translation set to zero)
void fm_getAABB(unsigned int vcount, const float *points, unsigned int pstride, float *bmin, float *bmax);
void fm_eulerToQuat(float roll, float pitch, float yaw, float *quat); // convert euler angles to quaternion.
void fm_quatToMatrix(const float *quat, float *matrix); // convert quaterinion rotation to matrix, translation set to zero.
void fm_quatRotate(const float *quat, const float *v, float *r); // rotate a vector directly by a quaternion.
void fm_getTranslation(const float *matrix, float *t);
void fm_matrixToQuat(const float *matrix, float *quat); // convert the 3x3 portion of a 4x4 matrix into a quaterion as x,y,z,w
float fm_sphereVolume(float radius); // return's the volume of a sphere of this radius (4/3 PI * R cubed )
#endif

80
Extras/ConvexDecomposition/meshvolume.cpp
View File

@ -35,94 +35,90 @@
// http://www.amillionpixels.us
//
inline float det(const float *p1,const float *p2,const float *p3)
inline float det(const float *p1, const float *p2, const float *p3)
{
return p1[0]*p2[1]*p3[2] + p2[0]*p3[1]*p1[2] + p3[0]*p1[1]*p2[2] -p1[0]*p3[1]*p2[2] - p2[0]*p1[1]*p3[2] - p3[0]*p2[1]*p1[2];
return p1[0] * p2[1] * p3[2] + p2[0] * p3[1] * p1[2] + p3[0] * p1[1] * p2[2] - p1[0] * p3[1] * p2[2] - p2[0] * p1[1] * p3[2] - p3[0] * p2[1] * p1[2];
}
float computeMeshVolume(const float *vertices,unsigned int tcount,const unsigned int *indices)
float computeMeshVolume(const float *vertices, unsigned int tcount, const unsigned int *indices)
{
float volume = 0;
for (unsigned int i=0; i<tcount; i++,indices+=3)
for (unsigned int i = 0; i < tcount; i++, indices += 3)
{
const float *p1 = &vertices[indices[0] * 3];
const float *p2 = &vertices[indices[1] * 3];
const float *p3 = &vertices[indices[2] * 3];
const float *p1 = &vertices[ indices[0]*3 ];
const float *p2 = &vertices[ indices[1]*3 ];
const float *p3 = &vertices[ indices[2]*3 ];
volume+=det(p1,p2,p3); // compute the volume of the tetrahedran relative to the origin.
volume += det(p1, p2, p3); // compute the volume of the tetrahedran relative to the origin.
}
volume*=(1.0f/6.0f);
if ( volume < 0 )
volume*=-1;
volume *= (1.0f / 6.0f);
if (volume < 0)
volume *= -1;
return volume;
}
inline void CrossProduct(const float *a,const float *b,float *cross)
inline void CrossProduct(const float *a, const float *b, float *cross)
{
cross[0] = a[1]*b[2] - a[2]*b[1];
cross[1] = a[2]*b[0] - a[0]*b[2];
cross[2] = a[0]*b[1] - a[1]*b[0];
cross[0] = a[1] * b[2] - a[2] * b[1];
cross[1] = a[2] * b[0] - a[0] * b[2];
cross[2] = a[0] * b[1] - a[1] * b[0];
}
inline float DotProduct(const float *a,const float *b)
inline float DotProduct(const float *a, const float *b)
{
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}
inline float tetVolume(const float *p0,const float *p1,const float *p2,const float *p3)
inline float tetVolume(const float *p0, const float *p1, const float *p2, const float *p3)
{
float a[3];
float b[3];
float c[3];
a[0] = p1[0] - p0[0];
a[1] = p1[1] - p0[1];
a[2] = p1[2] - p0[2];
a[0] = p1[0] - p0[0];
a[1] = p1[1] - p0[1];
a[2] = p1[2] - p0[2];
b[0] = p2[0] - p0[0];
b[1] = p2[1] - p0[1];
b[2] = p2[2] - p0[2];
c[0] = p3[0] - p0[0];
c[1] = p3[1] - p0[1];
c[2] = p3[2] - p0[2];
c[0] = p3[0] - p0[0];
c[1] = p3[1] - p0[1];
c[2] = p3[2] - p0[2];
float cross[3];
float cross[3];
CrossProduct( b, c, cross );
CrossProduct(b, c, cross);
float volume = DotProduct( a, cross );
float volume = DotProduct(a, cross);
if ( volume < 0 )
return -volume;
if (volume < 0)
return -volume;
return volume;
return volume;
}
inline float det(const float *p0,const float *p1,const float *p2,const float *p3)
inline float det(const float *p0, const float *p1, const float *p2, const float *p3)
{
return p1[0]*p2[1]*p3[2] + p2[0]*p3[1]*p1[2] + p3[0]*p1[1]*p2[2] -p1[0]*p3[1]*p2[2] - p2[0]*p1[1]*p3[2] - p3[0]*p2[1]*p1[2];
return p1[0] * p2[1] * p3[2] + p2[0] * p3[1] * p1[2] + p3[0] * p1[1] * p2[2] - p1[0] * p3[1] * p2[2] - p2[0] * p1[1] * p3[2] - p3[0] * p2[1] * p1[2];
}
float computeMeshVolume2(const float *vertices,unsigned int tcount,const unsigned int *indices)
float computeMeshVolume2(const float *vertices, unsigned int tcount, const unsigned int *indices)
{
float volume = 0;
const float *p0 = vertices;
for (unsigned int i=0; i<tcount; i++,indices+=3)
for (unsigned int i = 0; i < tcount; i++, indices += 3)
{
const float *p1 = &vertices[indices[0] * 3];
const float *p2 = &vertices[indices[1] * 3];
const float *p3 = &vertices[indices[2] * 3];
const float *p1 = &vertices[ indices[0]*3 ];
const float *p2 = &vertices[ indices[1]*3 ];
const float *p3 = &vertices[ indices[2]*3 ];
volume+=tetVolume(p0,p1,p2,p3); // compute the volume of the tetrahdren relative to the root vertice
volume += tetVolume(p0, p1, p2, p3); // compute the volume of the tetrahdren relative to the root vertice
}
return volume * (1.0f / 6.0f );
return volume * (1.0f / 6.0f);
}

7
Extras/ConvexDecomposition/meshvolume.h
View File

@ -36,10 +36,7 @@
// http://www.amillionpixels.us
//
float computeMeshVolume(const float *vertices,unsigned int tcount,const unsigned int *indices);
float computeMeshVolume2(const float *vertices,unsigned int tcount,const unsigned int *indices);
float computeMeshVolume(const float *vertices, unsigned int tcount, const unsigned int *indices);
float computeMeshVolume2(const float *vertices, unsigned int tcount, const unsigned int *indices);
#endif

295
Extras/ConvexDecomposition/planetri.cpp
View File

@ -40,199 +40,188 @@
// http://www.amillionpixels.us
//
static inline float DistToPt(const float *p,const float *plane)
static inline float DistToPt(const float *p, const float *plane)
{
float x = p[0];
float y = p[1];
float z = p[2];
float d = x*plane[0] + y*plane[1] + z*plane[2] + plane[3];
float d = x * plane[0] + y * plane[1] + z * plane[2] + plane[3];
return d;
}
static PlaneTriResult getSidePlane(const float *p,const float *plane,float epsilon)
static PlaneTriResult getSidePlane(const float *p, const float *plane, float epsilon)
{
float d = DistToPt(p, plane);
float d = DistToPt(p,plane);
if ((d + epsilon) > 0)
return PTR_FRONT; // it is 'in front' within the provided epsilon value.
if ( (d+epsilon) > 0 )
return PTR_FRONT; // it is 'in front' within the provided epsilon value.
return PTR_BACK;
return PTR_BACK;
}
static void add(const float *p,float *dest,unsigned int tstride,unsigned int &pcount)
static void add(const float *p, float *dest, unsigned int tstride, unsigned int &pcount)
{
char *d = (char *) dest;
d = d + pcount*tstride;
dest = (float *) d;
dest[0] = p[0];
dest[1] = p[1];
dest[2] = p[2];
pcount++;
assert( pcount <= 4 );
char *d = (char *)dest;
d = d + pcount * tstride;
dest = (float *)d;
dest[0] = p[0];
dest[1] = p[1];
dest[2] = p[2];
pcount++;
assert(pcount <= 4);
}
// assumes that the points are on opposite sides of the plane!
static void intersect(const float *p1,const float *p2,float *split,const float *plane)
static void intersect(const float *p1, const float *p2, float *split, const float *plane)
{
float dp1 = DistToPt(p1, plane);
float dp1 = DistToPt(p1,plane);
float dir[3];
float dir[3];
dir[0] = p2[0] - p1[0];
dir[1] = p2[1] - p1[1];
dir[2] = p2[2] - p1[2];
dir[0] = p2[0] - p1[0];
dir[1] = p2[1] - p1[1];
dir[2] = p2[2] - p1[2];
float dot1 = dir[0] * plane[0] + dir[1] * plane[1] + dir[2] * plane[2];
float dot2 = dp1 - plane[3];
float dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2];
float dot2 = dp1 - plane[3];
float t = -(plane[3] + dot2 ) / dot1;
split[0] = (dir[0]*t)+p1[0];
split[1] = (dir[1]*t)+p1[1];
split[2] = (dir[2]*t)+p1[2];
float t = -(plane[3] + dot2) / dot1;
split[0] = (dir[0] * t) + p1[0];
split[1] = (dir[1] * t) + p1[1];
split[2] = (dir[2] * t) + p1[2];
}
PlaneTriResult planeTriIntersection(const float *plane, // the plane equation in Ax+By+Cz+D format
const float *triangle, // the source triangle.
unsigned int tstride, // stride in bytes of the input and output triangles
float epsilon, // the co-planer epsilon value.
float *front, // the triangle in front of the
unsigned int &fcount, // number of vertices in the 'front' triangle
float *back, // the triangle in back of the plane
unsigned int &bcount) // the number of vertices in the 'back' triangle.
PlaneTriResult planeTriIntersection(const float *plane, // the plane equation in Ax+By+Cz+D format
const float *triangle, // the source triangle.
unsigned int tstride, // stride in bytes of the input and output triangles
float epsilon, // the co-planer epsilon value.
float *front, // the triangle in front of the
unsigned int &fcount, // number of vertices in the 'front' triangle
float *back, // the triangle in back of the plane
unsigned int &bcount) // the number of vertices in the 'back' triangle.
{
fcount = 0;
bcount = 0;
fcount = 0;
bcount = 0;
const char *tsource = (const char *) triangle;
const char *tsource = (const char *)triangle;
// get the three vertices of the triangle.
const float *p1 = (const float *) (tsource);
const float *p2 = (const float *) (tsource+tstride);
const float *p3 = (const float *) (tsource+tstride*2);
// get the three vertices of the triangle.
const float *p1 = (const float *)(tsource);
const float *p2 = (const float *)(tsource + tstride);
const float *p3 = (const float *)(tsource + tstride * 2);
PlaneTriResult r1 = getSidePlane(p1, plane, epsilon); // compute the side of the plane each vertex is on
PlaneTriResult r2 = getSidePlane(p2, plane, epsilon);
PlaneTriResult r3 = getSidePlane(p3, plane, epsilon);
PlaneTriResult r1 = getSidePlane(p1,plane,epsilon); // compute the side of the plane each vertex is on
PlaneTriResult r2 = getSidePlane(p2,plane,epsilon);
PlaneTriResult r3 = getSidePlane(p3,plane,epsilon);
if (r1 == r2 && r1 == r3) // if all three vertices are on the same side of the plane.
{
if (r1 == PTR_FRONT) // if all three are in front of the plane, then copy to the 'front' output triangle.
{
add(p1, front, tstride, fcount);
add(p2, front, tstride, fcount);
add(p3, front, tstride, fcount);
}
else
{
add(p1, back, tstride, bcount); // if all three are in 'abck' then copy to the 'back' output triangle.
add(p2, back, tstride, bcount);
add(p3, back, tstride, bcount);
}
return r1; // if all three points are on the same side of the plane return result
}
if ( r1 == r2 && r1 == r3 ) // if all three vertices are on the same side of the plane.
{
if ( r1 == PTR_FRONT ) // if all three are in front of the plane, then copy to the 'front' output triangle.
{
add(p1,front,tstride,fcount);
add(p2,front,tstride,fcount);
add(p3,front,tstride,fcount);
}
else
{
add(p1,back,tstride,bcount); // if all three are in 'abck' then copy to the 'back' output triangle.
add(p2,back,tstride,bcount);
add(p3,back,tstride,bcount);
}
return r1; // if all three points are on the same side of the plane return result
}
// ok.. we need to split the triangle at the plane.
// ok.. we need to split the triangle at the plane.
// First test ray segment P1 to P2
if (r1 == r2) // if these are both on the same side...
{
if (r1 == PTR_FRONT)
{
add(p1, front, tstride, fcount);
add(p2, front, tstride, fcount);
}
else
{
add(p1, back, tstride, bcount);
add(p2, back, tstride, bcount);
}
}
else
{
float split[3]; // split the point
intersect(p1, p2, split, plane);
// First test ray segment P1 to P2
if ( r1 == r2 ) // if these are both on the same side...
{
if ( r1 == PTR_FRONT )
{
add( p1, front, tstride, fcount );
add( p2, front, tstride, fcount );
}
else
{
add( p1, back, tstride, bcount );
add( p2, back, tstride, bcount );
}
}
else
{
float split[3]; // split the point
intersect(p1,p2,split,plane);
if (r1 == PTR_FRONT)
{
add(p1, front, tstride, fcount);
add(split, front, tstride, fcount);
if ( r1 == PTR_FRONT )
{
add(split, back, tstride, bcount);
add(p2, back, tstride, bcount);
}
else
{
add(p1, back, tstride, bcount);
add(split, back, tstride, bcount);
add(p1, front, tstride, fcount );
add(split, front, tstride, fcount );
add(split, front, tstride, fcount);
add(p2, front, tstride, fcount);
}
}
add(split, back, tstride, bcount );
add(p2, back, tstride, bcount );
// Next test ray segment P2 to P3
if (r2 == r3) // if these are both on the same side...
{
if (r3 == PTR_FRONT)
{
add(p3, front, tstride, fcount);
}
else
{
add(p3, back, tstride, bcount);
}
}
else
{
float split[3]; // split the point
intersect(p2, p3, split, plane);
}
else
{
add(p1, back, tstride, bcount );
add(split, back, tstride, bcount );
if (r3 == PTR_FRONT)
{
add(split, front, tstride, fcount);
add(split, back, tstride, bcount);
add(split, front, tstride, fcount );
add(p2, front, tstride, fcount );
}
add(p3, front, tstride, fcount);
}
else
{
add(split, front, tstride, fcount);
add(split, back, tstride, bcount);
}
add(p3, back, tstride, bcount);
}
}
// Next test ray segment P2 to P3
if ( r2 == r3 ) // if these are both on the same side...
{
if ( r3 == PTR_FRONT )
{
add( p3, front, tstride, fcount );
}
else
{
add( p3, back, tstride, bcount );
}
}
else
{
float split[3]; // split the point
intersect(p2,p3,split,plane);
// Next test ray segment P3 to P1
if (r3 != r1) // if these are both on the same side...
{
float split[3]; // split the point
if ( r3 == PTR_FRONT )
{
add(split, front, tstride, fcount );
add(split, back, tstride, bcount );
intersect(p3, p1, split, plane);
add(p3, front, tstride, fcount );
}
else
{
add(split, front, tstride, fcount );
add(split, back, tstride, bcount );
if (r1 == PTR_FRONT)
{
add(split, front, tstride, fcount);
add(split, back, tstride, bcount);
}
else
{
add(split, front, tstride, fcount);
add(split, back, tstride, bcount);
}
}
add(p3, back, tstride, bcount );
}
}
// Next test ray segment P3 to P1
if ( r3 != r1 ) // if these are both on the same side...
{
float split[3]; // split the point
intersect(p3,p1,split,plane);
if ( r1 == PTR_FRONT )
{
add(split, front, tstride, fcount );
add(split, back, tstride, bcount );
}
else
{
add(split, front, tstride, fcount );
add(split, back, tstride, bcount );
}
}
return PTR_SPLIT;
return PTR_SPLIT;
}

25
Extras/ConvexDecomposition/planetri.h
View File

@ -36,23 +36,20 @@
// http://www.amillionpixels.us
//
enum PlaneTriResult
{
PTR_FRONT,
PTR_BACK,
PTR_SPLIT
PTR_FRONT,
PTR_BACK,
PTR_SPLIT
};
PlaneTriResult planeTriIntersection(const float *plane, // the plane equation in Ax+By+Cz+D format
const float *triangle, // the source position triangle.
unsigned int tstride, // stride in bytes between vertices of the triangle.
float epsilon, // the co-planer epsilon value.
float *front, // the triangle in front of the
unsigned int &fcount, // number of vertices in the 'front' triangle.
float *back, // the triangle in back of the plane
unsigned int &bcount); // the number of vertices in the 'back' triangle.
PlaneTriResult planeTriIntersection(const float *plane, // the plane equation in Ax+By+Cz+D format
const float *triangle, // the source position triangle.
unsigned int tstride, // stride in bytes between vertices of the triangle.
float epsilon, // the co-planer epsilon value.
float *front, // the triangle in front of the
unsigned int &fcount, // number of vertices in the 'front' triangle.
float *back, // the triangle in back of the plane
unsigned int &bcount); // the number of vertices in the 'back' triangle.
#endif

98
Extras/ConvexDecomposition/raytri.cpp
View File

@ -41,88 +41,82 @@
// http://www.amillionpixels.us
//
/* a = b - c */
#define vector(a,b,c) \
(a)[0] = (b)[0] - (c)[0]; \
(a)[1] = (b)[1] - (c)[1]; \
#define vector(a, b, c) \
(a)[0] = (b)[0] - (c)[0]; \
(a)[1] = (b)[1] - (c)[1]; \
(a)[2] = (b)[2] - (c)[2];
#define innerProduct(v, q) \
((v)[0] * (q)[0] + \
(v)[1] * (q)[1] + \
(v)[2] * (q)[2])
#define innerProduct(v,q) \
((v)[0] * (q)[0] + \
(v)[1] * (q)[1] + \
(v)[2] * (q)[2])
#define crossProduct(a,b,c) \
#define crossProduct(a, b, c) \
(a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \
(a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \
(a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1];
bool rayIntersectsTriangle(const float *p,const float *d,const float *v0,const float *v1,const float *v2,float &t)
bool rayIntersectsTriangle(const float *p, const float *d, const float *v0, const float *v1, const float *v2, float &t)
{
float e1[3], e2[3], h[3], s[3], q[3];
float a, f, u, v;
float e1[3],e2[3],h[3],s[3],q[3];
float a,f,u,v;
vector(e1,v1,v0);
vector(e2,v2,v0);
crossProduct(h,d,e2);
a = innerProduct(e1,h);
vector(e1, v1, v0);
vector(e2, v2, v0);
crossProduct(h, d, e2);
a = innerProduct(e1, h);
if (a > -0.00001 && a < 0.00001)
return(false);
return (false);
f = 1/a;
vector(s,p,v0);
u = f * (innerProduct(s,h));
f = 1 / a;
vector(s, p, v0);
u = f * (innerProduct(s, h));
if (u < 0.0 || u > 1.0)
return(false);
return (false);
crossProduct(q,s,e1);
v = f * innerProduct(d,q);
crossProduct(q, s, e1);
v = f * innerProduct(d, q);
if (v < 0.0 || u + v > 1.0)
return(false);
return (false);
// at this stage we can compute t to find out where
// the intersection point is on the line
t = f * innerProduct(e2,q);
if (t > 0) // ray intersection
return(true);
else // this means that there is a line intersection
// but not a ray intersection
return (false);
t = f * innerProduct(e2, q);
if (t > 0) // ray intersection
return (true);
else // this means that there is a line intersection
// but not a ray intersection
return (false);
}
bool lineIntersectsTriangle(const float *rayStart,const float *rayEnd,const float *p1,const float *p2,const float *p3,float *sect)
bool lineIntersectsTriangle(const float *rayStart, const float *rayEnd, const float *p1, const float *p2, const float *p3, float *sect)
{
float dir[3];
dir[0] = rayEnd[0] - rayStart[0];
dir[1] = rayEnd[1] - rayStart[1];
dir[2] = rayEnd[2] - rayStart[2];
dir[0] = rayEnd[0] - rayStart[0];
dir[1] = rayEnd[1] - rayStart[1];
dir[2] = rayEnd[2] - rayStart[2];
float d = sqrtf(dir[0]*dir[0] + dir[1]*dir[1] + dir[2]*dir[2]);
float r = 1.0f / d;
float d = sqrtf(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]);
float r = 1.0f / d;
dir[0]*=r;
dir[1]*=r;
dir[2]*=r;
dir[0] *= r;
dir[1] *= r;
dir[2] *= r;
float t;
float t;
bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t);
bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t );
if ( ret )
if (ret)
{
if ( t > d )
if (t > d)
{
sect[0] = rayStart[0] + dir[0]*t;
sect[1] = rayStart[1] + dir[1]*t;
sect[2] = rayStart[2] + dir[2]*t;
sect[0] = rayStart[0] + dir[0] * t;
sect[1] = rayStart[1] + dir[1] * t;
sect[2] = rayStart[2] + dir[2] * t;
}
else
{
@ -130,5 +124,5 @@ bool lineIntersectsTriangle(const float *rayStart,const float *rayEnd,const floa
}
}
return ret;
return ret;
}

6
Extras/ConvexDecomposition/raytri.h
View File

@ -36,10 +36,8 @@
// http://www.amillionpixels.us
//
// returns true if the ray intersects the triangle.
bool lineIntersectsTriangle(const float *rayStart,const float *rayEnd,const float *p1,const float *p2,const float *p3,float *sect);
bool rayIntersectsTriangle(const float *p,const float *d,const float *v0,const float *v1,const float *v2,float &t);
bool lineIntersectsTriangle(const float *rayStart, const float *rayEnd, const float *p1, const float *p2, const float *p3, float *sect);
bool rayIntersectsTriangle(const float *p, const float *d, const float *v0, const float *v1, const float *v2, float &t);
#endif

266
Extras/ConvexDecomposition/splitplane.cpp
View File

@ -6,7 +6,6 @@
#include <float.h>
#include <math.h>
/*----------------------------------------------------------------------
Copyright (c) 2004 Open Dynamics Framework Group
www.physicstools.org
@ -53,10 +52,8 @@
namespace ConvexDecomposition
{
static void computePlane(const float *A,const float *B,const float *C,float *plane)
static void computePlane(const float *A, const float *B, const float *C, float *plane)
{
float vx = (B[0] - C[0]);
float vy = (B[1] - C[1]);
float vz = (B[2] - C[2]);
@ -71,108 +68,104 @@ static void computePlane(const float *A,const float *B,const float *C,float *pla
float mag = sqrtf((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
if ( mag < 0.000001f )
if (mag < 0.000001f)
{
mag = 0;
}
else
{
mag = 1.0f/mag;
mag = 1.0f / mag;
}
float x = vw_x * mag;
float y = vw_y * mag;
float z = vw_z * mag;
float D = 0.0f - ((x * A[0]) + (y * A[1]) + (z * A[2]));
float D = 0.0f - ((x*A[0])+(y*A[1])+(z*A[2]));
plane[0] = x;
plane[1] = y;
plane[2] = z;
plane[3] = D;
plane[0] = x;
plane[1] = y;
plane[2] = z;
plane[3] = D;
}
class Rect3d
{
public:
Rect3d(void) { };
Rect3d(void){};
Rect3d(const float *bmin,const float *bmax)
{
Rect3d(const float *bmin, const float *bmax)
{
mMin[0] = bmin[0];
mMin[1] = bmin[1];
mMin[2] = bmin[2];
mMin[0] = bmin[0];
mMin[1] = bmin[1];
mMin[2] = bmin[2];
mMax[0] = bmax[0];
mMax[1] = bmax[1];
mMax[2] = bmax[2];
}
mMax[0] = bmax[0];
mMax[1] = bmax[1];
mMax[2] = bmax[2];
void SetMin(const float *bmin)
{
mMin[0] = bmin[0];
mMin[1] = bmin[1];
mMin[2] = bmin[2];
}
}
void SetMax(const float *bmax)
{
mMax[0] = bmax[0];
mMax[1] = bmax[1];
mMax[2] = bmax[2];
}
void SetMin(const float *bmin)
{
mMin[0] = bmin[0];
mMin[1] = bmin[1];
mMin[2] = bmin[2];
}
void SetMax(const float *bmax)
{
mMax[0] = bmax[0];
mMax[1] = bmax[1];
mMax[2] = bmax[2];
}
void SetMin(float x,float y,float z)
void SetMin(float x, float y, float z)
{
mMin[0] = x;
mMin[1] = y;
mMin[2] = z;
}
void SetMax(float x,float y,float z)
void SetMax(float x, float y, float z)
{
mMax[0] = x;
mMax[1] = y;
mMax[2] = z;
}
float mMin[3];
float mMax[3];
float mMin[3];
float mMax[3];
};
void splitRect(unsigned int axis,
const Rect3d &source,
Rect3d &b1,
Rect3d &b2,
const float *midpoint)
const Rect3d &source,
Rect3d &b1,
Rect3d &b2,
const float *midpoint)
{
switch ( axis )
switch (axis)
{
case 0:
b1.SetMin(source.mMin);
b1.SetMax( midpoint[0], source.mMax[1], source.mMax[2] );
b1.SetMax(midpoint[0], source.mMax[1], source.mMax[2]);
b2.SetMin( midpoint[0], source.mMin[1], source.mMin[2] );
b2.SetMin(midpoint[0], source.mMin[1], source.mMin[2]);
b2.SetMax(source.mMax);
break;
case 1:
b1.SetMin(source.mMin);
b1.SetMax( source.mMax[0], midpoint[1], source.mMax[2] );
b1.SetMax(source.mMax[0], midpoint[1], source.mMax[2]);
b2.SetMin( source.mMin[0], midpoint[1], source.mMin[2] );
b2.SetMin(source.mMin[0], midpoint[1], source.mMin[2]);
b2.SetMax(source.mMax);
break;
case 2:
b1.SetMin(source.mMin);
b1.SetMax( source.mMax[0], source.mMax[1], midpoint[2] );
b1.SetMax(source.mMax[0], source.mMax[1], midpoint[2]);
b2.SetMin( source.mMin[0], source.mMin[1], midpoint[2] );
b2.SetMin(source.mMin[0], source.mMin[1], midpoint[2]);
b2.SetMax(source.mMax);
break;
@ -180,127 +173,122 @@ void splitRect(unsigned int axis,
}
bool computeSplitPlane(unsigned int vcount,
const float *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
float *plane)
const float *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
float *plane)
{
float bmin[3] = { 1e9, 1e9, 1e9 };
float bmax[3] = { -1e9, -1e9, -1e9 };
float bmin[3] = {1e9, 1e9, 1e9};
float bmax[3] = {-1e9, -1e9, -1e9};
for (unsigned int i=0; i<vcount; i++)
{
const float *p = &vertices[i*3];
for (unsigned int i = 0; i < vcount; i++)
{
const float *p = &vertices[i * 3];
if ( p[0] < bmin[0] ) bmin[0] = p[0];
if ( p[1] < bmin[1] ) bmin[1] = p[1];
if ( p[2] < bmin[2] ) bmin[2] = p[2];
if (p[0] < bmin[0]) bmin[0] = p[0];
if (p[1] < bmin[1]) bmin[1] = p[1];
if (p[2] < bmin[2]) bmin[2] = p[2];
if ( p[0] > bmax[0] ) bmax[0] = p[0];
if ( p[1] > bmax[1] ) bmax[1] = p[1];
if ( p[2] > bmax[2] ) bmax[2] = p[2];
}
float dx = bmax[0] - bmin[0];
float dy = bmax[1] - bmin[1];
float dz = bmax[2] - bmin[2];
if (p[0] > bmax[0]) bmax[0] = p[0];
if (p[1] > bmax[1]) bmax[1] = p[1];
if (p[2] > bmax[2]) bmax[2] = p[2];
}
float dx = bmax[0] - bmin[0];
float dy = bmax[1] - bmin[1];
float dz = bmax[2] - bmin[2];
float laxis = dx;
unsigned int axis = 0;
if ( dy > dx )
if (dy > dx)
{
axis = 1;
laxis = dy;
}
if ( dz > dx && dz > dy )
if (dz > dx && dz > dy)
{
axis = 2;
laxis = dz;
}
float p1[3];
float p2[3];
float p3[3];
float p1[3];
float p2[3];
float p3[3];
p3[0] = p2[0] = p1[0] = bmin[0] + dx*0.5f;
p3[1] = p2[1] = p1[1] = bmin[1] + dy*0.5f;
p3[2] = p2[2] = p1[2] = bmin[2] + dz*0.5f;
p3[0] = p2[0] = p1[0] = bmin[0] + dx * 0.5f;
p3[1] = p2[1] = p1[1] = bmin[1] + dy * 0.5f;
p3[2] = p2[2] = p1[2] = bmin[2] + dz * 0.5f;
Rect3d b(bmin,bmax);
Rect3d b(bmin, bmax);
Rect3d b1,b2;
Rect3d b1, b2;
splitRect(axis,b,b1,b2,p1);
splitRect(axis, b, b1, b2, p1);
// callback->ConvexDebugBound(b1.mMin,b1.mMax,0x00FF00);
// callback->ConvexDebugBound(b2.mMin,b2.mMax,0xFFFF00);
// callback->ConvexDebugBound(b1.mMin,b1.mMax,0x00FF00);
// callback->ConvexDebugBound(b2.mMin,b2.mMax,0xFFFF00);
switch (axis)
{
case 0:
p2[1] = bmin[1];
p2[2] = bmin[2];
switch ( axis )
{
case 0:
p2[1] = bmin[1];
p2[2] = bmin[2];
if (dz > dy)
{
p3[1] = bmax[1];
p3[2] = bmin[2];
}
else
{
p3[1] = bmin[1];
p3[2] = bmax[2];
}
if ( dz > dy )
{
p3[1] = bmax[1];
p3[2] = bmin[2];
}
else
{
p3[1] = bmin[1];
p3[2] = bmax[2];
}
break;
case 1:
p2[0] = bmin[0];
p2[2] = bmin[2];
break;
case 1:
p2[0] = bmin[0];
p2[2] = bmin[2];
if (dx > dz)
{
p3[0] = bmax[0];
p3[2] = bmin[2];
}
else
{
p3[0] = bmin[0];
p3[2] = bmax[2];
}
if ( dx > dz )
{
p3[0] = bmax[0];
p3[2] = bmin[2];
}
else
{
p3[0] = bmin[0];
p3[2] = bmax[2];
}
break;
case 2:
p2[0] = bmin[0];
p2[1] = bmin[1];
break;
case 2:
p2[0] = bmin[0];
p2[1] = bmin[1];
if (dx > dy)
{
p3[0] = bmax[0];
p3[1] = bmin[1];
}
else
{
p3[0] = bmin[0];
p3[1] = bmax[1];
}
if ( dx > dy )
{
p3[0] = bmax[0];
p3[1] = bmin[1];
}
else
{
p3[0] = bmin[0];
p3[1] = bmax[1];
}
break;
}
break;
}
// callback->ConvexDebugTri(p1,p2,p3,0xFF0000);
// callback->ConvexDebugTri(p1,p2,p3,0xFF0000);
computePlane(p1,p2,p3,plane);
return true;
computePlane(p1, p2, p3, plane);
return true;
}
}
} // namespace ConvexDecomposition

16
Extras/ConvexDecomposition/splitplane.h
View File

@ -39,21 +39,17 @@
// http://www.amillionpixels.us
//
namespace ConvexDecomposition
{
class ConvexDecompInterface;
bool computeSplitPlane(unsigned int vcount,
const float *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
float *plane);
const float *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
float *plane);
}
} // namespace ConvexDecomposition
#endif

167
Extras/ConvexDecomposition/vlookup.cpp
View File

@ -4,13 +4,12 @@
#include <string.h>
#include <assert.h>
#pragma warning(disable:4786)
#pragma warning(disable : 4786)
#include <vector>
#include <map>
#include <set>
/*----------------------------------------------------------------------
Copyright (c) 2004 Open Dynamics Framework Group
www.physicstools.org
@ -107,55 +106,50 @@
// You could easily modify this code to support other vertex
// formats with any number of interpolants.
#include "vlookup.h"
namespace Vlookup
{
class VertexPosition
{
public:
VertexPosition(void) { };
VertexPosition(const float *p)
{
mPos[0] = p[0];
mPos[1] = p[1];
mPos[2] = p[2];
};
void Set(int index,const float *pos)
VertexPosition(void){};
VertexPosition(const float *p)
{
const float * p = &pos[index*3];
mPos[0] = p[0];
mPos[1] = p[1];
mPos[2] = p[2];
mPos[0] = p[0];
mPos[1] = p[1];
mPos[2] = p[2];
};
float GetX(void) const { return mPos[0]; };
float GetY(void) const { return mPos[1]; };
float GetZ(void) const { return mPos[2]; };
void Set(int index, const float *pos)
{
const float *p = &pos[index * 3];
mPos[0] = p[0];
mPos[1] = p[1];
mPos[2] = p[2];
};
float GetX(void) const { return mPos[0]; };
float GetY(void) const { return mPos[1]; };
float GetZ(void) const { return mPos[2]; };
float mPos[3];
};
typedef std::vector< VertexPosition > VertexVector;
typedef std::vector<VertexPosition> VertexVector;
struct Tracker
{
VertexPosition mFind; // vertice to locate.
VertexVector *mList;
VertexPosition mFind; // vertice to locate.
VertexVector *mList;
Tracker()
{
mList = 0;
}
void SetSearch(const VertexPosition& match,VertexVector *list)
void SetSearch(const VertexPosition &match, VertexVector *list)
{
mFind = match;
mList = list;
@ -165,9 +159,9 @@ struct Tracker
struct VertexID
{
int mID;
Tracker* mTracker;
Tracker *mTracker;
VertexID(int ID, Tracker* Tracker)
VertexID(int ID, Tracker *Tracker)
{
mID = ID;
mTracker = Tracker;
@ -177,46 +171,45 @@ struct VertexID
class VertexLess
{
public:
bool operator()(VertexID v1,VertexID v2) const;
bool operator()(VertexID v1, VertexID v2) const;
private:
const VertexPosition& Get(VertexID index) const
const VertexPosition &Get(VertexID index) const
{
if ( index.mID == -1 ) return index.mTracker->mFind;
if (index.mID == -1) return index.mTracker->mFind;
VertexVector &vlist = *index.mTracker->mList;
return vlist[index.mID];
}
};
template <class Type> class VertexPool
template <class Type>
class VertexPool
{
public:
typedef std::set<VertexID, VertexLess > VertexSet;
typedef std::vector< Type > VertexVector;
typedef std::set<VertexID, VertexLess> VertexSet;
typedef std::vector<Type> VertexVector;
int getVertex(const Type& vtx)
int getVertex(const Type &vtx)
{
mTracker.SetSearch(vtx,&mVtxs);
mTracker.SetSearch(vtx, &mVtxs);
VertexSet::iterator found;
found = mVertSet.find( VertexID(-1,&mTracker) );
if ( found != mVertSet.end() )
found = mVertSet.find(VertexID(-1, &mTracker));
if (found != mVertSet.end())
{
return found->mID;
}
int idx = (int)mVtxs.size();
mVtxs.push_back( vtx );
mVertSet.insert( VertexID(idx,&mTracker) );
mVtxs.push_back(vtx);
mVertSet.insert(VertexID(idx, &mTracker));
return idx;
};
const float * GetPos(int idx) const
const float *GetPos(int idx) const
{
return mVtxs[idx].mPos;
}
const Type& Get(int idx) const
const Type &Get(int idx) const
{
return mVtxs[idx];
};
@ -233,9 +226,9 @@ public:
mVtxs.reserve(reservesize);
};
const VertexVector& GetVertexList(void) const { return mVtxs; };
const VertexVector &GetVertexList(void) const { return mVtxs; };
void Set(const Type& vtx)
void Set(const Type &vtx)
{
mVtxs.push_back(vtx);
}
@ -245,82 +238,74 @@ public:
return mVtxs.size();
};
Type * getBuffer(void)
Type *getBuffer(void)
{
return &mVtxs[0];
};
private:
VertexSet mVertSet; // ordered list.
VertexVector mVtxs; // set of vertices.
Tracker mTracker;
VertexSet mVertSet; // ordered list.
VertexVector mVtxs; // set of vertices.
Tracker mTracker;
};
bool VertexLess::operator()(VertexID v1,VertexID v2) const
bool VertexLess::operator()(VertexID v1, VertexID v2) const
{
const VertexPosition &a = Get(v1);
const VertexPosition &b = Get(v2);
const VertexPosition& a = Get(v1);
const VertexPosition& b = Get(v2);
int ixA = (int)(a.GetX() * 10000.0f);
int ixB = (int)(b.GetX() * 10000.0f);
int ixA = (int) (a.GetX()*10000.0f);
int ixB = (int) (b.GetX()*10000.0f);
if (ixA < ixB) return true;
if (ixA > ixB) return false;
if ( ixA < ixB ) return true;
if ( ixA > ixB ) return false;
int iyA = (int)(a.GetY() * 10000.0f);
int iyB = (int)(b.GetY() * 10000.0f);
int iyA = (int) (a.GetY()*10000.0f);
int iyB = (int) (b.GetY()*10000.0f);
if (iyA < iyB) return true;
if (iyA > iyB) return false;
if ( iyA < iyB ) return true;
if ( iyA > iyB ) return false;
int izA = (int) (a.GetZ()*10000.0f);
int izB = (int) (b.GetZ()*10000.0f);
if ( izA < izB ) return true;
if ( izA > izB ) return false;
int izA = (int)(a.GetZ() * 10000.0f);
int izB = (int)(b.GetZ() * 10000.0f);
if (izA < izB) return true;
if (izA > izB) return false;
return false;
}
}
} // namespace Vlookup
using namespace Vlookup;
VertexLookup Vl_createVertexLookup(void)
{
VertexLookup ret = new VertexPool< VertexPosition >;
return ret;
VertexLookup ret = new VertexPool<VertexPosition>;
return ret;
}
void Vl_releaseVertexLookup(VertexLookup vlook)
void Vl_releaseVertexLookup(VertexLookup vlook)
{
VertexPool< VertexPosition > *vp = (VertexPool< VertexPosition > *) vlook;
delete vp;
VertexPool<VertexPosition> *vp = (VertexPool<VertexPosition> *)vlook;
delete vp;
}
unsigned int Vl_getIndex(VertexLookup vlook,const float *pos) // get index.
unsigned int Vl_getIndex(VertexLookup vlook, const float *pos) // get index.
{
VertexPool< VertexPosition > *vp = (VertexPool< VertexPosition > *) vlook;
VertexPosition p(pos);
return vp->getVertex(p);
VertexPool<VertexPosition> *vp = (VertexPool<VertexPosition> *)vlook;
VertexPosition p(pos);
return vp->getVertex(p);
}
const float * Vl_getVertices(VertexLookup vlook)
const float *Vl_getVertices(VertexLookup vlook)
{
VertexPool< VertexPosition > *vp = (VertexPool< VertexPosition > *) vlook;
return vp->GetPos(0);
VertexPool<VertexPosition> *vp = (VertexPool<VertexPosition> *)vlook;
return vp->GetPos(0);
}
unsigned int Vl_getVcount(VertexLookup vlook)
unsigned int Vl_getVcount(VertexLookup vlook)
{
VertexPool< VertexPosition > *vp = (VertexPool< VertexPosition > *) vlook;
return vp->GetVertexCount();
VertexPool<VertexPosition> *vp = (VertexPool<VertexPosition> *)vlook;
return vp->GetVertexCount();
}

16
Extras/ConvexDecomposition/vlookup.h
View File

@ -2,7 +2,6 @@
#define VLOOKUP_H
/*----------------------------------------------------------------------
Copyright (c) 2004 Open Dynamics Framework Group
www.physicstools.org
@ -37,7 +36,6 @@
// http://www.amillionpixels.us
//
// CodeSnippet provided by John W. Ratcliff
// on March 23, 2006.
//
@ -105,15 +103,13 @@
// Uses an STL set to create an index table for a bunch of vertex positions
// used typically to re-index a collection of raw triangle data.
typedef void *VertexLookup;
typedef void * VertexLookup;
VertexLookup Vl_createVertexLookup(void);
void Vl_releaseVertexLookup(VertexLookup vlook);
unsigned int Vl_getIndex(VertexLookup vlook,const float *pos); // get index.
const float * Vl_getVertices(VertexLookup vlook);
unsigned int Vl_getVcount(VertexLookup vlook);
VertexLookup Vl_createVertexLookup(void);
void Vl_releaseVertexLookup(VertexLookup vlook);
unsigned int Vl_getIndex(VertexLookup vlook, const float *pos); // get index.
const float *Vl_getVertices(VertexLookup vlook);
unsigned int Vl_getVcount(VertexLookup vlook);
#endif

124
Extras/GIMPACTUtils/btGImpactConvexDecompositionShape.cpp
View File

@ -26,17 +26,16 @@ subject to the following restrictions:
class GIM_ConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
{
protected:
btGImpactConvexDecompositionShape * m_compoundShape;
btGImpactConvexDecompositionShape* m_compoundShape;
btAlignedObjectArray<btCollisionShape*> m_convexShapes;
public:
int mBaseCount;
int mHullCount;
int mBaseCount;
int mHullCount;
bool m_transformSubShapes;
GIM_ConvexDecomposition(btGImpactConvexDecompositionShape * compoundShape,bool transformSubShapes)
GIM_ConvexDecomposition(btGImpactConvexDecompositionShape* compoundShape, bool transformSubShapes)
{
mBaseCount = 0;
mHullCount = 0;
@ -47,43 +46,39 @@ public:
virtual ~GIM_ConvexDecomposition()
{
int i;
for (i=0;i<m_convexShapes.size();i++)
for (i = 0; i < m_convexShapes.size(); i++)
{
btCollisionShape* shape = m_convexShapes[i];
delete shape;
}
}
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult& result)
{
//calc centroid, to shift vertices around center of mass
btVector3 centroid(0,0,0);
btVector3 centroid(0, 0, 0);
btAlignedObjectArray<btVector3> vertices;
if(m_transformSubShapes)
if (m_transformSubShapes)
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
for (unsigned int i = 0; i < result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
btVector3 vertex(result.mHullVertices[i * 3], result.mHullVertices[i * 3 + 1], result.mHullVertices[i * 3 + 2]);
centroid += vertex;
}
centroid *= 1.f/(float(result.mHullVcount) );
centroid *= 1.f / (float(result.mHullVcount));
}
// collect vertices
for (unsigned int i=0; i<result.mHullVcount; i++)
for (unsigned int i = 0; i < result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
btVector3 vertex(result.mHullVertices[i * 3], result.mHullVertices[i * 3 + 1], result.mHullVertices[i * 3 + 2]);
if(m_transformSubShapes)
if (m_transformSubShapes)
{
vertex -= centroid ;
vertex -= centroid;
}
vertices.push_back(vertex);
}
@ -91,12 +86,12 @@ public:
// build convex shape
btCollisionShape* convexShape = new btConvexHullShape(
&(vertices[0].getX()),vertices.size(),sizeof(btVector3));
&(vertices[0].getX()), vertices.size(), sizeof(btVector3));
m_convexShapes.push_back(convexShape);
convexShape->setMargin(m_compoundShape->getMargin());
if(m_transformSubShapes)
if (m_transformSubShapes)
{
btTransform trans;
trans.setIdentity();
@ -104,7 +99,7 @@ public:
// add convex shape
m_compoundShape->addChildShape(trans,convexShape);
m_compoundShape->addChildShape(trans, convexShape);
}
else
{
@ -114,7 +109,7 @@ public:
// add convex shape
m_compoundShape->addChildShape(trans,convexShape);
m_compoundShape->addChildShape(trans, convexShape);
//m_compoundShape->addChildShape(convexShape);
}
@ -122,35 +117,32 @@ public:
void processDecomposition(int part)
{
btGImpactMeshShapePart::TrimeshPrimitiveManager * trimeshInterface =
m_compoundShape->getTrimeshInterface(part);
btGImpactMeshShapePart::TrimeshPrimitiveManager* trimeshInterface =
m_compoundShape->getTrimeshInterface(part);
trimeshInterface->lock();
//collect vertices
btAlignedObjectArray<float> vertices;
vertices.reserve(trimeshInterface->get_vertex_count()*3);
vertices.reserve(trimeshInterface->get_vertex_count() * 3);
for(int vi = 0;vi<trimeshInterface->get_vertex_count();vi++)
for (int vi = 0; vi < trimeshInterface->get_vertex_count(); vi++)
{
btVector3 vec;
trimeshInterface->get_vertex(vi,vec);
trimeshInterface->get_vertex(vi, vec);
vertices.push_back(vec[0]);
vertices.push_back(vec[1]);
vertices.push_back(vec[2]);
}
//collect indices
btAlignedObjectArray<unsigned int> indices;
indices.reserve(trimeshInterface->get_primitive_count()*3);
indices.reserve(trimeshInterface->get_primitive_count() * 3);
for(int i = 0;i<trimeshInterface->get_primitive_count();i++)
for (int i = 0; i < trimeshInterface->get_primitive_count(); i++)
{
unsigned int i0, i1,i2;
trimeshInterface->get_indices(i,i0,i1,i2);
unsigned int i0, i1, i2;
trimeshInterface->get_indices(i, i0, i1, i2);
indices.push_back(i0);
indices.push_back(i1);
indices.push_back(i2);
@ -158,25 +150,22 @@ public:
trimeshInterface->unlock();
unsigned int depth = 5;
float cpercent = 5;
float ppercent = 15;
unsigned int maxv = 16;
float skinWidth = 0.0f;
float cpercent = 5;
float ppercent = 15;
unsigned int maxv = 16;
float skinWidth = 0.0f;
ConvexDecomposition::DecompDesc desc;
desc.mVcount = trimeshInterface->get_vertex_count();
desc.mVertices = &vertices[0];
desc.mTcount = trimeshInterface->get_primitive_count();
desc.mIndices = &indices[0];
desc.mDepth = depth;
desc.mCpercent = cpercent;
desc.mPpercent = ppercent;
desc.mMaxVertices = maxv;
desc.mSkinWidth = skinWidth;
desc.mVcount = trimeshInterface->get_vertex_count();
desc.mVertices = &vertices[0];
desc.mTcount = trimeshInterface->get_primitive_count();
desc.mIndices = &indices[0];
desc.mDepth = depth;
desc.mCpercent = cpercent;
desc.mPpercent = ppercent;
desc.mMaxVertices = maxv;
desc.mSkinWidth = skinWidth;
desc.mCallback = this;
//convexDecomposition.performConvexDecomposition(desc);
@ -184,21 +173,14 @@ public:
ConvexBuilder cb(desc.mCallback);
cb.process(desc);
}
};
void btGImpactConvexDecompositionShape::buildConvexDecomposition(bool transformSubShapes)
{
m_decomposition = new GIM_ConvexDecomposition(this,transformSubShapes);
m_decomposition = new GIM_ConvexDecomposition(this, transformSubShapes);
int part_count = m_trimeshInterfaces.size();
for (int i = 0;i<part_count ;i++ )
for (int i = 0; i < part_count; i++)
{
m_decomposition->processDecomposition(i);
}
@ -210,31 +192,27 @@ btGImpactConvexDecompositionShape::~btGImpactConvexDecompositionShape()
{
delete m_decomposition;
}
void btGImpactConvexDecompositionShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
void btGImpactConvexDecompositionShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
{
int part_count = m_trimeshInterfaces.size();
for (int part = 0;part<part_count ;part++ )
for (int part = 0; part < part_count; part++)
{
void * ptr = (void * )&m_trimeshInterfaces[part];
void* ptr = (void*)&m_trimeshInterfaces[part];
btGImpactMeshShapePart::TrimeshPrimitiveManager * trimeshInterface =
static_cast<btGImpactMeshShapePart::TrimeshPrimitiveManager *>(ptr);
btGImpactMeshShapePart::TrimeshPrimitiveManager* trimeshInterface =
static_cast<btGImpactMeshShapePart::TrimeshPrimitiveManager*>(ptr);
trimeshInterface->lock();
btPrimitiveTriangle triangle;
int i = trimeshInterface->get_primitive_count();
while(i--)
while (i--)
{
trimeshInterface->get_primitive_triangle(i,triangle);
callback->processTriangle(triangle.m_vertices,part,i);
trimeshInterface->get_primitive_triangle(i, triangle);
callback->processTriangle(triangle.m_vertices, part, i);
}
trimeshInterface->unlock();
}
}

32
Extras/GIMPACTUtils/btGImpactConvexDecompositionShape.h
View File

@ -24,32 +24,28 @@ subject to the following restrictions:
#ifndef GIMPACT_CONVEX_DECOMPOSITION_SHAPE_H
#define GIMPACT_CONVEX_DECOMPOSITION_SHAPE_H
#include "BulletCollision/Gimpact/btGImpactShape.h" // box tree class
#include "BulletCollision/Gimpact/btGImpactShape.h" // box tree class
//! This class creates a decomposition from a trimesh.
/*!
*/
class btGImpactConvexDecompositionShape : public btGImpactCompoundShape
class btGImpactConvexDecompositionShape : public btGImpactCompoundShape
{
protected:
btAlignedObjectArray<btGImpactMeshShapePart::TrimeshPrimitiveManager> m_trimeshInterfaces;
class GIM_ConvexDecomposition* m_decomposition;
class GIM_ConvexDecomposition* m_decomposition;
void buildConvexDecomposition(bool transformSubShapes);
public:
btGImpactConvexDecompositionShape(
btStridingMeshInterface * meshInterface,
const btVector3 & mesh_scale,
btScalar margin = btScalar(0.01),bool children_has_transform = true)
:btGImpactCompoundShape(children_has_transform)
btStridingMeshInterface* meshInterface,
const btVector3& mesh_scale,
btScalar margin = btScalar(0.01), bool children_has_transform = true)
: btGImpactCompoundShape(children_has_transform)
{
m_collisionMargin = margin;
btGImpactMeshShapePart::TrimeshPrimitiveManager triInterface;
@ -59,7 +55,7 @@ public:
//add parts
int part_count = meshInterface->getNumSubParts();
for (int i=0;i< part_count;i++ )
for (int i = 0; i < part_count; i++)
{
triInterface.m_part = i;
m_trimeshInterfaces.push_back(triInterface);
@ -72,16 +68,12 @@ public:
virtual ~btGImpactConvexDecompositionShape();
SIMD_FORCE_INLINE btGImpactMeshShapePart::TrimeshPrimitiveManager * getTrimeshInterface(int part)
SIMD_FORCE_INLINE btGImpactMeshShapePart::TrimeshPrimitiveManager* getTrimeshInterface(int part)
{
return &m_trimeshInterfaces[part];
}
virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const;
};
#endif //GIMPACT_MESH_SHAPE_H
#endif //GIMPACT_MESH_SHAPE_H

116
Extras/HACD/hacdCircularList.h
View File

@ -15,66 +15,72 @@
#pragma once
#ifndef HACD_CIRCULAR_LIST_H
#define HACD_CIRCULAR_LIST_H
#include<stdlib.h>
#include <stdlib.h>
#include "hacdVersion.h"
namespace HACD
{
//! CircularListElement class.
template < typename T > class CircularListElement
{
public:
T & GetData() { return m_data; }
const T & GetData() const { return m_data; }
CircularListElement<T> * & GetNext() { return m_next; }
CircularListElement<T> * & GetPrev() { return m_prev; }
const CircularListElement<T> * & GetNext() const { return m_next; }
const CircularListElement<T> * & GetPrev() const { return m_prev; }
//! Constructor
CircularListElement(const T & data) {m_data = data;}
CircularListElement(void){}
//! Destructor
~CircularListElement(void){}
private:
T m_data;
CircularListElement<T> * m_next;
CircularListElement<T> * m_prev;
//! CircularListElement class.
template <typename T>
class CircularListElement
{
public:
T& GetData() { return m_data; }
const T& GetData() const { return m_data; }
CircularListElement<T>*& GetNext() { return m_next; }
CircularListElement<T>*& GetPrev() { return m_prev; }
const CircularListElement<T>*& GetNext() const { return m_next; }
const CircularListElement<T>*& GetPrev() const { return m_prev; }
//! Constructor
CircularListElement(const T& data) { m_data = data; }
CircularListElement(void) {}
//! Destructor
~CircularListElement(void) {}
CircularListElement(const CircularListElement & rhs);
};
//! CircularList class.
template < typename T > class CircularList
private:
T m_data;
CircularListElement<T>* m_next;
CircularListElement<T>* m_prev;
CircularListElement(const CircularListElement& rhs);
};
//! CircularList class.
template <typename T>
class CircularList
{
public:
CircularListElement<T>*& GetHead() { return m_head; }
const CircularListElement<T>* GetHead() const { return m_head; }
bool IsEmpty() const { return (m_size == 0); }
size_t GetSize() const { return m_size; }
const T& GetData() const { return m_head->GetData(); }
T& GetData() { return m_head->GetData(); }
bool Delete();
bool Delete(CircularListElement<T>* element);
CircularListElement<T>* Add(const T* data = 0);
CircularListElement<T>* Add(const T& data);
bool Next();
bool Prev();
void Clear()
{
public:
CircularListElement<T> * & GetHead() { return m_head;}
const CircularListElement<T> * GetHead() const { return m_head;}
bool IsEmpty() const { return (m_size == 0);}
size_t GetSize() const { return m_size; }
const T & GetData() const { return m_head->GetData(); }
T & GetData() { return m_head->GetData();}
bool Delete() ;
bool Delete(CircularListElement<T> * element);
CircularListElement<T> * Add(const T * data = 0);
CircularListElement<T> * Add(const T & data);
bool Next();
bool Prev();
void Clear() { while(Delete());};
const CircularList& operator=(const CircularList& rhs);
//! Constructor
CircularList()
{
m_head = 0;
m_size = 0;
}
CircularList(const CircularList& rhs);
//! Destructor
virtual ~CircularList(void) {Clear();};
private:
CircularListElement<T> * m_head; //!< a pointer to the head of the circular list
size_t m_size; //!< number of element in the circular list
while (Delete())
;
};
}
const CircularList& operator=(const CircularList& rhs);
//! Constructor
CircularList()
{
m_head = 0;
m_size = 0;
}
CircularList(const CircularList& rhs);
//! Destructor
virtual ~CircularList(void) { Clear(); };
private:
CircularListElement<T>* m_head; //!< a pointer to the head of the circular list
size_t m_size; //!< number of element in the circular list
};
} // namespace HACD
#include "hacdCircularList.inl"
#endif

548
Extras/HACD/hacdGraph.cpp
View File

@ -14,279 +14,277 @@
*/
#include "hacdGraph.h"
namespace HACD
{
GraphEdge::GraphEdge()
{
m_convexHull = 0;
m_v1 = -1;
m_v2 = -1;
m_name = -1;
m_error = 0;
m_surf = 0;
m_perimeter = 0;
m_concavity = 0;
m_volume = 0;
m_deleted = false;
}
GraphVertex::GraphVertex()
{
m_convexHull = 0;
m_name = -1;
m_cc = -1;
m_error = 0;
m_surf = 0;
m_perimeter = 0;
m_concavity = 0;
m_volume = 0;
m_deleted = false;
}
bool GraphVertex::DeleteEdge(long name)
{
std::set<long>::iterator it = m_edges.find(name);
if (it != m_edges.end() )
{
m_edges.erase(it);
return true;
}
return false;
}
Graph::Graph()
{
m_nV = 0;
m_nE = 0;
m_nCCs = 0;
}
Graph::~Graph()
{
}
void Graph::Allocate(size_t nV, size_t nE)
{
m_nV = nV;
m_edges.reserve(nE);
m_vertices.resize(nV);
for(size_t i = 0; i < nV; i++)
{
m_vertices[i].m_name = static_cast<long>(i);
}
}
long Graph::AddVertex()
{
size_t name = m_vertices.size();
m_vertices.resize(name+1);
m_vertices[name].m_name = static_cast<long>(name);
m_nV++;
return static_cast<long>(name);
}
long Graph::AddEdge(long v1, long v2)
{
size_t name = m_edges.size();
m_edges.push_back(GraphEdge());
m_edges[name].m_name = static_cast<long>(name);
m_edges[name].m_v1 = v1;
m_edges[name].m_v2 = v2;
m_vertices[v1].AddEdge(static_cast<long>(name));
m_vertices[v2].AddEdge(static_cast<long>(name));
m_nE++;
return static_cast<long>(name);
}
bool Graph::DeleteEdge(long name)
{
if (name < static_cast<long>(m_edges.size()))
{
long v1 = m_edges[name].m_v1;
long v2 = m_edges[name].m_v2;
m_edges[name].m_deleted = true;
m_vertices[v1].DeleteEdge(name);
m_vertices[v2].DeleteEdge(name);
delete m_edges[name].m_convexHull;
m_edges[name].m_distPoints.clear();
m_edges[name].m_boudaryEdges.clear();
m_edges[name].m_convexHull = 0;
m_nE--;
return true;
}
return false;
}
bool Graph::DeleteVertex(long name)
{
if (name < static_cast<long>(m_vertices.size()))
{
m_vertices[name].m_deleted = true;
m_vertices[name].m_edges.clear();
m_vertices[name].m_ancestors = std::vector<long>();
delete m_vertices[name].m_convexHull;
m_vertices[name].m_distPoints.clear();
m_vertices[name].m_boudaryEdges.clear();
m_vertices[name].m_convexHull = 0;
m_nV--;
return true;
}
return false;
}
bool Graph::EdgeCollapse(long v1, long v2)
{
long edgeToDelete = GetEdgeID(v1, v2);
if (edgeToDelete >= 0)
{
// delete the edge (v1, v2)
DeleteEdge(edgeToDelete);
// add v2 to v1 ancestors
m_vertices[v1].m_ancestors.push_back(v2);
// add v2's ancestors to v1's ancestors
m_vertices[v1].m_ancestors.insert(m_vertices[v1].m_ancestors.begin(),
m_vertices[v2].m_ancestors.begin(),
m_vertices[v2].m_ancestors.end());
// update adjacency information
std::set<long> & v1Edges = m_vertices[v1].m_edges;
std::set<long>::const_iterator ed(m_vertices[v2].m_edges.begin());
std::set<long>::const_iterator itEnd(m_vertices[v2].m_edges.end());
long b = -1;
for(; ed != itEnd; ++ed)
{
if (m_edges[*ed].m_v1 == v2)
{
b = m_edges[*ed].m_v2;
}
else
{
b = m_edges[*ed].m_v1;
}
if (GetEdgeID(v1, b) >= 0)
{
m_edges[*ed].m_deleted = true;
m_vertices[b].DeleteEdge(*ed);
m_nE--;
}
else
{
m_edges[*ed].m_v1 = v1;
m_edges[*ed].m_v2 = b;
v1Edges.insert(*ed);
}
}
// delete the vertex v2
DeleteVertex(v2);
return true;
}
return false;
}
long Graph::GetEdgeID(long v1, long v2) const
{
if (v1 < static_cast<long>(m_vertices.size()) && !m_vertices[v1].m_deleted)
{
std::set<long>::const_iterator ed(m_vertices[v1].m_edges.begin());
std::set<long>::const_iterator itEnd(m_vertices[v1].m_edges.end());
for(; ed != itEnd; ++ed)
{
if ( (m_edges[*ed].m_v1 == v2) ||
(m_edges[*ed].m_v2 == v2) )
{
return m_edges[*ed].m_name;
}
}
}
return -1;
}
void Graph::Print() const
{
std::cout << "-----------------------------" << std::endl;
std::cout << "vertices (" << m_nV << ")" << std::endl;
for (size_t v = 0; v < m_vertices.size(); ++v)
{
const GraphVertex & currentVertex = m_vertices[v];
if (!m_vertices[v].m_deleted)
{
std::cout << currentVertex.m_name << "\t";
std::set<long>::const_iterator ed(currentVertex.m_edges.begin());
std::set<long>::const_iterator itEnd(currentVertex.m_edges.end());
for(; ed != itEnd; ++ed)
{
std::cout << "(" << m_edges[*ed].m_v1 << "," << m_edges[*ed].m_v2 << ") ";
}
std::cout << std::endl;
}
}
std::cout << "vertices (" << m_nE << ")" << std::endl;
for (size_t e = 0; e < m_edges.size(); ++e)
{
const GraphEdge & currentEdge = m_edges[e];
if (!m_edges[e].m_deleted)
{
std::cout << currentEdge.m_name << "\t("
<< m_edges[e].m_v1 << ","
<< m_edges[e].m_v2 << ") "<< std::endl;
}
}
}
void Graph::Clear()
{
m_vertices.clear();
m_edges.clear();
m_nV = 0;
m_nE = 0;
}
long Graph::ExtractCCs()
{
// all CCs to -1
for (size_t v = 0; v < m_vertices.size(); ++v)
{
if (!m_vertices[v].m_deleted)
{
m_vertices[v].m_cc = -1;
}
}
// we get the CCs
m_nCCs = 0;
long v2 = -1;
std::vector<long> temp;
for (size_t v = 0; v < m_vertices.size(); ++v)
{
if (!m_vertices[v].m_deleted && m_vertices[v].m_cc == -1)
{
m_vertices[v].m_cc = static_cast<long>(m_nCCs);
temp.clear();
temp.push_back(m_vertices[v].m_name);
while (temp.size())
{
long vertex = temp[temp.size()-1];
temp.pop_back();
std::set<long>::const_iterator ed(m_vertices[vertex].m_edges.begin());
std::set<long>::const_iterator itEnd(m_vertices[vertex].m_edges.end());
for(; ed != itEnd; ++ed)
{
if (m_edges[*ed].m_v1 == vertex)
{
v2 = m_edges[*ed].m_v2;
}
else
{
v2 = m_edges[*ed].m_v1;
}
if ( !m_vertices[v2].m_deleted && m_vertices[v2].m_cc == -1)
{
m_vertices[v2].m_cc = static_cast<long>(m_nCCs);
temp.push_back(v2);
}
}
}
m_nCCs++;
}
}
return static_cast<long>(m_nCCs);
}
{
GraphEdge::GraphEdge()
{
m_convexHull = 0;
m_v1 = -1;
m_v2 = -1;
m_name = -1;
m_error = 0;
m_surf = 0;
m_perimeter = 0;
m_concavity = 0;
m_volume = 0;
m_deleted = false;
}
GraphVertex::GraphVertex()
{
m_convexHull = 0;
m_name = -1;
m_cc = -1;
m_error = 0;
m_surf = 0;
m_perimeter = 0;
m_concavity = 0;
m_volume = 0;
m_deleted = false;
}
bool GraphVertex::DeleteEdge(long name)
{
std::set<long>::iterator it = m_edges.find(name);
if (it != m_edges.end())
{
m_edges.erase(it);
return true;
}
return false;
}
Graph::Graph()
{
m_nV = 0;
m_nE = 0;
m_nCCs = 0;
}
Graph::~Graph()
{
}
void Graph::Allocate(size_t nV, size_t nE)
{
m_nV = nV;
m_edges.reserve(nE);
m_vertices.resize(nV);
for (size_t i = 0; i < nV; i++)
{
m_vertices[i].m_name = static_cast<long>(i);
}
}
long Graph::AddVertex()
{
size_t name = m_vertices.size();
m_vertices.resize(name + 1);
m_vertices[name].m_name = static_cast<long>(name);
m_nV++;
return static_cast<long>(name);
}
long Graph::AddEdge(long v1, long v2)
{
size_t name = m_edges.size();
m_edges.push_back(GraphEdge());
m_edges[name].m_name = static_cast<long>(name);
m_edges[name].m_v1 = v1;
m_edges[name].m_v2 = v2;
m_vertices[v1].AddEdge(static_cast<long>(name));
m_vertices[v2].AddEdge(static_cast<long>(name));
m_nE++;
return static_cast<long>(name);
}
bool Graph::DeleteEdge(long name)
{
if (name < static_cast<long>(m_edges.size()))
{
long v1 = m_edges[name].m_v1;
long v2 = m_edges[name].m_v2;
m_edges[name].m_deleted = true;
m_vertices[v1].DeleteEdge(name);
m_vertices[v2].DeleteEdge(name);
delete m_edges[name].m_convexHull;
m_edges[name].m_distPoints.clear();
m_edges[name].m_boudaryEdges.clear();
m_edges[name].m_convexHull = 0;
m_nE--;
return true;
}
return false;
}
bool Graph::DeleteVertex(long name)
{
if (name < static_cast<long>(m_vertices.size()))
{
m_vertices[name].m_deleted = true;
m_vertices[name].m_edges.clear();
m_vertices[name].m_ancestors = std::vector<long>();
delete m_vertices[name].m_convexHull;
m_vertices[name].m_distPoints.clear();
m_vertices[name].m_boudaryEdges.clear();
m_vertices[name].m_convexHull = 0;
m_nV--;
return true;
}
return false;
}
bool Graph::EdgeCollapse(long v1, long v2)
{
long edgeToDelete = GetEdgeID(v1, v2);
if (edgeToDelete >= 0)
{
// delete the edge (v1, v2)
DeleteEdge(edgeToDelete);
// add v2 to v1 ancestors
m_vertices[v1].m_ancestors.push_back(v2);
// add v2's ancestors to v1's ancestors
m_vertices[v1].m_ancestors.insert(m_vertices[v1].m_ancestors.begin(),
m_vertices[v2].m_ancestors.begin(),
m_vertices[v2].m_ancestors.end());
// update adjacency information
std::set<long>& v1Edges = m_vertices[v1].m_edges;
std::set<long>::const_iterator ed(m_vertices[v2].m_edges.begin());
std::set<long>::const_iterator itEnd(m_vertices[v2].m_edges.end());
long b = -1;
for (; ed != itEnd; ++ed)
{
if (m_edges[*ed].m_v1 == v2)
{
b = m_edges[*ed].m_v2;
}
else
{
b = m_edges[*ed].m_v1;
}
if (GetEdgeID(v1, b) >= 0)
{
m_edges[*ed].m_deleted = true;
m_vertices[b].DeleteEdge(*ed);
m_nE--;
}
else
{
m_edges[*ed].m_v1 = v1;
m_edges[*ed].m_v2 = b;
v1Edges.insert(*ed);
}
}
// delete the vertex v2
DeleteVertex(v2);
return true;
}
return false;
}
long Graph::GetEdgeID(long v1, long v2) const
{
if (v1 < static_cast<long>(m_vertices.size()) && !m_vertices[v1].m_deleted)
{
std::set<long>::const_iterator ed(m_vertices[v1].m_edges.begin());
std::set<long>::const_iterator itEnd(m_vertices[v1].m_edges.end());
for (; ed != itEnd; ++ed)
{
if ((m_edges[*ed].m_v1 == v2) ||
(m_edges[*ed].m_v2 == v2))
{
return m_edges[*ed].m_name;
}
}
}
return -1;
}
void Graph::Print() const
{
std::cout << "-----------------------------" << std::endl;
std::cout << "vertices (" << m_nV << ")" << std::endl;
for (size_t v = 0; v < m_vertices.size(); ++v)
{
const GraphVertex& currentVertex = m_vertices[v];
if (!m_vertices[v].m_deleted)
{
std::cout << currentVertex.m_name << "\t";
std::set<long>::const_iterator ed(currentVertex.m_edges.begin());
std::set<long>::const_iterator itEnd(currentVertex.m_edges.end());
for (; ed != itEnd; ++ed)
{
std::cout << "(" << m_edges[*ed].m_v1 << "," << m_edges[*ed].m_v2 << ") ";
}
std::cout << std::endl;
}
}
std::cout << "vertices (" << m_nE << ")" << std::endl;
for (size_t e = 0; e < m_edges.size(); ++e)
{
const GraphEdge& currentEdge = m_edges[e];
if (!m_edges[e].m_deleted)
{
std::cout << currentEdge.m_name << "\t("
<< m_edges[e].m_v1 << ","
<< m_edges[e].m_v2 << ") " << std::endl;
}
}
}
void Graph::Clear()
{
m_vertices.clear();
m_edges.clear();
m_nV = 0;
m_nE = 0;
}
long Graph::ExtractCCs()
{
// all CCs to -1
for (size_t v = 0; v < m_vertices.size(); ++v)
{
if (!m_vertices[v].m_deleted)
{
m_vertices[v].m_cc = -1;
}
}
// we get the CCs
m_nCCs = 0;
long v2 = -1;
std::vector<long> temp;
for (size_t v = 0; v < m_vertices.size(); ++v)
{
if (!m_vertices[v].m_deleted && m_vertices[v].m_cc == -1)
{
m_vertices[v].m_cc = static_cast<long>(m_nCCs);
temp.clear();
temp.push_back(m_vertices[v].m_name);
while (temp.size())
{
long vertex = temp[temp.size() - 1];
temp.pop_back();
std::set<long>::const_iterator ed(m_vertices[vertex].m_edges.begin());
std::set<long>::const_iterator itEnd(m_vertices[vertex].m_edges.end());
for (; ed != itEnd; ++ed)
{
if (m_edges[*ed].m_v1 == vertex)
{
v2 = m_edges[*ed].m_v2;
}
else
{
v2 = m_edges[*ed].m_v1;
}
if (!m_vertices[v2].m_deleted && m_vertices[v2].m_cc == -1)
{
m_vertices[v2].m_cc = static_cast<long>(m_nCCs);
temp.push_back(v2);
}
}
}
m_nCCs++;
}
}
return static_cast<long>(m_nCCs);
}
} // namespace HACD

175
Extras/HACD/hacdGraph.h
View File

@ -24,97 +24,96 @@
namespace HACD
{
class GraphVertex;
class GraphEdge;
class Graph;
class HACD;
class GraphVertex
{
public:
bool AddEdge(long name)
{
m_edges.insert(name);
return true;
}
bool DeleteEdge(long name);
GraphVertex();
~GraphVertex(){ delete m_convexHull;};
private:
long m_name;
long m_cc;
std::set<long> m_edges;
bool m_deleted;
std::vector<long> m_ancestors;
std::map<long, DPoint> m_distPoints;
class GraphVertex;
class GraphEdge;
class Graph;
class HACD;
Real m_error;
double m_surf;
double m_volume;
double m_perimeter;
double m_concavity;
ICHull * m_convexHull;
std::set<unsigned long long> m_boudaryEdges;
class GraphVertex
{
public:
bool AddEdge(long name)
{
m_edges.insert(name);
return true;
}
bool DeleteEdge(long name);
GraphVertex();
~GraphVertex() { delete m_convexHull; };
friend class GraphEdge;
friend class Graph;
friend class HACD;
};
class GraphEdge
{
public:
GraphEdge();
~GraphEdge(){delete m_convexHull;};
private:
long m_name;
long m_v1;
long m_v2;
std::map<long, DPoint> m_distPoints;
Real m_error;
double m_surf;
double m_volume;
double m_perimeter;
double m_concavity;
ICHull * m_convexHull;
std::set<unsigned long long> m_boudaryEdges;
bool m_deleted;
private:
long m_name;
long m_cc;
std::set<long> m_edges;
bool m_deleted;
std::vector<long> m_ancestors;
std::map<long, DPoint> m_distPoints;
friend class GraphVertex;
friend class Graph;
friend class HACD;
};
class Graph
{
public:
size_t GetNEdges() const { return m_nE;}
size_t GetNVertices() const { return m_nV;}
bool EdgeCollapse(long v1, long v2);
long AddVertex();
long AddEdge(long v1, long v2);
bool DeleteEdge(long name);
bool DeleteVertex(long name);
long GetEdgeID(long v1, long v2) const;
void Clear();
void Print() const;
long ExtractCCs();
Graph();
virtual ~Graph();
void Allocate(size_t nV, size_t nE);
Real m_error;
double m_surf;
double m_volume;
double m_perimeter;
double m_concavity;
ICHull* m_convexHull;
std::set<unsigned long long> m_boudaryEdges;
private:
size_t m_nCCs;
size_t m_nV;
size_t m_nE;
std::vector<GraphEdge> m_edges;
std::vector<GraphVertex> m_vertices;
friend class GraphEdge;
friend class Graph;
friend class HACD;
};
friend class HACD;
};
}
class GraphEdge
{
public:
GraphEdge();
~GraphEdge() { delete m_convexHull; };
private:
long m_name;
long m_v1;
long m_v2;
std::map<long, DPoint> m_distPoints;
Real m_error;
double m_surf;
double m_volume;
double m_perimeter;
double m_concavity;
ICHull* m_convexHull;
std::set<unsigned long long> m_boudaryEdges;
bool m_deleted;
friend class GraphVertex;
friend class Graph;
friend class HACD;
};
class Graph
{
public:
size_t GetNEdges() const { return m_nE; }
size_t GetNVertices() const { return m_nV; }
bool EdgeCollapse(long v1, long v2);
long AddVertex();
long AddEdge(long v1, long v2);
bool DeleteEdge(long name);
bool DeleteVertex(long name);
long GetEdgeID(long v1, long v2) const;
void Clear();
void Print() const;
long ExtractCCs();
Graph();
virtual ~Graph();
void Allocate(size_t nV, size_t nE);
private:
size_t m_nCCs;
size_t m_nV;
size_t m_nE;
std::vector<GraphEdge> m_edges;
std::vector<GraphVertex> m_vertices;
friend class HACD;
};
} // namespace HACD
#endif

1574
Extras/HACD/hacdHACD.cpp
View File

File diff suppressed because it is too large Load Diff

501
Extras/HACD/hacdHACD.h
View File

@ -26,257 +26,260 @@
namespace HACD
{
const double sc_pi = 3.14159265;
class HACD;
const double sc_pi = 3.14159265;
class HACD;
// just to be able to set the capcity of the container
template<class _Ty, class _Container = std::vector<_Ty>, class _Pr = std::less<typename _Container::value_type> >
class reservable_priority_queue: public std::priority_queue<_Ty, _Container, _Pr>
// just to be able to set the capcity of the container
template <class _Ty, class _Container = std::vector<_Ty>, class _Pr = std::less<typename _Container::value_type> >
class reservable_priority_queue : public std::priority_queue<_Ty, _Container, _Pr>
{
typedef typename std::priority_queue<_Ty, _Container, _Pr>::size_type size_type;
public:
reservable_priority_queue(size_type capacity = 0) { reserve(capacity); };
void reserve(size_type capacity) { this->c.reserve(capacity); }
size_type capacity() const { return this->c.capacity(); }
};
//! priority queque element
class GraphEdgePriorityQueue
{
public:
//! Constructor
//! @param name edge's id
//! @param priority edge's priority
GraphEdgePriorityQueue(long name, Real priority)
{
typedef typename std::priority_queue<_Ty, _Container, _Pr>::size_type size_type;
public:
reservable_priority_queue(size_type capacity = 0) { reserve(capacity); };
void reserve(size_type capacity) { this->c.reserve(capacity); }
size_type capacity() const { return this->c.capacity(); }
};
//! priority queque element
class GraphEdgePriorityQueue
{
public:
//! Constructor
//! @param name edge's id
//! @param priority edge's priority
GraphEdgePriorityQueue(long name, Real priority)
{
m_name = name;
m_priority = priority;
}
//! Destructor
~GraphEdgePriorityQueue(void){}
private:
long m_name; //!< edge name
Real m_priority; //!< priority
//! Operator < for GraphEdgePQ
friend bool operator<(const GraphEdgePriorityQueue & lhs, const GraphEdgePriorityQueue & rhs);
//! Operator > for GraphEdgePQ
friend bool operator>(const GraphEdgePriorityQueue & lhs, const GraphEdgePriorityQueue & rhs);
friend class HACD;
};
inline bool operator<(const GraphEdgePriorityQueue & lhs, const GraphEdgePriorityQueue & rhs)
{
return lhs.m_priority<rhs.m_priority;
}
inline bool operator>(const GraphEdgePriorityQueue & lhs, const GraphEdgePriorityQueue & rhs)
{
return lhs.m_priority>rhs.m_priority;
}
typedef bool (*CallBackFunction)(const char *, double, double, size_t);
m_name = name;
m_priority = priority;
}
//! Destructor
~GraphEdgePriorityQueue(void) {}
//! Provides an implementation of the Hierarchical Approximate Convex Decomposition (HACD) technique described in "A Simple and Efficient Approach for 3D Mesh Approximate Convex Decomposition" Game Programming Gems 8 - Chapter 2.8, p.202. A short version of the chapter was published in ICIP09 and is available at ftp://ftp.elet.polimi.it/users/Stefano.Tubaro/ICIP_USB_Proceedings_v2/pdfs/0003501.pdf
class HACD
{
public:
//! Gives the triangles partitionas an array of size m_nTriangles where the i-th element specifies the cluster to which belong the i-th triangle
//! @return triangles partition
const long * GetPartition() const { return m_partition;}
//! Sets the scale factor
//! @param scale scale factor
void SetScaleFactor(double scale) { m_scale = scale;}
//! Gives the scale factor
//! @return scale factor
double GetScaleFactor() const { return m_scale;}
//! Sets the call-back function
//! @param callBack pointer to the call-back function
void SetCallBack(CallBackFunction callBack) { m_callBack = callBack;}
//! Gives the call-back function
//! @return pointer to the call-back function
CallBackFunction GetCallBack() const { return m_callBack;}
//! Specifies whether faces points should be added when computing the concavity
//! @param addFacesPoints true = faces points should be added
void SetAddFacesPoints(bool addFacesPoints) { m_addFacesPoints = addFacesPoints;}
//! Specifies wheter faces points should be added when computing the concavity
//! @return true = faces points should be added
bool GetAddFacesPoints() const { return m_addFacesPoints;}
//! Specifies whether extra points should be added when computing the concavity
//! @param addExteraDistPoints true = extra points should be added
void SetAddExtraDistPoints(bool addExtraDistPoints) { m_addExtraDistPoints = addExtraDistPoints;}
//! Specifies wheter extra points should be added when computing the concavity
//! @return true = extra points should be added
bool GetAddExtraDistPoints() const { return m_addExtraDistPoints;}
//! Specifies whether extra points should be added when computing the concavity
//! @param addExteraDistPoints true = extra points should be added
void SetAddNeighboursDistPoints(bool addNeighboursDistPoints) { m_addNeighboursDistPoints = addNeighboursDistPoints;}
//! Specifies wheter extra points should be added when computing the concavity
//! @return true = extra points should be added
bool GetAddNeighboursDistPoints() const { return m_addNeighboursDistPoints;}
//! Sets the points of the input mesh (Remark: the input points will be scaled and shifted. Use DenormalizeData() to invert those operations)
//! @param points pointer to the input points
void SetPoints(Vec3<Real> * points) { m_points = points;}
//! Gives the points of the input mesh (Remark: the input points will be scaled and shifted. Use DenormalizeData() to invert those operations)
//! @return pointer to the input points
const Vec3<Real> * GetPoints() const { return m_points;}
//! Sets the triangles of the input mesh.
//! @param triangles points pointer to the input points
void SetTriangles(Vec3<long> * triangles) { m_triangles = triangles;}
//! Gives the triangles in the input mesh
//! @return pointer to the input triangles
const Vec3<long> * GetTriangles() const { return m_triangles;}
//! Sets the number of points in the input mesh.
//! @param nPoints number of points the input mesh
void SetNPoints(size_t nPoints) { m_nPoints = nPoints;}
//! Gives the number of points in the input mesh.
//! @return number of points the input mesh
size_t GetNPoints() const { return m_nPoints;}
//! Sets the number of triangles in the input mesh.
//! @param nTriangles number of triangles in the input mesh
void SetNTriangles(size_t nTriangles) { m_nTriangles = nTriangles;}
//! Gives the number of triangles in the input mesh.
//! @return number of triangles the input mesh
size_t GetNTriangles() const { return m_nTriangles;}
//! Sets the minimum number of clusters to be generated.
//! @param nClusters minimum number of clusters
void SetNClusters(size_t nClusters) { m_nMinClusters = nClusters;}
//! Gives the number of generated clusters.
//! @return number of generated clusters
size_t GetNClusters() const { return m_nClusters;}
//! Sets the maximum allowed concavity.
//! @param concavity maximum concavity
void SetConcavity(double concavity) { m_concavity = concavity;}
//! Gives the maximum allowed concavity.
//! @return maximum concavity
double GetConcavity() const { return m_concavity;}
//! Sets the maximum allowed distance to get CCs connected.
//! @param concavity maximum distance to get CCs connected
void SetConnectDist(double ccConnectDist) { m_ccConnectDist = ccConnectDist;}
//! Gives the maximum allowed distance to get CCs connected.
//! @return maximum distance to get CCs connected
double GetConnectDist() const { return m_ccConnectDist;}
//! Sets the volume weight.
//! @param beta volume weight
void SetVolumeWeight(double beta) { m_beta = beta;}
//! Gives the volume weight.
//! @return volume weight
double GetVolumeWeight() const { return m_beta;}
//! Sets the compacity weight (i.e. parameter alpha in ftp://ftp.elet.polimi.it/users/Stefano.Tubaro/ICIP_USB_Proceedings_v2/pdfs/0003501.pdf).
//! @param alpha compacity weight
void SetCompacityWeight(double alpha) { m_alpha = alpha;}
//! Gives the compacity weight (i.e. parameter alpha in ftp://ftp.elet.polimi.it/users/Stefano.Tubaro/ICIP_USB_Proceedings_v2/pdfs/0003501.pdf).
//! @return compacity weight
double GetCompacityWeight() const { return m_alpha;}
//! Sets the maximum number of vertices for each generated convex-hull.
//! @param nVerticesPerCH maximum # vertices per CH
void SetNVerticesPerCH(size_t nVerticesPerCH) { m_nVerticesPerCH = nVerticesPerCH;}
//! Gives the maximum number of vertices for each generated convex-hull.
//! @return maximum # vertices per CH
size_t GetNVerticesPerCH() const { return m_nVerticesPerCH;}
//! Gives the number of vertices for the cluster number numCH.
//! @return number of vertices
size_t GetNPointsCH(size_t numCH) const;
//! Gives the number of triangles for the cluster number numCH.
//! @param numCH cluster's number
//! @return number of triangles
size_t GetNTrianglesCH(size_t numCH) const;
//! Gives the vertices and the triangles of the cluster number numCH.
//! @param numCH cluster's number
//! @param points pointer to the vector of points to be filled
//! @param triangles pointer to the vector of triangles to be filled
//! @return true if sucess
bool GetCH(size_t numCH, Vec3<Real> * const points, Vec3<long> * const triangles);
//! Computes the HACD decomposition.
//! @param fullCH specifies whether to generate convex-hulls with a full or limited (i.e. < m_nVerticesPerCH) number of vertices
//! @param exportDistPoints specifies wheter distance points should ne exported or not (used only for debugging).
//! @return true if sucess
bool Compute(bool fullCH=false, bool exportDistPoints=false);
//! Saves the generated convex-hulls in a VRML 2.0 file.
//! @param fileName the output file name
//! @param uniColor specifies whether the different convex-hulls should have the same color or not
//! @param numCluster specifies the cluster to be saved, if numCluster < 0 export all clusters
//! @return true if sucess
bool Save(const char * fileName, bool uniColor, long numCluster=-1) const;
//! Shifts and scales to the data to have all the coordinates between 0.0 and 1000.0.
void NormalizeData();
//! Inverse the operations applied by NormalizeData().
void DenormalizeData();
//! Constructor.
HACD(void);
//! Destructor.
~HACD(void);
private:
//! Gives the edge index.
//! @param a first vertex id
//! @param b second vertex id
//! @return edge's index
static unsigned long long GetEdgeIndex(unsigned long long a, unsigned long long b)
{
if (a > b) return (a << 32) + b;
else return (b << 32) + a;
}
//! Computes the concavity of a cluster.
//! @param ch the cluster's convex-hull
//! @param distPoints the cluster's points
//! @return cluster's concavity
double Concavity(ICHull & ch, std::map<long, DPoint> & distPoints);
//! Computes the perimeter of a cluster.
//! @param triIndices the cluster's triangles
//! @param distPoints the cluster's points
//! @return cluster's perimeter
double ComputePerimeter(const std::vector<long> & triIndices) const;
//! Creates the Graph by associating to each mesh triangle a vertex in the graph and to each couple of adjacent triangles an edge in the graph.
void CreateGraph();
//! Initializes the graph costs and computes the vertices normals
void InitializeDualGraph();
//! Computes the cost of an edge
//! @param e edge's id
void ComputeEdgeCost(size_t e);
//! Initializes the priority queue
//! @param fast specifies whether fast mode is used
//! @return true if success
bool InitializePriorityQueue();
//! Cleans the intersection between convex-hulls
void CleanClusters();
//! Computes convex-hulls from partition information
//! @param fullCH specifies whether to generate convex-hulls with a full or limited (i.e. < m_nVerticesPerCH) number of vertices
void ComputeConvexHulls(bool fullCH);
//! Simplifies the graph
//! @param fast specifies whether fast mode is used
void Simplify();
private:
double m_scale; //>! scale factor used for NormalizeData() and DenormalizeData()
Vec3<long> * m_triangles; //>! pointer the triangles array
Vec3<Real> * m_points; //>! pointer the points array
Vec3<Real> * m_facePoints; //>! pointer to the faces points array
Vec3<Real> * m_faceNormals; //>! pointer to the faces normals array
Vec3<Real> * m_normals; //>! pointer the normals array
size_t m_nTriangles; //>! number of triangles in the original mesh
size_t m_nPoints; //>! number of vertices in the original mesh
size_t m_nClusters; //>! number of clusters
size_t m_nMinClusters; //>! minimum number of clusters
double m_ccConnectDist; //>! maximum allowed distance to connect CCs
double m_concavity; //>! maximum concavity
double m_alpha; //>! compacity weigth
double m_beta; //>! volume weigth
double m_diag; //>! length of the BB diagonal
Vec3<Real> m_barycenter; //>! barycenter of the mesh
std::vector< long > m_cVertices; //>! array of vertices each belonging to a different cluster
ICHull * m_convexHulls; //>! convex-hulls associated with the final HACD clusters
Graph m_graph; //>! simplification graph
size_t m_nVerticesPerCH; //>! maximum number of vertices per convex-hull
reservable_priority_queue<GraphEdgePriorityQueue,
std::vector<GraphEdgePriorityQueue>,
std::greater<std::vector<GraphEdgePriorityQueue>::value_type> > m_pqueue; //!> priority queue
HACD(const HACD & rhs);
CallBackFunction m_callBack; //>! call-back function
long * m_partition; //>! array of size m_nTriangles where the i-th element specifies the cluster to which belong the i-th triangle
bool m_addFacesPoints; //>! specifies whether to add faces points or not
bool m_addExtraDistPoints; //>! specifies whether to add extra points for concave shapes or not
bool m_addNeighboursDistPoints; //>! specifies whether to add extra points from adjacent clusters or not
};
private:
long m_name; //!< edge name
Real m_priority; //!< priority
//! Operator < for GraphEdgePQ
friend bool operator<(const GraphEdgePriorityQueue &lhs, const GraphEdgePriorityQueue &rhs);
//! Operator > for GraphEdgePQ
friend bool operator>(const GraphEdgePriorityQueue &lhs, const GraphEdgePriorityQueue &rhs);
friend class HACD;
};
inline bool operator<(const GraphEdgePriorityQueue &lhs, const GraphEdgePriorityQueue &rhs)
{
return lhs.m_priority < rhs.m_priority;
}
inline bool operator>(const GraphEdgePriorityQueue &lhs, const GraphEdgePriorityQueue &rhs)
{
return lhs.m_priority > rhs.m_priority;
}
typedef bool (*CallBackFunction)(const char *, double, double, size_t);
//! Provides an implementation of the Hierarchical Approximate Convex Decomposition (HACD) technique described in "A Simple and Efficient Approach for 3D Mesh Approximate Convex Decomposition" Game Programming Gems 8 - Chapter 2.8, p.202. A short version of the chapter was published in ICIP09 and is available at ftp://ftp.elet.polimi.it/users/Stefano.Tubaro/ICIP_USB_Proceedings_v2/pdfs/0003501.pdf
class HACD
{
public:
//! Gives the triangles partitionas an array of size m_nTriangles where the i-th element specifies the cluster to which belong the i-th triangle
//! @return triangles partition
const long *GetPartition() const { return m_partition; }
//! Sets the scale factor
//! @param scale scale factor
void SetScaleFactor(double scale) { m_scale = scale; }
//! Gives the scale factor
//! @return scale factor
double GetScaleFactor() const { return m_scale; }
//! Sets the call-back function
//! @param callBack pointer to the call-back function
void SetCallBack(CallBackFunction callBack) { m_callBack = callBack; }
//! Gives the call-back function
//! @return pointer to the call-back function
CallBackFunction GetCallBack() const { return m_callBack; }
//! Specifies whether faces points should be added when computing the concavity
//! @param addFacesPoints true = faces points should be added
void SetAddFacesPoints(bool addFacesPoints) { m_addFacesPoints = addFacesPoints; }
//! Specifies wheter faces points should be added when computing the concavity
//! @return true = faces points should be added
bool GetAddFacesPoints() const { return m_addFacesPoints; }
//! Specifies whether extra points should be added when computing the concavity
//! @param addExteraDistPoints true = extra points should be added
void SetAddExtraDistPoints(bool addExtraDistPoints) { m_addExtraDistPoints = addExtraDistPoints; }
//! Specifies wheter extra points should be added when computing the concavity
//! @return true = extra points should be added
bool GetAddExtraDistPoints() const { return m_addExtraDistPoints; }
//! Specifies whether extra points should be added when computing the concavity
//! @param addExteraDistPoints true = extra points should be added
void SetAddNeighboursDistPoints(bool addNeighboursDistPoints) { m_addNeighboursDistPoints = addNeighboursDistPoints; }
//! Specifies wheter extra points should be added when computing the concavity
//! @return true = extra points should be added
bool GetAddNeighboursDistPoints() const { return m_addNeighboursDistPoints; }
//! Sets the points of the input mesh (Remark: the input points will be scaled and shifted. Use DenormalizeData() to invert those operations)
//! @param points pointer to the input points
void SetPoints(Vec3<Real> *points) { m_points = points; }
//! Gives the points of the input mesh (Remark: the input points will be scaled and shifted. Use DenormalizeData() to invert those operations)
//! @return pointer to the input points
const Vec3<Real> *GetPoints() const { return m_points; }
//! Sets the triangles of the input mesh.
//! @param triangles points pointer to the input points
void SetTriangles(Vec3<long> *triangles) { m_triangles = triangles; }
//! Gives the triangles in the input mesh
//! @return pointer to the input triangles
const Vec3<long> *GetTriangles() const { return m_triangles; }
//! Sets the number of points in the input mesh.
//! @param nPoints number of points the input mesh
void SetNPoints(size_t nPoints) { m_nPoints = nPoints; }
//! Gives the number of points in the input mesh.
//! @return number of points the input mesh
size_t GetNPoints() const { return m_nPoints; }
//! Sets the number of triangles in the input mesh.
//! @param nTriangles number of triangles in the input mesh
void SetNTriangles(size_t nTriangles) { m_nTriangles = nTriangles; }
//! Gives the number of triangles in the input mesh.
//! @return number of triangles the input mesh
size_t GetNTriangles() const { return m_nTriangles; }
//! Sets the minimum number of clusters to be generated.
//! @param nClusters minimum number of clusters
void SetNClusters(size_t nClusters) { m_nMinClusters = nClusters; }
//! Gives the number of generated clusters.
//! @return number of generated clusters
size_t GetNClusters() const { return m_nClusters; }
//! Sets the maximum allowed concavity.
//! @param concavity maximum concavity
void SetConcavity(double concavity) { m_concavity = concavity; }
//! Gives the maximum allowed concavity.
//! @return maximum concavity
double GetConcavity() const { return m_concavity; }
//! Sets the maximum allowed distance to get CCs connected.
//! @param concavity maximum distance to get CCs connected
void SetConnectDist(double ccConnectDist) { m_ccConnectDist = ccConnectDist; }
//! Gives the maximum allowed distance to get CCs connected.
//! @return maximum distance to get CCs connected
double GetConnectDist() const { return m_ccConnectDist; }
//! Sets the volume weight.
//! @param beta volume weight
void SetVolumeWeight(double beta) { m_beta = beta; }
//! Gives the volume weight.
//! @return volume weight
double GetVolumeWeight() const { return m_beta; }
//! Sets the compacity weight (i.e. parameter alpha in ftp://ftp.elet.polimi.it/users/Stefano.Tubaro/ICIP_USB_Proceedings_v2/pdfs/0003501.pdf).
//! @param alpha compacity weight
void SetCompacityWeight(double alpha) { m_alpha = alpha; }
//! Gives the compacity weight (i.e. parameter alpha in ftp://ftp.elet.polimi.it/users/Stefano.Tubaro/ICIP_USB_Proceedings_v2/pdfs/0003501.pdf).
//! @return compacity weight
double GetCompacityWeight() const { return m_alpha; }
//! Sets the maximum number of vertices for each generated convex-hull.
//! @param nVerticesPerCH maximum # vertices per CH
void SetNVerticesPerCH(size_t nVerticesPerCH) { m_nVerticesPerCH = nVerticesPerCH; }
//! Gives the maximum number of vertices for each generated convex-hull.
//! @return maximum # vertices per CH
size_t GetNVerticesPerCH() const { return m_nVerticesPerCH; }
//! Gives the number of vertices for the cluster number numCH.
//! @return number of vertices
size_t GetNPointsCH(size_t numCH) const;
//! Gives the number of triangles for the cluster number numCH.
//! @param numCH cluster's number
//! @return number of triangles
size_t GetNTrianglesCH(size_t numCH) const;
//! Gives the vertices and the triangles of the cluster number numCH.
//! @param numCH cluster's number
//! @param points pointer to the vector of points to be filled
//! @param triangles pointer to the vector of triangles to be filled
//! @return true if sucess
bool GetCH(size_t numCH, Vec3<Real> *const points, Vec3<long> *const triangles);
//! Computes the HACD decomposition.
//! @param fullCH specifies whether to generate convex-hulls with a full or limited (i.e. < m_nVerticesPerCH) number of vertices
//! @param exportDistPoints specifies wheter distance points should ne exported or not (used only for debugging).
//! @return true if sucess
bool Compute(bool fullCH = false, bool exportDistPoints = false);
//! Saves the generated convex-hulls in a VRML 2.0 file.
//! @param fileName the output file name
//! @param uniColor specifies whether the different convex-hulls should have the same color or not
//! @param numCluster specifies the cluster to be saved, if numCluster < 0 export all clusters
//! @return true if sucess
bool Save(const char *fileName, bool uniColor, long numCluster = -1) const;
//! Shifts and scales to the data to have all the coordinates between 0.0 and 1000.0.
void NormalizeData();
//! Inverse the operations applied by NormalizeData().
void DenormalizeData();
//! Constructor.
HACD(void);
//! Destructor.
~HACD(void);
private:
//! Gives the edge index.
//! @param a first vertex id
//! @param b second vertex id
//! @return edge's index
static unsigned long long GetEdgeIndex(unsigned long long a, unsigned long long b)
{
if (a > b)
return (a << 32) + b;
else
return (b << 32) + a;
}
//! Computes the concavity of a cluster.
//! @param ch the cluster's convex-hull
//! @param distPoints the cluster's points
//! @return cluster's concavity
double Concavity(ICHull &ch, std::map<long, DPoint> &distPoints);
//! Computes the perimeter of a cluster.
//! @param triIndices the cluster's triangles
//! @param distPoints the cluster's points
//! @return cluster's perimeter
double ComputePerimeter(const std::vector<long> &triIndices) const;
//! Creates the Graph by associating to each mesh triangle a vertex in the graph and to each couple of adjacent triangles an edge in the graph.
void CreateGraph();
//! Initializes the graph costs and computes the vertices normals
void InitializeDualGraph();
//! Computes the cost of an edge
//! @param e edge's id
void ComputeEdgeCost(size_t e);
//! Initializes the priority queue
//! @param fast specifies whether fast mode is used
//! @return true if success
bool InitializePriorityQueue();
//! Cleans the intersection between convex-hulls
void CleanClusters();
//! Computes convex-hulls from partition information
//! @param fullCH specifies whether to generate convex-hulls with a full or limited (i.e. < m_nVerticesPerCH) number of vertices
void ComputeConvexHulls(bool fullCH);
//! Simplifies the graph
//! @param fast specifies whether fast mode is used
void Simplify();
private:
double m_scale; //>! scale factor used for NormalizeData() and DenormalizeData()
Vec3<long> *m_triangles; //>! pointer the triangles array
Vec3<Real> *m_points; //>! pointer the points array
Vec3<Real> *m_facePoints; //>! pointer to the faces points array
Vec3<Real> *m_faceNormals; //>! pointer to the faces normals array
Vec3<Real> *m_normals; //>! pointer the normals array
size_t m_nTriangles; //>! number of triangles in the original mesh
size_t m_nPoints; //>! number of vertices in the original mesh
size_t m_nClusters; //>! number of clusters
size_t m_nMinClusters; //>! minimum number of clusters
double m_ccConnectDist; //>! maximum allowed distance to connect CCs
double m_concavity; //>! maximum concavity
double m_alpha; //>! compacity weigth
double m_beta; //>! volume weigth
double m_diag; //>! length of the BB diagonal
Vec3<Real> m_barycenter; //>! barycenter of the mesh
std::vector<long> m_cVertices; //>! array of vertices each belonging to a different cluster
ICHull *m_convexHulls; //>! convex-hulls associated with the final HACD clusters
Graph m_graph; //>! simplification graph
size_t m_nVerticesPerCH; //>! maximum number of vertices per convex-hull
reservable_priority_queue<GraphEdgePriorityQueue,
std::vector<GraphEdgePriorityQueue>,
std::greater<std::vector<GraphEdgePriorityQueue>::value_type> >
m_pqueue; //!> priority queue
HACD(const HACD &rhs);
CallBackFunction m_callBack; //>! call-back function
long *m_partition; //>! array of size m_nTriangles where the i-th element specifies the cluster to which belong the i-th triangle
bool m_addFacesPoints; //>! specifies whether to add faces points or not
bool m_addExtraDistPoints; //>! specifies whether to add extra points for concave shapes or not
bool m_addNeighboursDistPoints; //>! specifies whether to add extra points from adjacent clusters or not
};
} // namespace HACD
#endif

1895
Extras/HACD/hacdICHull.cpp
View File

File diff suppressed because it is too large Load Diff

184
Extras/HACD/hacdICHull.h
View File

@ -22,99 +22,99 @@
#include <map>
namespace HACD
{
class DPoint;
class HACD;
//! Incremental Convex Hull algorithm (cf. http://maven.smith.edu/~orourke/books/ftp.html ).
enum ICHullError
{
ICHullErrorOK = 0,
ICHullErrorCoplanarPoints,
ICHullErrorNoVolume,
ICHullErrorInconsistent,
ICHullErrorNotEnoughPoints
};
class ICHull
{
public:
//!
bool IsFlat() { return m_isFlat;}
//!
std::map<long, DPoint> * GetDistPoints() const { return m_distPoints;}
//!
void SetDistPoints(std::map<long, DPoint> * distPoints) { m_distPoints = distPoints;}
//! Returns the computed mesh
TMMesh & GetMesh() { return m_mesh;}
//! Add one point to the convex-hull
bool AddPoint(const Vec3<Real> & point) {return AddPoints(&point, 1);}
//! Add one point to the convex-hull
bool AddPoint(const Vec3<Real> & point, long id);
//! Add points to the convex-hull
bool AddPoints(const Vec3<Real> * points, size_t nPoints);
bool AddPoints(std::vector< Vec3<Real> > points);
//!
ICHullError Process();
//!
ICHullError Process(unsigned long nPointsCH);
//!
double ComputeVolume();
//!
bool IsInside(const Vec3<Real> & pt0);
//!
double ComputeDistance(long name, const Vec3<Real> & pt, const Vec3<Real> & normal, bool & insideHull, bool updateIncidentPoints);
//!
const ICHull & operator=(ICHull & rhs);
class DPoint;
class HACD;
//! Incremental Convex Hull algorithm (cf. http://maven.smith.edu/~orourke/books/ftp.html ).
enum ICHullError
{
ICHullErrorOK = 0,
ICHullErrorCoplanarPoints,
ICHullErrorNoVolume,
ICHullErrorInconsistent,
ICHullErrorNotEnoughPoints
};
class ICHull
{
public:
//!
bool IsFlat() { return m_isFlat; }
//!
std::map<long, DPoint> *GetDistPoints() const { return m_distPoints; }
//!
void SetDistPoints(std::map<long, DPoint> *distPoints) { m_distPoints = distPoints; }
//! Returns the computed mesh
TMMesh &GetMesh() { return m_mesh; }
//! Add one point to the convex-hull
bool AddPoint(const Vec3<Real> &point) { return AddPoints(&point, 1); }
//! Add one point to the convex-hull
bool AddPoint(const Vec3<Real> &point, long id);
//! Add points to the convex-hull
bool AddPoints(const Vec3<Real> *points, size_t nPoints);
bool AddPoints(std::vector<Vec3<Real> > points);
//!
ICHullError Process();
//!
ICHullError Process(unsigned long nPointsCH);
//!
double ComputeVolume();
//!
bool IsInside(const Vec3<Real> &pt0);
//!
double ComputeDistance(long name, const Vec3<Real> &pt, const Vec3<Real> &normal, bool &insideHull, bool updateIncidentPoints);
//!
const ICHull &operator=(ICHull &rhs);
//! Constructor
ICHull(void);
//! Destructor
virtual ~ICHull(void) {};
//! Constructor
ICHull(void);
//! Destructor
virtual ~ICHull(void){};
private:
//! DoubleTriangle builds the initial double triangle. It first finds 3 noncollinear points and makes two faces out of them, in opposite order. It then finds a fourth point that is not coplanar with that face. The vertices are stored in the face structure in counterclockwise order so that the volume between the face and the point is negative. Lastly, the 3 newfaces to the fourth point are constructed and the data structures are cleaned up.
ICHullError DoubleTriangle();
//! MakeFace creates a new face structure from three vertices (in ccw order). It returns a pointer to the face.
CircularListElement<TMMTriangle> * MakeFace(CircularListElement<TMMVertex> * v0,
CircularListElement<TMMVertex> * v1,
CircularListElement<TMMVertex> * v2,
CircularListElement<TMMTriangle> * fold);
//!
CircularListElement<TMMTriangle> * MakeConeFace(CircularListElement<TMMEdge> * e, CircularListElement<TMMVertex> * v);
//!
bool ProcessPoint();
//!
bool ComputePointVolume(double &totalVolume, bool markVisibleFaces);
//!
bool FindMaxVolumePoint();
//!
bool CleanEdges();
//!
bool CleanVertices(unsigned long & addedPoints);
//!
bool CleanTriangles();
//!
bool CleanUp(unsigned long & addedPoints);
//!
bool MakeCCW(CircularListElement<TMMTriangle> * f,
CircularListElement<TMMEdge> * e,
CircularListElement<TMMVertex> * v);
void Clear();
private:
static const long sc_dummyIndex;
static const double sc_distMin;
TMMesh m_mesh;
std::vector<CircularListElement<TMMEdge> *> m_edgesToDelete;
std::vector<CircularListElement<TMMEdge> *> m_edgesToUpdate;
std::vector<CircularListElement<TMMTriangle> *> m_trianglesToDelete;
std::map<long, DPoint> * m_distPoints;
CircularListElement<TMMVertex> * m_dummyVertex;
Vec3<Real> m_normal;
bool m_isFlat;
ICHull(const ICHull & rhs);
friend class HACD;
};
private:
//! DoubleTriangle builds the initial double triangle. It first finds 3 noncollinear points and makes two faces out of them, in opposite order. It then finds a fourth point that is not coplanar with that face. The vertices are stored in the face structure in counterclockwise order so that the volume between the face and the point is negative. Lastly, the 3 newfaces to the fourth point are constructed and the data structures are cleaned up.
ICHullError DoubleTriangle();
//! MakeFace creates a new face structure from three vertices (in ccw order). It returns a pointer to the face.
CircularListElement<TMMTriangle> *MakeFace(CircularListElement<TMMVertex> *v0,
CircularListElement<TMMVertex> *v1,
CircularListElement<TMMVertex> *v2,
CircularListElement<TMMTriangle> *fold);
//!
CircularListElement<TMMTriangle> *MakeConeFace(CircularListElement<TMMEdge> *e, CircularListElement<TMMVertex> *v);
//!
bool ProcessPoint();
//!
bool ComputePointVolume(double &totalVolume, bool markVisibleFaces);
//!
bool FindMaxVolumePoint();
//!
bool CleanEdges();
//!
bool CleanVertices(unsigned long &addedPoints);
//!
bool CleanTriangles();
//!
bool CleanUp(unsigned long &addedPoints);
//!
bool MakeCCW(CircularListElement<TMMTriangle> *f,
CircularListElement<TMMEdge> *e,
CircularListElement<TMMVertex> *v);
void Clear();
}
private:
static const long sc_dummyIndex;
static const double sc_distMin;
TMMesh m_mesh;
std::vector<CircularListElement<TMMEdge> *> m_edgesToDelete;
std::vector<CircularListElement<TMMEdge> *> m_edgesToUpdate;
std::vector<CircularListElement<TMMTriangle> *> m_trianglesToDelete;
std::map<long, DPoint> *m_distPoints;
CircularListElement<TMMVertex> *m_dummyVertex;
Vec3<Real> m_normal;
bool m_isFlat;
ICHull(const ICHull &rhs);
friend class HACD;
};
} // namespace HACD
#endif

1071
Extras/HACD/hacdManifoldMesh.cpp
View File

File diff suppressed because it is too large Load Diff

383
Extras/HACD/hacdManifoldMesh.h
View File

@ -38,213 +38,214 @@ All rights reserved.
#include <set>
namespace HACD
{
class TMMTriangle;
class TMMEdge;
class TMMesh;
class ICHull;
class HACD;
class TMMTriangle;
class TMMEdge;
class TMMesh;
class ICHull;
class HACD;
class DPoint
{
public:
DPoint(Real dist=0, bool computed=false, bool distOnly=false)
:m_dist(dist),
m_computed(computed),
m_distOnly(distOnly){};
~DPoint(){};
private:
Real m_dist;
bool m_computed;
bool m_distOnly;
friend class TMMTriangle;
friend class TMMesh;
friend class GraphVertex;
friend class GraphEdge;
friend class Graph;
friend class ICHull;
friend class HACD;
};
class DPoint
{
public:
DPoint(Real dist = 0, bool computed = false, bool distOnly = false)
: m_dist(dist),
m_computed(computed),
m_distOnly(distOnly){};
~DPoint(){};
//! Vertex data structure used in a triangular manifold mesh (TMM).
class TMMVertex
{
public:
TMMVertex(void);
~TMMVertex(void);
private:
Vec3<Real> m_pos;
long m_name;
size_t m_id;
CircularListElement<TMMEdge> * m_duplicate; // pointer to incident cone edge (or NULL)
bool m_onHull;
bool m_tag;
TMMVertex(const TMMVertex & rhs);
friend class HACD;
friend class ICHull;
friend class TMMesh;
friend class TMMTriangle;
friend class TMMEdge;
};
private:
Real m_dist;
bool m_computed;
bool m_distOnly;
friend class TMMTriangle;
friend class TMMesh;
friend class GraphVertex;
friend class GraphEdge;
friend class Graph;
friend class ICHull;
friend class HACD;
};
//! Edge data structure used in a triangular manifold mesh (TMM).
class TMMEdge
{
public:
TMMEdge(void);
~TMMEdge(void);
private:
size_t m_id;
CircularListElement<TMMTriangle> * m_triangles[2];
CircularListElement<TMMVertex> * m_vertices[2];
CircularListElement<TMMTriangle> * m_newFace;
//! Vertex data structure used in a triangular manifold mesh (TMM).
class TMMVertex
{
public:
TMMVertex(void);
~TMMVertex(void);
private:
Vec3<Real> m_pos;
long m_name;
size_t m_id;
CircularListElement<TMMEdge> *m_duplicate; // pointer to incident cone edge (or NULL)
bool m_onHull;
bool m_tag;
TMMVertex(const TMMVertex &rhs);
TMMEdge(const TMMEdge & rhs);
friend class HACD;
friend class ICHull;
friend class TMMTriangle;
friend class TMMVertex;
friend class TMMesh;
};
friend class HACD;
friend class ICHull;
friend class TMMesh;
friend class TMMTriangle;
friend class TMMEdge;
};
//! Triangle data structure used in a triangular manifold mesh (TMM).
class TMMTriangle
{
public:
TMMTriangle(void);
~TMMTriangle(void);
private:
size_t m_id;
CircularListElement<TMMEdge> * m_edges[3];
CircularListElement<TMMVertex> * m_vertices[3];
std::set<long> m_incidentPoints;
bool m_visible;
TMMTriangle(const TMMTriangle & rhs);
friend class HACD;
friend class ICHull;
friend class TMMesh;
friend class TMMVertex;
friend class TMMEdge;
};
class Material
{
public:
Material(void);
~Material(void){}
// private:
Vec3<double> m_diffuseColor;
double m_ambientIntensity;
Vec3<double> m_specularColor;
Vec3<double> m_emissiveColor;
double m_shininess;
double m_transparency;
friend class TMMesh;
friend class HACD;
};
//! Edge data structure used in a triangular manifold mesh (TMM).
class TMMEdge
{
public:
TMMEdge(void);
~TMMEdge(void);
//! triangular manifold mesh data structure.
class TMMesh
{
public:
private:
size_t m_id;
CircularListElement<TMMTriangle> *m_triangles[2];
CircularListElement<TMMVertex> *m_vertices[2];
CircularListElement<TMMTriangle> *m_newFace;
//! Returns the number of vertices>
inline size_t GetNVertices() const { return m_vertices.GetSize();}
//! Returns the number of edges
inline size_t GetNEdges() const { return m_edges.GetSize();}
//! Returns the number of triangles
inline size_t GetNTriangles() const { return m_triangles.GetSize();}
//! Returns the vertices circular list
inline const CircularList<TMMVertex> & GetVertices() const { return m_vertices;}
//! Returns the edges circular list
inline const CircularList<TMMEdge> & GetEdges() const { return m_edges;}
//! Returns the triangles circular list
inline const CircularList<TMMTriangle> & GetTriangles() const { return m_triangles;}
//! Returns the vertices circular list
inline CircularList<TMMVertex> & GetVertices() { return m_vertices;}
//! Returns the edges circular list
inline CircularList<TMMEdge> & GetEdges() { return m_edges;}
//! Returns the triangles circular list
inline CircularList<TMMTriangle> & GetTriangles() { return m_triangles;}
//! Add vertex to the mesh
CircularListElement<TMMVertex> * AddVertex() {return m_vertices.Add();}
//! Add vertex to the mesh
CircularListElement<TMMEdge> * AddEdge() {return m_edges.Add();}
//! Add vertex to the mesh
CircularListElement<TMMTriangle> * AddTriangle() {return m_triangles.Add();}
//! Print mesh information
void Print();
//!
void GetIFS(Vec3<Real> * const points, Vec3<long> * const triangles);
//! Save mesh
bool Save(const char *fileName);
//! Save mesh to VRML 2.0 format
bool SaveVRML2(std::ofstream &fout);
//! Save mesh to VRML 2.0 format
bool SaveVRML2(std::ofstream &fout, const Material & material);
//!
void Clear();
//!
void Copy(TMMesh & mesh);
//!
bool CheckConsistancy();
//!
bool Normalize();
//!
bool Denormalize();
//! Constructor
TMMesh(void);
//! Destructor
virtual ~TMMesh(void);
TMMEdge(const TMMEdge &rhs);
private:
CircularList<TMMVertex> m_vertices;
CircularList<TMMEdge> m_edges;
CircularList<TMMTriangle> m_triangles;
Real m_diag; //>! length of the BB diagonal
Vec3<Real> m_barycenter; //>! barycenter of the mesh
friend class HACD;
friend class ICHull;
friend class TMMTriangle;
friend class TMMVertex;
friend class TMMesh;
};
// not defined
TMMesh(const TMMesh & rhs);
friend class ICHull;
friend class HACD;
};
//! IntersectRayTriangle(): intersect a ray with a 3D triangle
//! Input: a ray R, and a triangle T
//! Output: *I = intersection point (when it exists)
//! 0 = disjoint (no intersect)
//! 1 = intersect in unique point I1
long IntersectRayTriangle( const Vec3<double> & P0, const Vec3<double> & dir,
const Vec3<double> & V0, const Vec3<double> & V1,
const Vec3<double> & V2, double &t);
//! Triangle data structure used in a triangular manifold mesh (TMM).
class TMMTriangle
{
public:
TMMTriangle(void);
~TMMTriangle(void);
// intersect_RayTriangle(): intersect a ray with a 3D triangle
// Input: a ray R, and a triangle T
// Output: *I = intersection point (when it exists)
// Return: -1 = triangle is degenerate (a segment or point)
// 0 = disjoint (no intersect)
// 1 = intersect in unique point I1
// 2 = are in the same plane
long IntersectRayTriangle2(const Vec3<double> & P0, const Vec3<double> & dir,
const Vec3<double> & V0, const Vec3<double> & V1,
const Vec3<double> & V2, double &r);
private:
size_t m_id;
CircularListElement<TMMEdge> *m_edges[3];
CircularListElement<TMMVertex> *m_vertices[3];
std::set<long> m_incidentPoints;
bool m_visible;
/*
TMMTriangle(const TMMTriangle &rhs);
friend class HACD;
friend class ICHull;
friend class TMMesh;
friend class TMMVertex;
friend class TMMEdge;
};
class Material
{
public:
Material(void);
~Material(void) {}
// private:
Vec3<double> m_diffuseColor;
double m_ambientIntensity;
Vec3<double> m_specularColor;
Vec3<double> m_emissiveColor;
double m_shininess;
double m_transparency;
friend class TMMesh;
friend class HACD;
};
//! triangular manifold mesh data structure.
class TMMesh
{
public:
//! Returns the number of vertices>
inline size_t GetNVertices() const { return m_vertices.GetSize(); }
//! Returns the number of edges
inline size_t GetNEdges() const { return m_edges.GetSize(); }
//! Returns the number of triangles
inline size_t GetNTriangles() const { return m_triangles.GetSize(); }
//! Returns the vertices circular list
inline const CircularList<TMMVertex> &GetVertices() const { return m_vertices; }
//! Returns the edges circular list
inline const CircularList<TMMEdge> &GetEdges() const { return m_edges; }
//! Returns the triangles circular list
inline const CircularList<TMMTriangle> &GetTriangles() const { return m_triangles; }
//! Returns the vertices circular list
inline CircularList<TMMVertex> &GetVertices() { return m_vertices; }
//! Returns the edges circular list
inline CircularList<TMMEdge> &GetEdges() { return m_edges; }
//! Returns the triangles circular list
inline CircularList<TMMTriangle> &GetTriangles() { return m_triangles; }
//! Add vertex to the mesh
CircularListElement<TMMVertex> *AddVertex() { return m_vertices.Add(); }
//! Add vertex to the mesh
CircularListElement<TMMEdge> *AddEdge() { return m_edges.Add(); }
//! Add vertex to the mesh
CircularListElement<TMMTriangle> *AddTriangle() { return m_triangles.Add(); }
//! Print mesh information
void Print();
//!
void GetIFS(Vec3<Real> *const points, Vec3<long> *const triangles);
//! Save mesh
bool Save(const char *fileName);
//! Save mesh to VRML 2.0 format
bool SaveVRML2(std::ofstream &fout);
//! Save mesh to VRML 2.0 format
bool SaveVRML2(std::ofstream &fout, const Material &material);
//!
void Clear();
//!
void Copy(TMMesh &mesh);
//!
bool CheckConsistancy();
//!
bool Normalize();
//!
bool Denormalize();
//! Constructor
TMMesh(void);
//! Destructor
virtual ~TMMesh(void);
private:
CircularList<TMMVertex> m_vertices;
CircularList<TMMEdge> m_edges;
CircularList<TMMTriangle> m_triangles;
Real m_diag; //>! length of the BB diagonal
Vec3<Real> m_barycenter; //>! barycenter of the mesh
// not defined
TMMesh(const TMMesh &rhs);
friend class ICHull;
friend class HACD;
};
//! IntersectRayTriangle(): intersect a ray with a 3D triangle
//! Input: a ray R, and a triangle T
//! Output: *I = intersection point (when it exists)
//! 0 = disjoint (no intersect)
//! 1 = intersect in unique point I1
long IntersectRayTriangle(const Vec3<double> &P0, const Vec3<double> &dir,
const Vec3<double> &V0, const Vec3<double> &V1,
const Vec3<double> &V2, double &t);
// intersect_RayTriangle(): intersect a ray with a 3D triangle
// Input: a ray R, and a triangle T
// Output: *I = intersection point (when it exists)
// Return: -1 = triangle is degenerate (a segment or point)
// 0 = disjoint (no intersect)
// 1 = intersect in unique point I1
// 2 = are in the same plane
long IntersectRayTriangle2(const Vec3<double> &P0, const Vec3<double> &dir,
const Vec3<double> &V0, const Vec3<double> &V1,
const Vec3<double> &V2, double &r);
/*
Calculate the line segment PaPb that is the shortest route between
two lines P1P2 and P3P4. Calculate also the values of mua and mub where
Pa = P1 + mua (P2 - P1)
Pb = P3 + mub (P4 - P3)
Return FALSE if no solution exists.
*/
bool IntersectLineLine(const Vec3<double> & p1, const Vec3<double> & p2,
const Vec3<double> & p3, const Vec3<double> & p4,
Vec3<double> & pa, Vec3<double> & pb,
double & mua, double &mub);
}
bool IntersectLineLine(const Vec3<double> &p1, const Vec3<double> &p2,
const Vec3<double> &p3, const Vec3<double> &p4,
Vec3<double> &pa, Vec3<double> &pb,
double &mua, double &mub);
} // namespace HACD
#endif

89
Extras/HACD/hacdVector.h
View File

@ -15,53 +15,54 @@
#pragma once
#ifndef HACD_VECTOR_H
#define HACD_VECTOR_H
#include<math.h>
#include<iostream>
#include <math.h>
#include <iostream>
#include "hacdVersion.h"
namespace HACD
{
typedef double Real;
//! Vector dim 3.
template < typename T > class Vec3
{
public:
T & X();
T & Y();
T & Z();
const T & X() const;
const T & Y() const;
const T & Z() const;
void Normalize();
T GetNorm() const;
void operator= (const Vec3 & rhs);
void operator+=(const Vec3 & rhs);
void operator-=(const Vec3 & rhs);
void operator-=(T a);
void operator+=(T a);
void operator/=(T a);
void operator*=(T a);
Vec3 operator^ (const Vec3 & rhs) const;
T operator* (const Vec3 & rhs) const;
Vec3 operator+ (const Vec3 & rhs) const;
Vec3 operator- (const Vec3 & rhs) const;
Vec3 operator- () const;
Vec3 operator* (T rhs) const;
Vec3 operator/ (T rhs) const;
Vec3();
Vec3(T a);
Vec3(T x, T y, T z);
Vec3(const Vec3 & rhs);
/*virtual*/ ~Vec3(void);
typedef double Real;
//! Vector dim 3.
template <typename T>
class Vec3
{
public:
T& X();
T& Y();
T& Z();
const T& X() const;
const T& Y() const;
const T& Z() const;
void Normalize();
T GetNorm() const;
void operator=(const Vec3& rhs);
void operator+=(const Vec3& rhs);
void operator-=(const Vec3& rhs);
void operator-=(T a);
void operator+=(T a);
void operator/=(T a);
void operator*=(T a);
Vec3 operator^(const Vec3& rhs) const;
T operator*(const Vec3& rhs) const;
Vec3 operator+(const Vec3& rhs) const;
Vec3 operator-(const Vec3& rhs) const;
Vec3 operator-() const;
Vec3 operator*(T rhs) const;
Vec3 operator/(T rhs) const;
Vec3();
Vec3(T a);
Vec3(T x, T y, T z);
Vec3(const Vec3& rhs);
/*virtual*/ ~Vec3(void);
private:
T m_data[3];
};
template<typename T>
bool Colinear(const Vec3<T> & a, const Vec3<T> & b, const Vec3<T> & c);
template<typename T>
const T Volume(const Vec3<T> & a, const Vec3<T> & b, const Vec3<T> & c, const Vec3<T> & d);
}
#include "hacdVector.inl" // template implementation
private:
T m_data[3];
};
template <typename T>
bool Colinear(const Vec3<T>& a, const Vec3<T>& b, const Vec3<T>& c);
template <typename T>
const T Volume(const Vec3<T>& a, const Vec3<T>& b, const Vec3<T>& c, const Vec3<T>& d);
} // namespace HACD
#include "hacdVector.inl" // template implementation
#endif

4
Extras/InverseDynamics/BulletInverseDynamicsUtilsCommon.h
View File

@ -12,6 +12,4 @@
#include "MultiBodyNameMap.hpp"
#include "User2InternalIndex.hpp"
#endif//BULLET_INVERSE_DYNAMICS_UTILS_COMMON_H
#endif //BULLET_INVERSE_DYNAMICS_UTILS_COMMON_H

82
Extras/InverseDynamics/CloneTreeCreator.cpp
View File

@ -2,48 +2,58 @@
#include <cstdio>
namespace btInverseDynamics {
#define CHECK_NULLPTR() \
do { \
if (m_reference == 0x0) { \
bt_id_error_message("m_reference == 0x0\n"); \
return -1; \
} \
} while (0)
namespace btInverseDynamics
{
#define CHECK_NULLPTR() \
do \
{ \
if (m_reference == 0x0) \
{ \
bt_id_error_message("m_reference == 0x0\n"); \
return -1; \
} \
} while (0)
#define TRY(x) \
do { \
if (x == -1) { \
bt_id_error_message("error calling " #x "\n"); \
return -1; \
} \
} while (0)
CloneTreeCreator::CloneTreeCreator(const MultiBodyTree* reference) { m_reference = reference; }
#define TRY(x) \
do \
{ \
if (x == -1) \
{ \
bt_id_error_message("error calling " #x "\n"); \
return -1; \
} \
} while (0)
CloneTreeCreator::CloneTreeCreator(const MultiBodyTree* reference)
{
m_reference = reference;
}
CloneTreeCreator::~CloneTreeCreator() {}
int CloneTreeCreator::getNumBodies(int* num_bodies) const {
CHECK_NULLPTR();
*num_bodies = m_reference->numBodies();
return 0;
int CloneTreeCreator::getNumBodies(int* num_bodies) const
{
CHECK_NULLPTR();
*num_bodies = m_reference->numBodies();
return 0;
}
int CloneTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const {
CHECK_NULLPTR();
TRY(m_reference->getParentIndex(body_index, parent_index));
TRY(m_reference->getJointType(body_index, joint_type));
TRY(m_reference->getParentRParentBodyRef(body_index, parent_r_parent_body_ref));
TRY(m_reference->getBodyTParentRef(body_index, body_T_parent_ref));
TRY(m_reference->getBodyAxisOfMotion(body_index, body_axis_of_motion));
TRY(m_reference->getBodyMass(body_index, mass));
TRY(m_reference->getBodyFirstMassMoment(body_index, body_r_body_com));
TRY(m_reference->getBodySecondMassMoment(body_index, body_I_body));
TRY(m_reference->getUserInt(body_index, user_int));
TRY(m_reference->getUserPtr(body_index, user_ptr));
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const
{
CHECK_NULLPTR();
TRY(m_reference->getParentIndex(body_index, parent_index));
TRY(m_reference->getJointType(body_index, joint_type));
TRY(m_reference->getParentRParentBodyRef(body_index, parent_r_parent_body_ref));
TRY(m_reference->getBodyTParentRef(body_index, body_T_parent_ref));
TRY(m_reference->getBodyAxisOfMotion(body_index, body_axis_of_motion));
TRY(m_reference->getBodyMass(body_index, mass));
TRY(m_reference->getBodyFirstMassMoment(body_index, body_r_body_com));
TRY(m_reference->getBodySecondMassMoment(body_index, body_I_body));
TRY(m_reference->getUserInt(body_index, user_int));
TRY(m_reference->getUserPtr(body_index, user_ptr));
return 0;
}
return 0;
}
} // namespace btInverseDynamics

32
Extras/InverseDynamics/CloneTreeCreator.hpp
View File

@ -4,24 +4,26 @@
#include "BulletInverseDynamics/IDConfig.hpp"
#include "MultiBodyTreeCreator.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// Generate an identical multibody tree from a reference system.
class CloneTreeCreator : public MultiBodyTreeCreator {
class CloneTreeCreator : public MultiBodyTreeCreator
{
public:
/// ctor
/// @param reference the MultiBodyTree to clone
CloneTreeCreator(const MultiBodyTree*reference);
~CloneTreeCreator();
///\copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
void** user_ptr) const;
/// ctor
/// @param reference the MultiBodyTree to clone
CloneTreeCreator(const MultiBodyTree* reference);
~CloneTreeCreator();
///\copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
void** user_ptr) const;
private:
const MultiBodyTree *m_reference;
const MultiBodyTree* m_reference;
};
}
} // namespace btInverseDynamics
#endif // CLONETREE_CREATOR_HPP_

108
Extras/InverseDynamics/CoilCreator.cpp
View File

@ -2,66 +2,72 @@
#include "CoilCreator.hpp"
namespace btInverseDynamics {
CoilCreator::CoilCreator(int n) : m_num_bodies(n), m_parent(n) {
for (int i = 0; i < m_num_bodies; i++) {
m_parent[i] = i - 1;
}
namespace btInverseDynamics
{
CoilCreator::CoilCreator(int n) : m_num_bodies(n), m_parent(n)
{
for (int i = 0; i < m_num_bodies; i++)
{
m_parent[i] = i - 1;
}
// DH parameters (that's what's in the paper ...)
const idScalar theta_DH = 0;
const idScalar d_DH = 0.0;
const idScalar a_DH = 1.0 / m_num_bodies;
const idScalar alpha_DH = 5.0 * BT_ID_PI / m_num_bodies;
getVecMatFromDH(theta_DH, d_DH, a_DH, alpha_DH, &m_parent_r_parent_body_ref,
&m_body_T_parent_ref);
// always z-axis
m_body_axis_of_motion(0) = 0.0;
m_body_axis_of_motion(1) = 0.0;
m_body_axis_of_motion(2) = 1.0;
// DH parameters (that's what's in the paper ...)
const idScalar theta_DH = 0;
const idScalar d_DH = 0.0;
const idScalar a_DH = 1.0 / m_num_bodies;
const idScalar alpha_DH = 5.0 * BT_ID_PI / m_num_bodies;
getVecMatFromDH(theta_DH, d_DH, a_DH, alpha_DH, &m_parent_r_parent_body_ref,
&m_body_T_parent_ref);
// always z-axis
m_body_axis_of_motion(0) = 0.0;
m_body_axis_of_motion(1) = 0.0;
m_body_axis_of_motion(2) = 1.0;
m_mass = 1.0 / m_num_bodies;
m_body_r_body_com(0) = 1.0 / (2.0 * m_num_bodies);
m_body_r_body_com(1) = 0.0;
m_body_r_body_com(2) = 0.0;
m_mass = 1.0 / m_num_bodies;
m_body_r_body_com(0) = 1.0 / (2.0 * m_num_bodies);
m_body_r_body_com(1) = 0.0;
m_body_r_body_com(2) = 0.0;
m_body_I_body(0, 0) = 1e-4 / (2.0 * m_num_bodies);
m_body_I_body(0, 1) = 0.0;
m_body_I_body(0, 2) = 0.0;
m_body_I_body(1, 0) = 0.0;
m_body_I_body(1, 1) = (3e-4 + 4.0 / BT_ID_POW(m_num_bodies, 2)) / (12.0 * m_num_bodies);
m_body_I_body(1, 2) = 0.0;
m_body_I_body(2, 0) = 0.0;
m_body_I_body(2, 1) = 0.0;
m_body_I_body(2, 2) = m_body_I_body(1, 1);
m_body_I_body(0, 0) = 1e-4 / (2.0 * m_num_bodies);
m_body_I_body(0, 1) = 0.0;
m_body_I_body(0, 2) = 0.0;
m_body_I_body(1, 0) = 0.0;
m_body_I_body(1, 1) = (3e-4 + 4.0 / BT_ID_POW(m_num_bodies, 2)) / (12.0 * m_num_bodies);
m_body_I_body(1, 2) = 0.0;
m_body_I_body(2, 0) = 0.0;
m_body_I_body(2, 1) = 0.0;
m_body_I_body(2, 2) = m_body_I_body(1, 1);
}
CoilCreator::~CoilCreator() {}
int CoilCreator::getNumBodies(int* num_bodies) const {
*num_bodies = m_num_bodies;
return 0;
int CoilCreator::getNumBodies(int* num_bodies) const
{
*num_bodies = m_num_bodies;
return 0;
}
int CoilCreator::getBody(int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const {
if (body_index < 0 || body_index >= m_num_bodies) {
bt_id_error_message("invalid body index %d\n", body_index);
return -1;
}
*parent_index = m_parent[body_index];
*joint_type = REVOLUTE;
*parent_r_parent_body_ref = m_parent_r_parent_body_ref;
*body_T_parent_ref = m_body_T_parent_ref;
*body_axis_of_motion = m_body_axis_of_motion;
*mass = m_mass;
*body_r_body_com = m_body_r_body_com;
*body_I_body = m_body_I_body;
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const
{
if (body_index < 0 || body_index >= m_num_bodies)
{
bt_id_error_message("invalid body index %d\n", body_index);
return -1;
}
*parent_index = m_parent[body_index];
*joint_type = REVOLUTE;
*parent_r_parent_body_ref = m_parent_r_parent_body_ref;
*body_T_parent_ref = m_body_T_parent_ref;
*body_axis_of_motion = m_body_axis_of_motion;
*mass = m_mass;
*body_r_body_com = m_body_r_body_com;
*body_I_body = m_body_I_body;
*user_int = 0;
*user_ptr = 0;
return 0;
}
*user_int = 0;
*user_ptr = 0;
return 0;
}
} // namespace btInverseDynamics

49
Extras/InverseDynamics/CoilCreator.hpp
View File

@ -3,38 +3,39 @@
#include "MultiBodyTreeCreator.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// Creator class for building a "coil" system as intruduced as benchmark example in
/// Featherstone (1999), "A Divide-and-Conquer Articulated-Body Algorithm for Parallel O(log(n))
/// Calculation of Rigid-Body Dynamics. Part 2: Trees, Loops, and Accuracy.", The International
/// Journal of Robotics Research 18 (9): 876–892. doi : 10.1177 / 02783649922066628.
///
/// This is a serial chain, with an initial configuration resembling a coil.
class CoilCreator : public MultiBodyTreeCreator {
class CoilCreator : public MultiBodyTreeCreator
{
public:
/// ctor.
/// @param n the number of bodies in the system
CoilCreator(int n);
/// dtor
~CoilCreator();
// \copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
// \copydoc MultiBodyTreeCreator::getBody
int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
void** user_ptr) const;
/// ctor.
/// @param n the number of bodies in the system
CoilCreator(int n);
/// dtor
~CoilCreator();
// \copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
// \copydoc MultiBodyTreeCreator::getBody
int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
void** user_ptr) const;
private:
int m_num_bodies;
std::vector<int> m_parent;
vec3 m_parent_r_parent_body_ref;
mat33 m_body_T_parent_ref;
vec3 m_body_axis_of_motion;
idScalar m_mass;
vec3 m_body_r_body_com;
mat33 m_body_I_body;
int m_num_bodies;
std::vector<int> m_parent;
vec3 m_parent_r_parent_body_ref;
mat33 m_body_T_parent_ref;
vec3 m_body_axis_of_motion;
idScalar m_mass;
vec3 m_body_r_body_com;
mat33 m_body_I_body;
};
}
} // namespace btInverseDynamics
#endif

210
Extras/InverseDynamics/DillCreator.cpp
View File

@ -1,124 +1,136 @@
#include "DillCreator.hpp"
#include <cmath>
namespace btInverseDynamics {
namespace btInverseDynamics
{
DillCreator::DillCreator(int level)
: m_level(level),
m_num_bodies(BT_ID_POW(2, level))
{
m_parent.resize(m_num_bodies);
m_parent_r_parent_body_ref.resize(m_num_bodies);
m_body_T_parent_ref.resize(m_num_bodies);
m_body_axis_of_motion.resize(m_num_bodies);
m_mass.resize(m_num_bodies);
m_body_r_body_com.resize(m_num_bodies);
m_body_I_body.resize(m_num_bodies);
: m_level(level),
m_num_bodies(BT_ID_POW(2, level))
{
m_parent.resize(m_num_bodies);
m_parent_r_parent_body_ref.resize(m_num_bodies);
m_body_T_parent_ref.resize(m_num_bodies);
m_body_axis_of_motion.resize(m_num_bodies);
m_mass.resize(m_num_bodies);
m_body_r_body_com.resize(m_num_bodies);
m_body_I_body.resize(m_num_bodies);
// generate names (for debugging)
for (int i = 0; i < m_num_bodies; i++) {
m_parent[i] = i - 1;
// generate names (for debugging)
for (int i = 0; i < m_num_bodies; i++)
{
m_parent[i] = i - 1;
// all z-axis (DH convention)
m_body_axis_of_motion[i](0) = 0.0;
m_body_axis_of_motion[i](1) = 0.0;
m_body_axis_of_motion[i](2) = 1.0;
}
// all z-axis (DH convention)
m_body_axis_of_motion[i](0) = 0.0;
m_body_axis_of_motion[i](1) = 0.0;
m_body_axis_of_motion[i](2) = 1.0;
}
// recursively build data structures
m_current_body = 0;
const int parent = -1;
const idScalar d_DH = 0.0;
const idScalar a_DH = 0.0;
const idScalar alpha_DH = 0.0;
// recursively build data structures
m_current_body = 0;
const int parent = -1;
const idScalar d_DH = 0.0;
const idScalar a_DH = 0.0;
const idScalar alpha_DH = 0.0;
if (-1 == recurseDill(m_level, parent, d_DH, a_DH, alpha_DH)) {
bt_id_error_message("recurseDill failed\n");
abort();
}
if (-1 == recurseDill(m_level, parent, d_DH, a_DH, alpha_DH))
{
bt_id_error_message("recurseDill failed\n");
abort();
}
}
DillCreator::~DillCreator() {}
int DillCreator::getNumBodies(int* num_bodies) const {
*num_bodies = m_num_bodies;
return 0;
int DillCreator::getNumBodies(int* num_bodies) const
{
*num_bodies = m_num_bodies;
return 0;
}
int DillCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const {
if (body_index < 0 || body_index >= m_num_bodies) {
bt_id_error_message("invalid body index %d\n", body_index);
return -1;
}
*parent_index = m_parent[body_index];
*joint_type = REVOLUTE;
*parent_r_parent_body_ref = m_parent_r_parent_body_ref[body_index];
*body_T_parent_ref = m_body_T_parent_ref[body_index];
*body_axis_of_motion = m_body_axis_of_motion[body_index];
*mass = m_mass[body_index];
*body_r_body_com = m_body_r_body_com[body_index];
*body_I_body = m_body_I_body[body_index];
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const
{
if (body_index < 0 || body_index >= m_num_bodies)
{
bt_id_error_message("invalid body index %d\n", body_index);
return -1;
}
*parent_index = m_parent[body_index];
*joint_type = REVOLUTE;
*parent_r_parent_body_ref = m_parent_r_parent_body_ref[body_index];
*body_T_parent_ref = m_body_T_parent_ref[body_index];
*body_axis_of_motion = m_body_axis_of_motion[body_index];
*mass = m_mass[body_index];
*body_r_body_com = m_body_r_body_com[body_index];
*body_I_body = m_body_I_body[body_index];
*user_int = 0;
*user_ptr = 0;
return 0;
*user_int = 0;
*user_ptr = 0;
return 0;
}
int DillCreator::recurseDill(const int level, const int parent, const idScalar d_DH_in,
const idScalar a_DH_in, const idScalar alpha_DH_in) {
if (level < 0) {
bt_id_error_message("invalid level parameter (%d)\n", level);
return -1;
}
const idScalar a_DH_in, const idScalar alpha_DH_in)
{
if (level < 0)
{
bt_id_error_message("invalid level parameter (%d)\n", level);
return -1;
}
if (m_current_body >= m_num_bodies || m_current_body < 0) {
bt_id_error_message("invalid body parameter (%d, num_bodies: %d)\n", m_current_body,
m_num_bodies);
return -1;
}
if (m_current_body >= m_num_bodies || m_current_body < 0)
{
bt_id_error_message("invalid body parameter (%d, num_bodies: %d)\n", m_current_body,
m_num_bodies);
return -1;
}
idScalar size = BT_ID_MAX(level, 1);
const int body = m_current_body;
// length = 0.1 * size;
// with = 2 * 0.01 * size;
idScalar size = BT_ID_MAX(level, 1);
const int body = m_current_body;
// length = 0.1 * size;
// with = 2 * 0.01 * size;
/// these parameters are from the paper ...
/// TODO: add proper citation
m_parent[body] = parent;
m_mass[body] = 0.1 * BT_ID_POW(size, 3);
m_body_r_body_com[body](0) = 0.05 * size;
m_body_r_body_com[body](1) = 0;
m_body_r_body_com[body](2) = 0;
// initialization
for (int i = 0; i < 3; i++) {
m_parent_r_parent_body_ref[body](i) = 0;
for (int j = 0; j < 3; j++) {
m_body_I_body[body](i, j) = 0.0;
m_body_T_parent_ref[body](i, j) = 0.0;
}
}
const idScalar size_5 = pow(size, 5);
m_body_I_body[body](0, 0) = size_5 / 0.2e6;
m_body_I_body[body](1, 1) = size_5 * 403 / 1.2e6;
m_body_I_body[body](2, 2) = m_body_I_body[body](1, 1);
/// these parameters are from the paper ...
/// TODO: add proper citation
m_parent[body] = parent;
m_mass[body] = 0.1 * BT_ID_POW(size, 3);
m_body_r_body_com[body](0) = 0.05 * size;
m_body_r_body_com[body](1) = 0;
m_body_r_body_com[body](2) = 0;
// initialization
for (int i = 0; i < 3; i++)
{
m_parent_r_parent_body_ref[body](i) = 0;
for (int j = 0; j < 3; j++)
{
m_body_I_body[body](i, j) = 0.0;
m_body_T_parent_ref[body](i, j) = 0.0;
}
}
const idScalar size_5 = pow(size, 5);
m_body_I_body[body](0, 0) = size_5 / 0.2e6;
m_body_I_body[body](1, 1) = size_5 * 403 / 1.2e6;
m_body_I_body[body](2, 2) = m_body_I_body[body](1, 1);
getVecMatFromDH(0, 0, a_DH_in, alpha_DH_in, &m_parent_r_parent_body_ref[body],
&m_body_T_parent_ref[body]);
getVecMatFromDH(0, 0, a_DH_in, alpha_DH_in, &m_parent_r_parent_body_ref[body],
&m_body_T_parent_ref[body]);
// attach "level" Dill systems of levels 1...level
for (int i = 1; i <= level; i++) {
idScalar d_DH = 0.01 * size;
if (i == level) {
d_DH = 0.0;
}
const idScalar a_DH = i * 0.1;
const idScalar alpha_DH = i * BT_ID_PI / 3.0;
m_current_body++;
recurseDill(i - 1, body, d_DH, a_DH, alpha_DH);
}
// attach "level" Dill systems of levels 1...level
for (int i = 1; i <= level; i++)
{
idScalar d_DH = 0.01 * size;
if (i == level)
{
d_DH = 0.0;
}
const idScalar a_DH = i * 0.1;
const idScalar alpha_DH = i * BT_ID_PI / 3.0;
m_current_body++;
recurseDill(i - 1, body, d_DH, a_DH, alpha_DH);
}
return 0; // ok!
}
return 0; // ok!
}
} // namespace btInverseDynamics

62
Extras/InverseDynamics/DillCreator.hpp
View File

@ -3,45 +3,45 @@
#include "MultiBodyTreeCreator.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// Creator class for building a "Dill" system as intruduced as benchmark example in
/// Featherstone (1999), "A Divide-and-Conquer Articulated-Body Algorithm for Parallel O(log(n))
/// Calculation of Rigid-Body Dynamics. Part 2: Trees, Loops, and Accuracy.", The International
/// Journal of Robotics Research 18 (9): 876–892. doi : 10.1177 / 02783649922066628.
///
/// This is a self-similar branched tree, somewhat resembling a dill plant
class DillCreator : public MultiBodyTreeCreator {
class DillCreator : public MultiBodyTreeCreator
{
public:
/// ctor
/// @param levels the number of dill levels
DillCreator(int levels);
/// dtor
~DillCreator();
///\copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
void** user_ptr) const;
/// ctor
/// @param levels the number of dill levels
DillCreator(int levels);
/// dtor
~DillCreator();
///\copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
void** user_ptr) const;
private:
/// recursively generate dill bodies.
/// TODO better documentation
int recurseDill(const int levels, const int parent, const idScalar d_DH_in,
const idScalar a_DH_in, const idScalar alpha_DH_in);
int m_level;
int m_num_bodies;
idArray<int>::type m_parent;
idArray<vec3>::type m_parent_r_parent_body_ref;
idArray<mat33>::type m_body_T_parent_ref;
idArray<vec3>::type m_body_axis_of_motion;
idArray<idScalar>::type m_mass;
idArray<vec3>::type m_body_r_body_com;
idArray<mat33>::type m_body_I_body;
int m_current_body;
/// recursively generate dill bodies.
/// TODO better documentation
int recurseDill(const int levels, const int parent, const idScalar d_DH_in,
const idScalar a_DH_in, const idScalar alpha_DH_in);
int m_level;
int m_num_bodies;
idArray<int>::type m_parent;
idArray<vec3>::type m_parent_r_parent_body_ref;
idArray<mat33>::type m_body_T_parent_ref;
idArray<vec3>::type m_body_axis_of_motion;
idArray<idScalar>::type m_mass;
idArray<vec3>::type m_body_r_body_com;
idArray<mat33>::type m_body_I_body;
int m_current_body;
};
}
} // namespace btInverseDynamics
#endif

89
Extras/InverseDynamics/IDRandomUtil.cpp
View File

@ -6,9 +6,8 @@
#include "BulletInverseDynamics/IDMath.hpp"
#include "IDRandomUtil.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
// constants for random mass and inertia generation
// these are arbitrary positive values.
static const float mass_min = 0.001;
@ -19,53 +18,59 @@ void randomInit(unsigned seed) { srand(seed); }
int randomInt(int low, int high) { return rand() % (high + 1 - low) + low; }
float randomFloat(float low, float high) {
return low + static_cast<float>(rand()) / RAND_MAX * (high - low);
float randomFloat(float low, float high)
{
return low + static_cast<float>(rand()) / RAND_MAX * (high - low);
}
float randomMass() { return randomFloat(mass_min, mass_max); }
vec3 randomInertiaPrincipal() {
vec3 inertia;
do {
inertia(0) = randomFloat(mass_min, mass_max);
inertia(1) = randomFloat(mass_min, mass_max);
inertia(2) = randomFloat(mass_min, mass_max);
} while (inertia(0) + inertia(1) < inertia(2) || inertia(0) + inertia(2) < inertia(1) ||
inertia(1) + inertia(2) < inertia(0));
return inertia;
vec3 randomInertiaPrincipal()
{
vec3 inertia;
do
{
inertia(0) = randomFloat(mass_min, mass_max);
inertia(1) = randomFloat(mass_min, mass_max);
inertia(2) = randomFloat(mass_min, mass_max);
} while (inertia(0) + inertia(1) < inertia(2) || inertia(0) + inertia(2) < inertia(1) ||
inertia(1) + inertia(2) < inertia(0));
return inertia;
}
mat33 randomInertiaMatrix() {
// generate random valid inertia matrix by first getting valid components
// along major axes and then rotating by random amount
vec3 principal = randomInertiaPrincipal();
mat33 rot(transformX(randomFloat(-BT_ID_PI, BT_ID_PI)) * transformY(randomFloat(-BT_ID_PI, BT_ID_PI)) *
transformZ(randomFloat(-BT_ID_PI, BT_ID_PI)));
mat33 inertia;
inertia(0, 0) = principal(0);
inertia(0, 1) = 0;
inertia(0, 2) = 0;
inertia(1, 0) = 0;
inertia(1, 1) = principal(1);
inertia(1, 2) = 0;
inertia(2, 0) = 0;
inertia(2, 1) = 0;
inertia(2, 2) = principal(2);
return rot * inertia * rot.transpose();
mat33 randomInertiaMatrix()
{
// generate random valid inertia matrix by first getting valid components
// along major axes and then rotating by random amount
vec3 principal = randomInertiaPrincipal();
mat33 rot(transformX(randomFloat(-BT_ID_PI, BT_ID_PI)) * transformY(randomFloat(-BT_ID_PI, BT_ID_PI)) *
transformZ(randomFloat(-BT_ID_PI, BT_ID_PI)));
mat33 inertia;
inertia(0, 0) = principal(0);
inertia(0, 1) = 0;
inertia(0, 2) = 0;
inertia(1, 0) = 0;
inertia(1, 1) = principal(1);
inertia(1, 2) = 0;
inertia(2, 0) = 0;
inertia(2, 1) = 0;
inertia(2, 2) = principal(2);
return rot * inertia * rot.transpose();
}
vec3 randomAxis() {
vec3 axis;
idScalar length;
do {
axis(0) = randomFloat(-1.0, 1.0);
axis(1) = randomFloat(-1.0, 1.0);
axis(2) = randomFloat(-1.0, 1.0);
vec3 randomAxis()
{
vec3 axis;
idScalar length;
do
{
axis(0) = randomFloat(-1.0, 1.0);
axis(1) = randomFloat(-1.0, 1.0);
axis(2) = randomFloat(-1.0, 1.0);
length = BT_ID_SQRT(BT_ID_POW(axis(0), 2) + BT_ID_POW(axis(1), 2) + BT_ID_POW(axis(2), 2));
} while (length < 0.01);
length = BT_ID_SQRT(BT_ID_POW(axis(0), 2) + BT_ID_POW(axis(1), 2) + BT_ID_POW(axis(2), 2));
} while (length < 0.01);
return axis / length;
}
return axis / length;
}
} // namespace btInverseDynamics

5
Extras/InverseDynamics/IDRandomUtil.hpp
View File

@ -1,7 +1,8 @@
#ifndef ID_RANDOM_UTIL_HPP_
#define ID_RANDOM_UTIL_HPP_
#include "BulletInverseDynamics/IDConfig.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// seed random number generator using time()
void randomInit();
/// seed random number generator with identical value to get repeatable results
@ -32,5 +33,5 @@ vec3 randomInertiaPrincipal();
mat33 randomInertiaMatrix();
/// generate a random unit vector
vec3 randomAxis();
}
} // namespace btInverseDynamics
#endif

132
Extras/InverseDynamics/MultiBodyNameMap.cpp
View File

@ -1,78 +1,92 @@
#include "MultiBodyNameMap.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
MultiBodyNameMap::MultiBodyNameMap() {}
int MultiBodyNameMap::addBody(const int index, const std::string& name) {
if (m_index_to_body_name.count(index) > 0) {
bt_id_error_message("trying to add index %d again\n", index);
return -1;
}
if (m_body_name_to_index.count(name) > 0) {
bt_id_error_message("trying to add name %s again\n", name.c_str());
return -1;
}
int MultiBodyNameMap::addBody(const int index, const std::string& name)
{
if (m_index_to_body_name.count(index) > 0)
{
bt_id_error_message("trying to add index %d again\n", index);
return -1;
}
if (m_body_name_to_index.count(name) > 0)
{
bt_id_error_message("trying to add name %s again\n", name.c_str());
return -1;
}
m_index_to_body_name[index] = name;
m_body_name_to_index[name] = index;
m_index_to_body_name[index] = name;
m_body_name_to_index[name] = index;
return 0;
return 0;
}
int MultiBodyNameMap::addJoint(const int index, const std::string& name) {
if (m_index_to_joint_name.count(index) > 0) {
bt_id_error_message("trying to add index %d again\n", index);
return -1;
}
if (m_joint_name_to_index.count(name) > 0) {
bt_id_error_message("trying to add name %s again\n", name.c_str());
return -1;
}
int MultiBodyNameMap::addJoint(const int index, const std::string& name)
{
if (m_index_to_joint_name.count(index) > 0)
{
bt_id_error_message("trying to add index %d again\n", index);
return -1;
}
if (m_joint_name_to_index.count(name) > 0)
{
bt_id_error_message("trying to add name %s again\n", name.c_str());
return -1;
}
m_index_to_joint_name[index] = name;
m_joint_name_to_index[name] = index;
m_index_to_joint_name[index] = name;
m_joint_name_to_index[name] = index;
return 0;
return 0;
}
int MultiBodyNameMap::getBodyName(const int index, std::string* name) const {
std::map<int, std::string>::const_iterator it = m_index_to_body_name.find(index);
if (it == m_index_to_body_name.end()) {
bt_id_error_message("index %d not known\n", index);
return -1;
}
*name = it->second;
return 0;
int MultiBodyNameMap::getBodyName(const int index, std::string* name) const
{
std::map<int, std::string>::const_iterator it = m_index_to_body_name.find(index);
if (it == m_index_to_body_name.end())
{
bt_id_error_message("index %d not known\n", index);
return -1;
}
*name = it->second;
return 0;
}
int MultiBodyNameMap::getJointName(const int index, std::string* name) const {
std::map<int, std::string>::const_iterator it = m_index_to_joint_name.find(index);
if (it == m_index_to_joint_name.end()) {
bt_id_error_message("index %d not known\n", index);
return -1;
}
*name = it->second;
return 0;
int MultiBodyNameMap::getJointName(const int index, std::string* name) const
{
std::map<int, std::string>::const_iterator it = m_index_to_joint_name.find(index);
if (it == m_index_to_joint_name.end())
{
bt_id_error_message("index %d not known\n", index);
return -1;
}
*name = it->second;
return 0;
}
int MultiBodyNameMap::getBodyIndex(const std::string& name, int* index) const {
std::map<std::string, int>::const_iterator it = m_body_name_to_index.find(name);
if (it == m_body_name_to_index.end()) {
bt_id_error_message("name %s not known\n", name.c_str());
return -1;
}
*index = it->second;
return 0;
int MultiBodyNameMap::getBodyIndex(const std::string& name, int* index) const
{
std::map<std::string, int>::const_iterator it = m_body_name_to_index.find(name);
if (it == m_body_name_to_index.end())
{
bt_id_error_message("name %s not known\n", name.c_str());
return -1;
}
*index = it->second;
return 0;
}
int MultiBodyNameMap::getJointIndex(const std::string& name, int* index) const {
std::map<std::string, int>::const_iterator it = m_joint_name_to_index.find(name);
if (it == m_joint_name_to_index.end()) {
bt_id_error_message("name %s not known\n", name.c_str());
return -1;
}
*index = it->second;
return 0;
}
int MultiBodyNameMap::getJointIndex(const std::string& name, int* index) const
{
std::map<std::string, int>::const_iterator it = m_joint_name_to_index.find(name);
if (it == m_joint_name_to_index.end())
{
bt_id_error_message("name %s not known\n", name.c_str());
return -1;
}
*index = it->second;
return 0;
}
} // namespace btInverseDynamics

79
Extras/InverseDynamics/MultiBodyNameMap.hpp
View File

@ -5,50 +5,51 @@
#include <string>
#include <map>
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// \brief The MultiBodyNameMap class
/// Utility class that stores a maps from body/joint indices to/from body and joint names
class MultiBodyNameMap {
class MultiBodyNameMap
{
public:
MultiBodyNameMap();
/// add a body to the map
/// @param index of the body
/// @param name name of the body
/// @return 0 on success, -1 on failure
int addBody(const int index, const std::string& name);
/// add a joint to the map
/// @param index of the joint
/// @param name name of the joint
/// @return 0 on success, -1 on failure
int addJoint(const int index, const std::string& name);
/// get body name from index
/// @param index of the body
/// @param body_name name of the body
/// @return 0 on success, -1 on failure
int getBodyName(const int index, std::string* name) const;
/// get joint name from index
/// @param index of the joint
/// @param joint_name name of the joint
/// @return 0 on success, -1 on failure
int getJointName(const int index, std::string* name) const;
/// get body index from name
/// @param index of the body
/// @param name name of the body
/// @return 0 on success, -1 on failure
int getBodyIndex(const std::string& name, int* index) const;
/// get joint index from name
/// @param index of the joint
/// @param name name of the joint
/// @return 0 on success, -1 on failure
int getJointIndex(const std::string& name, int* index) const;
MultiBodyNameMap();
/// add a body to the map
/// @param index of the body
/// @param name name of the body
/// @return 0 on success, -1 on failure
int addBody(const int index, const std::string& name);
/// add a joint to the map
/// @param index of the joint
/// @param name name of the joint
/// @return 0 on success, -1 on failure
int addJoint(const int index, const std::string& name);
/// get body name from index
/// @param index of the body
/// @param body_name name of the body
/// @return 0 on success, -1 on failure
int getBodyName(const int index, std::string* name) const;
/// get joint name from index
/// @param index of the joint
/// @param joint_name name of the joint
/// @return 0 on success, -1 on failure
int getJointName(const int index, std::string* name) const;
/// get body index from name
/// @param index of the body
/// @param name name of the body
/// @return 0 on success, -1 on failure
int getBodyIndex(const std::string& name, int* index) const;
/// get joint index from name
/// @param index of the joint
/// @param name name of the joint
/// @return 0 on success, -1 on failure
int getJointIndex(const std::string& name, int* index) const;
private:
std::map<int, std::string> m_index_to_joint_name;
std::map<int, std::string> m_index_to_body_name;
std::map<int, std::string> m_index_to_joint_name;
std::map<int, std::string> m_index_to_body_name;
std::map<std::string, int> m_joint_name_to_index;
std::map<std::string, int> m_body_name_to_index;
std::map<std::string, int> m_joint_name_to_index;
std::map<std::string, int> m_body_name_to_index;
};
}
} // namespace btInverseDynamics
#endif // MULTIBODYNAMEMAP_HPP_

119
Extras/InverseDynamics/MultiBodyTreeCreator.cpp
View File

@ -1,64 +1,71 @@
#include "MultiBodyTreeCreator.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator)
{
int num_bodies;
int parent_index;
JointType joint_type;
vec3 body_r_parent_body_ref;
mat33 body_R_parent_ref;
vec3 body_axis_of_motion;
idScalar mass;
vec3 body_r_body_com;
mat33 body_I_body;
int user_int;
void* user_ptr;
MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator) {
int num_bodies;
int parent_index;
JointType joint_type;
vec3 body_r_parent_body_ref;
mat33 body_R_parent_ref;
vec3 body_axis_of_motion;
idScalar mass;
vec3 body_r_body_com;
mat33 body_I_body;
int user_int;
void* user_ptr;
MultiBodyTree* tree = new MultiBodyTree();
if (0x0 == tree)
{
bt_id_error_message("cannot allocate tree\n");
return 0x0;
}
MultiBodyTree* tree = new MultiBodyTree();
if (0x0 == tree) {
bt_id_error_message("cannot allocate tree\n");
return 0x0;
}
// TODO: move to some policy argument
tree->setAcceptInvalidMassParameters(false);
// TODO: move to some policy argument
tree->setAcceptInvalidMassParameters(false);
// get number of bodies in the system
if (-1 == creator.getNumBodies(&num_bodies))
{
bt_id_error_message("getting body indices\n");
delete tree;
return 0x0;
}
// get number of bodies in the system
if (-1 == creator.getNumBodies(&num_bodies)) {
bt_id_error_message("getting body indices\n");
delete tree;
return 0x0;
}
// get data for all bodies
for (int index = 0; index < num_bodies; index++)
{
// get body parameters from user callbacks
if (-1 ==
creator.getBody(index, &parent_index, &joint_type, &body_r_parent_body_ref,
&body_R_parent_ref, &body_axis_of_motion, &mass, &body_r_body_com,
&body_I_body, &user_int, &user_ptr))
{
bt_id_error_message("getting data for body %d\n", index);
delete tree;
return 0x0;
}
// add body to system
if (-1 ==
tree->addBody(index, parent_index, joint_type, body_r_parent_body_ref,
body_R_parent_ref, body_axis_of_motion, mass, body_r_body_com,
body_I_body, user_int, user_ptr))
{
bt_id_error_message("adding body %d\n", index);
delete tree;
return 0x0;
}
}
// finalize initialization
if (-1 == tree->finalize())
{
bt_id_error_message("building system\n");
delete tree;
return 0x0;
}
// get data for all bodies
for (int index = 0; index < num_bodies; index++) {
// get body parameters from user callbacks
if (-1 ==
creator.getBody(index, &parent_index, &joint_type, &body_r_parent_body_ref,
&body_R_parent_ref, &body_axis_of_motion, &mass, &body_r_body_com,
&body_I_body, &user_int, &user_ptr)) {
bt_id_error_message("getting data for body %d\n", index);
delete tree;
return 0x0;
}
// add body to system
if (-1 ==
tree->addBody(index, parent_index, joint_type, body_r_parent_body_ref,
body_R_parent_ref, body_axis_of_motion, mass, body_r_body_com,
body_I_body, user_int, user_ptr)) {
bt_id_error_message("adding body %d\n", index);
delete tree;
return 0x0;
}
}
// finalize initialization
if (-1 == tree->finalize()) {
bt_id_error_message("building system\n");
delete tree;
return 0x0;
}
return tree;
}
return tree;
}
} // namespace btInverseDynamics

38
Extras/InverseDynamics/MultiBodyTreeCreator.hpp
View File

@ -8,28 +8,30 @@
#include "BulletInverseDynamics/MultiBodyTree.hpp"
#include "MultiBodyNameMap.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// Interface class for initializing a MultiBodyTree instance.
/// Data to be provided is modeled on the URDF specification.
/// The user can derive from this class in order to programmatically
/// initialize a system.
class MultiBodyTreeCreator {
class MultiBodyTreeCreator
{
public:
/// the dtor
virtual ~MultiBodyTreeCreator() {}
/// Get the number of bodies in the system
/// @param num_bodies write number of bodies here
/// @return 0 on success, -1 on error
virtual int getNumBodies(int* num_bodies) const = 0;
/// Interface for accessing link mass properties.
/// For detailed description of data, @sa MultiBodyTree::addBody
/// \copydoc MultiBodyTree::addBody
virtual int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const = 0;
/// @return a pointer to a name mapping utility class, or 0x0 if not available
virtual const MultiBodyNameMap* getNameMap() const {return 0x0;}
/// the dtor
virtual ~MultiBodyTreeCreator() {}
/// Get the number of bodies in the system
/// @param num_bodies write number of bodies here
/// @return 0 on success, -1 on error
virtual int getNumBodies(int* num_bodies) const = 0;
/// Interface for accessing link mass properties.
/// For detailed description of data, @sa MultiBodyTree::addBody
/// \copydoc MultiBodyTree::addBody
virtual int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const = 0;
/// @return a pointer to a name mapping utility class, or 0x0 if not available
virtual const MultiBodyNameMap* getNameMap() const { return 0x0; }
};
/// Create a multibody object.
@ -38,7 +40,7 @@ public:
/// @return A pointer to an allocated multibodytree instance, or
/// 0x0 if an error occured.
MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator);
}
} // namespace btInverseDynamics
// does urdf have gravity direction ??

123
Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp
View File

@ -2,63 +2,76 @@
#include <cstdio>
namespace btInverseDynamics {
namespace btInverseDynamics
{
int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map,
const char* filename) {
if (0x0 == tree) {
bt_id_error_message("tree pointer is null\n");
return -1;
}
if (0x0 == filename) {
bt_id_error_message("filename is null\n");
return -1;
}
const char* filename)
{
if (0x0 == tree)
{
bt_id_error_message("tree pointer is null\n");
return -1;
}
if (0x0 == filename)
{
bt_id_error_message("filename is null\n");
return -1;
}
FILE* fp = fopen(filename, "w");
if (NULL == fp) {
bt_id_error_message("cannot open file %s for writing\n", filename);
return -1;
}
fprintf(fp, "// to generate postscript file, run dot -Tps %s -o %s.ps\n"
"// details see graphviz documentation at http://graphviz.org\n"
"digraph tree {\n",
filename, filename);
FILE* fp = fopen(filename, "w");
if (NULL == fp)
{
bt_id_error_message("cannot open file %s for writing\n", filename);
return -1;
}
fprintf(fp,
"// to generate postscript file, run dot -Tps %s -o %s.ps\n"
"// details see graphviz documentation at http://graphviz.org\n"
"digraph tree {\n",
filename, filename);
for (int body = 0; body < tree->numBodies(); body++) {
std::string name;
if (0x0 != map) {
if (-1 == map->getBodyName(body, &name)) {
bt_id_error_message("can't get name of body %d\n", body);
return -1;
}
fprintf(fp, " %d [label=\"%d/%s\"];\n", body, body, name.c_str());
}
}
for (int body = 0; body < tree->numBodies(); body++) {
int parent;
const char* joint_type;
int qi;
if (-1 == tree->getParentIndex(body, &parent)) {
bt_id_error_message("indexing error\n");
return -1;
}
if (-1 == tree->getJointTypeStr(body, &joint_type)) {
bt_id_error_message("indexing error\n");
return -1;
}
if (-1 == tree->getDoFOffset(body, &qi)) {
bt_id_error_message("indexing error\n");
return -1;
}
if (-1 != parent) {
fprintf(fp, " %d -> %d [label= \"type:%s, q=%d\"];\n", parent, body,
joint_type, qi);
}
}
for (int body = 0; body < tree->numBodies(); body++)
{
std::string name;
if (0x0 != map)
{
if (-1 == map->getBodyName(body, &name))
{
bt_id_error_message("can't get name of body %d\n", body);
return -1;
}
fprintf(fp, " %d [label=\"%d/%s\"];\n", body, body, name.c_str());
}
}
for (int body = 0; body < tree->numBodies(); body++)
{
int parent;
const char* joint_type;
int qi;
if (-1 == tree->getParentIndex(body, &parent))
{
bt_id_error_message("indexing error\n");
return -1;
}
if (-1 == tree->getJointTypeStr(body, &joint_type))
{
bt_id_error_message("indexing error\n");
return -1;
}
if (-1 == tree->getDoFOffset(body, &qi))
{
bt_id_error_message("indexing error\n");
return -1;
}
if (-1 != parent)
{
fprintf(fp, " %d -> %d [label= \"type:%s, q=%d\"];\n", parent, body,
joint_type, qi);
}
}
fprintf(fp, "}\n");
fclose(fp);
return 0;
}
fprintf(fp, "}\n");
fclose(fp);
return 0;
}
} // namespace btInverseDynamics

7
Extras/InverseDynamics/MultiBodyTreeDebugGraph.hpp
View File

@ -4,14 +4,15 @@
#include "BulletInverseDynamics/MultiBodyTree.hpp"
#include "MultiBodyNameMap.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// generate a dot-file of the multibody tree for generating a graph using graphviz' dot tool
/// @param tree the multibody tree
/// @param map to add names of links (if 0x0, no names will be added)
/// @param filename name for the output file
/// @return 0 on success, -1 on error
int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map,
const char* filename);
}
const char* filename);
} // namespace btInverseDynamics
#endif // MULTIBODYTREEDEBUGGRAPH_HPP

134
Extras/InverseDynamics/RandomTreeCreator.cpp
View File

@ -4,78 +4,88 @@
#include "IDRandomUtil.hpp"
namespace btInverseDynamics {
RandomTreeCreator::RandomTreeCreator(const int max_bodies, bool random_seed) {
// seed generator
if(random_seed) {
randomInit(); // seeds with time()
} else {
randomInit(1); // seeds with 1
}
m_num_bodies = randomInt(1, max_bodies);
namespace btInverseDynamics
{
RandomTreeCreator::RandomTreeCreator(const int max_bodies, bool random_seed)
{
// seed generator
if (random_seed)
{
randomInit(); // seeds with time()
}
else
{
randomInit(1); // seeds with 1
}
m_num_bodies = randomInt(1, max_bodies);
}
RandomTreeCreator::~RandomTreeCreator() {}
int RandomTreeCreator::getNumBodies(int* num_bodies) const {
*num_bodies = m_num_bodies;
return 0;
int RandomTreeCreator::getNumBodies(int* num_bodies) const
{
*num_bodies = m_num_bodies;
return 0;
}
int RandomTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const {
if(0 == body_index) { //root body
*parent_index = -1;
} else {
*parent_index = randomInt(0, body_index - 1);
}
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const
{
if (0 == body_index)
{ //root body
*parent_index = -1;
}
else
{
*parent_index = randomInt(0, body_index - 1);
}
switch (randomInt(0, 3)) {
case 0:
*joint_type = FIXED;
break;
case 1:
*joint_type = REVOLUTE;
break;
case 2:
*joint_type = PRISMATIC;
break;
case 3:
*joint_type = FLOATING;
break;
default:
bt_id_error_message("randomInt() result out of range\n");
return -1;
}
switch (randomInt(0, 3))
{
case 0:
*joint_type = FIXED;
break;
case 1:
*joint_type = REVOLUTE;
break;
case 2:
*joint_type = PRISMATIC;
break;
case 3:
*joint_type = FLOATING;
break;
default:
bt_id_error_message("randomInt() result out of range\n");
return -1;
}
(*parent_r_parent_body_ref)(0) = randomFloat(-1.0, 1.0);
(*parent_r_parent_body_ref)(1) = randomFloat(-1.0, 1.0);
(*parent_r_parent_body_ref)(2) = randomFloat(-1.0, 1.0);
(*parent_r_parent_body_ref)(0) = randomFloat(-1.0, 1.0);
(*parent_r_parent_body_ref)(1) = randomFloat(-1.0, 1.0);
(*parent_r_parent_body_ref)(2) = randomFloat(-1.0, 1.0);
bodyTParentFromAxisAngle(randomAxis(), randomFloat(-BT_ID_PI, BT_ID_PI), body_T_parent_ref);
bodyTParentFromAxisAngle(randomAxis(), randomFloat(-BT_ID_PI, BT_ID_PI), body_T_parent_ref);
*body_axis_of_motion = randomAxis();
*mass = randomMass();
(*body_r_body_com)(0) = randomFloat(-1.0, 1.0);
(*body_r_body_com)(1) = randomFloat(-1.0, 1.0);
(*body_r_body_com)(2) = randomFloat(-1.0, 1.0);
const double a = randomFloat(-BT_ID_PI, BT_ID_PI);
const double b = randomFloat(-BT_ID_PI, BT_ID_PI);
const double c = randomFloat(-BT_ID_PI, BT_ID_PI);
vec3 ii = randomInertiaPrincipal();
mat33 ii_diag;
setZero(ii_diag);
ii_diag(0,0)=ii(0);
ii_diag(1,1)=ii(1);
ii_diag(2,2)=ii(2);
*body_I_body = transformX(a) * transformY(b) * transformZ(c) * ii_diag *
transformZ(-c) * transformY(-b) * transformX(-a);
*user_int = 0;
*user_ptr = 0;
*body_axis_of_motion = randomAxis();
*mass = randomMass();
(*body_r_body_com)(0) = randomFloat(-1.0, 1.0);
(*body_r_body_com)(1) = randomFloat(-1.0, 1.0);
(*body_r_body_com)(2) = randomFloat(-1.0, 1.0);
const double a = randomFloat(-BT_ID_PI, BT_ID_PI);
const double b = randomFloat(-BT_ID_PI, BT_ID_PI);
const double c = randomFloat(-BT_ID_PI, BT_ID_PI);
vec3 ii = randomInertiaPrincipal();
mat33 ii_diag;
setZero(ii_diag);
ii_diag(0, 0) = ii(0);
ii_diag(1, 1) = ii(1);
ii_diag(2, 2) = ii(2);
*body_I_body = transformX(a) * transformY(b) * transformZ(c) * ii_diag *
transformZ(-c) * transformY(-b) * transformX(-a);
*user_int = 0;
*user_ptr = 0;
return 0;
}
return 0;
}
} // namespace btInverseDynamics

36
Extras/InverseDynamics/RandomTreeCreator.hpp
View File

@ -4,28 +4,30 @@
#include "BulletInverseDynamics/IDConfig.hpp"
#include "MultiBodyTreeCreator.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// Generate a random MultiBodyTree with fixed or floating base and fixed, prismatic or revolute
/// joints
/// Uses a pseudo random number generator seeded from a random device.
class RandomTreeCreator : public MultiBodyTreeCreator {
class RandomTreeCreator : public MultiBodyTreeCreator
{
public:
/// ctor
/// @param max_bodies maximum number of bodies
/// @param gravity gravitational acceleration
/// @param use_seed if true, seed random number generator
RandomTreeCreator(const int max_bodies, bool use_seed=false);
~RandomTreeCreator();
///\copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
void** user_ptr) const;
/// ctor
/// @param max_bodies maximum number of bodies
/// @param gravity gravitational acceleration
/// @param use_seed if true, seed random number generator
RandomTreeCreator(const int max_bodies, bool use_seed = false);
~RandomTreeCreator();
///\copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
void** user_ptr) const;
private:
int m_num_bodies;
int m_num_bodies;
};
}
} // namespace btInverseDynamics
#endif // RANDOMTREE_CREATOR_HPP_

118
Extras/InverseDynamics/SimpleTreeCreator.cpp
View File

@ -2,68 +2,76 @@
#include <cstdio>
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// minimal "tree" (chain)
SimpleTreeCreator::SimpleTreeCreator(int dim) : m_num_bodies(dim) {
m_mass = 1.0;
m_body_T_parent_ref(0, 0) = 1;
m_body_T_parent_ref(0, 1) = 0;
m_body_T_parent_ref(0, 2) = 0;
m_body_T_parent_ref(1, 0) = 0;
m_body_T_parent_ref(1, 1) = 1;
m_body_T_parent_ref(1, 2) = 0;
m_body_T_parent_ref(2, 0) = 0;
m_body_T_parent_ref(2, 1) = 0;
m_body_T_parent_ref(2, 2) = 1;
SimpleTreeCreator::SimpleTreeCreator(int dim) : m_num_bodies(dim)
{
m_mass = 1.0;
m_body_T_parent_ref(0, 0) = 1;
m_body_T_parent_ref(0, 1) = 0;
m_body_T_parent_ref(0, 2) = 0;
m_body_T_parent_ref(1, 0) = 0;
m_body_T_parent_ref(1, 1) = 1;
m_body_T_parent_ref(1, 2) = 0;
m_body_T_parent_ref(2, 0) = 0;
m_body_T_parent_ref(2, 1) = 0;
m_body_T_parent_ref(2, 2) = 1;
m_parent_r_parent_body_ref(0) = 1.0;
m_parent_r_parent_body_ref(1) = 0.0;
m_parent_r_parent_body_ref(2) = 0.0;
m_parent_r_parent_body_ref(0) = 1.0;
m_parent_r_parent_body_ref(1) = 0.0;
m_parent_r_parent_body_ref(2) = 0.0;
m_body_r_body_com(0) = 0.5;
m_body_r_body_com(1) = 0.0;
m_body_r_body_com(2) = 0.0;
m_body_r_body_com(0) = 0.5;
m_body_r_body_com(1) = 0.0;
m_body_r_body_com(2) = 0.0;
m_body_I_body(0, 0) = 1;
m_body_I_body(0, 1) = 0;
m_body_I_body(0, 2) = 0;
m_body_I_body(1, 0) = 0;
m_body_I_body(1, 1) = 1;
m_body_I_body(1, 2) = 0;
m_body_I_body(2, 0) = 0;
m_body_I_body(2, 1) = 0;
m_body_I_body(2, 2) = 1;
m_body_I_body(0, 0) = 1;
m_body_I_body(0, 1) = 0;
m_body_I_body(0, 2) = 0;
m_body_I_body(1, 0) = 0;
m_body_I_body(1, 1) = 1;
m_body_I_body(1, 2) = 0;
m_body_I_body(2, 0) = 0;
m_body_I_body(2, 1) = 0;
m_body_I_body(2, 2) = 1;
m_axis(0) = 0;
m_axis(1) = 0;
m_axis(2) = 1;
m_axis(0) = 0;
m_axis(1) = 0;
m_axis(2) = 1;
}
int SimpleTreeCreator::getNumBodies(int* num_bodies) const {
*num_bodies = m_num_bodies;
return 0;
int SimpleTreeCreator::getNumBodies(int* num_bodies) const
{
*num_bodies = m_num_bodies;
return 0;
}
int SimpleTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const {
*parent_index = body_index - 1;
if (body_index % 2) {
*joint_type = PRISMATIC;
} else {
*joint_type = REVOLUTE;
}
*parent_r_parent_body_ref = m_parent_r_parent_body_ref;
if (0 == body_index) {
(*parent_r_parent_body_ref)(2) = 1.0;
}
*body_T_parent_ref = m_body_T_parent_ref;
*body_axis_of_motion = m_axis;
*mass = m_mass;
*body_r_body_com = m_body_r_body_com;
*body_I_body = m_body_I_body;
*user_int = 0;
*user_ptr = 0;
return 0;
}
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const
{
*parent_index = body_index - 1;
if (body_index % 2)
{
*joint_type = PRISMATIC;
}
else
{
*joint_type = REVOLUTE;
}
*parent_r_parent_body_ref = m_parent_r_parent_body_ref;
if (0 == body_index)
{
(*parent_r_parent_body_ref)(2) = 1.0;
}
*body_T_parent_ref = m_body_T_parent_ref;
*body_axis_of_motion = m_axis;
*mass = m_mass;
*body_r_body_com = m_body_r_body_com;
*body_I_body = m_body_I_body;
*user_int = 0;
*user_ptr = 0;
return 0;
}
} // namespace btInverseDynamics

47
Extras/InverseDynamics/SimpleTreeCreator.hpp
View File

@ -3,32 +3,33 @@
#include "MultiBodyTreeCreator.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// minimal "tree" (chain)
class SimpleTreeCreator : public MultiBodyTreeCreator {
class SimpleTreeCreator : public MultiBodyTreeCreator
{
public:
/// ctor
/// @param dim number of bodies
SimpleTreeCreator(int dim);
// dtor
~SimpleTreeCreator() {}
///\copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
void** user_ptr) const;
/// ctor
/// @param dim number of bodies
SimpleTreeCreator(int dim);
// dtor
~SimpleTreeCreator() {}
///\copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
void** user_ptr) const;
private:
int m_num_bodies;
idScalar m_mass;
mat33 m_body_T_parent_ref;
vec3 m_parent_r_parent_body_ref;
vec3 m_body_r_body_com;
mat33 m_body_I_body;
vec3 m_axis;
int m_num_bodies;
idScalar m_mass;
mat33 m_body_T_parent_ref;
vec3 m_parent_r_parent_body_ref;
vec3 m_body_r_body_com;
mat33 m_body_I_body;
vec3 m_axis;
};
}
} // namespace btInverseDynamics
#endif // SIMPLETREECREATOR_HPP_

172
Extras/InverseDynamics/User2InternalIndex.cpp
View File

@ -1,99 +1,121 @@
#include "User2InternalIndex.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
User2InternalIndex::User2InternalIndex() : m_map_built(false) {}
void User2InternalIndex::addBody(const int body, const int parent) {
m_user_parent_index_map[body] = parent;
void User2InternalIndex::addBody(const int body, const int parent)
{
m_user_parent_index_map[body] = parent;
}
int User2InternalIndex::findRoot(int index) {
if (0 == m_user_parent_index_map.count(index)) {
return index;
}
return findRoot(m_user_parent_index_map[index]);
int User2InternalIndex::findRoot(int index)
{
if (0 == m_user_parent_index_map.count(index))
{
return index;
}
return findRoot(m_user_parent_index_map[index]);
}
// modelled after URDF2Bullet.cpp:void ComputeParentIndices(const
// URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex,
// int urdfParentIndex)
void User2InternalIndex::recurseIndexSets(const int user_body_index) {
m_user_to_internal[user_body_index] = m_current_index;
m_current_index++;
for (size_t i = 0; i < m_user_child_indices[user_body_index].size(); i++) {
recurseIndexSets(m_user_child_indices[user_body_index][i]);
}
void User2InternalIndex::recurseIndexSets(const int user_body_index)
{
m_user_to_internal[user_body_index] = m_current_index;
m_current_index++;
for (size_t i = 0; i < m_user_child_indices[user_body_index].size(); i++)
{
recurseIndexSets(m_user_child_indices[user_body_index][i]);
}
}
int User2InternalIndex::buildMapping() {
// find root index
int user_root_index = -1;
for (std::map<int, int>::iterator it = m_user_parent_index_map.begin();
it != m_user_parent_index_map.end(); it++) {
int current_root_index = findRoot(it->second);
if (it == m_user_parent_index_map.begin()) {
user_root_index = current_root_index;
} else {
if (user_root_index != current_root_index) {
bt_id_error_message("multiple roots (at least) %d and %d\n", user_root_index,
current_root_index);
return -1;
}
}
}
int User2InternalIndex::buildMapping()
{
// find root index
int user_root_index = -1;
for (std::map<int, int>::iterator it = m_user_parent_index_map.begin();
it != m_user_parent_index_map.end(); it++)
{
int current_root_index = findRoot(it->second);
if (it == m_user_parent_index_map.begin())
{
user_root_index = current_root_index;
}
else
{
if (user_root_index != current_root_index)
{
bt_id_error_message("multiple roots (at least) %d and %d\n", user_root_index,
current_root_index);
return -1;
}
}
}
// build child index map
for (std::map<int, int>::iterator it = m_user_parent_index_map.begin();
it != m_user_parent_index_map.end(); it++) {
m_user_child_indices[it->second].push_back(it->first);
}
// build child index map
for (std::map<int, int>::iterator it = m_user_parent_index_map.begin();
it != m_user_parent_index_map.end(); it++)
{
m_user_child_indices[it->second].push_back(it->first);
}
m_current_index = -1;
// build internal index set
m_user_to_internal[user_root_index] = -1; // add map for root link
recurseIndexSets(user_root_index);
m_current_index = -1;
// build internal index set
m_user_to_internal[user_root_index] = -1; // add map for root link
recurseIndexSets(user_root_index);
// reverse mapping
for (std::map<int, int>::iterator it = m_user_to_internal.begin();
it != m_user_to_internal.end(); it++) {
m_internal_to_user[it->second] = it->first;
}
// reverse mapping
for (std::map<int, int>::iterator it = m_user_to_internal.begin();
it != m_user_to_internal.end(); it++)
{
m_internal_to_user[it->second] = it->first;
}
m_map_built = true;
return 0;
m_map_built = true;
return 0;
}
int User2InternalIndex::user2internal(const int user, int *internal) const {
int User2InternalIndex::user2internal(const int user, int *internal) const
{
if (!m_map_built)
{
return -1;
}
if (!m_map_built) {
return -1;
}
std::map<int, int>::const_iterator it;
it = m_user_to_internal.find(user);
if (it != m_user_to_internal.end()) {
*internal = it->second;
return 0;
} else {
bt_id_error_message("no user index %d\n", user);
return -1;
}
std::map<int, int>::const_iterator it;
it = m_user_to_internal.find(user);
if (it != m_user_to_internal.end())
{
*internal = it->second;
return 0;
}
else
{
bt_id_error_message("no user index %d\n", user);
return -1;
}
}
int User2InternalIndex::internal2user(const int internal, int *user) const {
int User2InternalIndex::internal2user(const int internal, int *user) const
{
if (!m_map_built)
{
return -1;
}
if (!m_map_built) {
return -1;
}
std::map<int, int>::const_iterator it;
it = m_internal_to_user.find(internal);
if (it != m_internal_to_user.end()) {
*user = it->second;
return 0;
} else {
bt_id_error_message("no internal index %d\n", internal);
return -1;
}
}
std::map<int, int>::const_iterator it;
it = m_internal_to_user.find(internal);
if (it != m_internal_to_user.end())
{
*user = it->second;
return 0;
}
else
{
bt_id_error_message("no internal index %d\n", internal);
return -1;
}
}
} // namespace btInverseDynamics

63
Extras/InverseDynamics/User2InternalIndex.hpp
View File

@ -5,42 +5,43 @@
#include "BulletInverseDynamics/IDConfig.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// Convert arbitrary indexing scheme to internal indexing
/// used for MultiBodyTree
class User2InternalIndex {
class User2InternalIndex
{
public:
/// Ctor
User2InternalIndex();
/// add body to index maps
/// @param body index of body to add (external)
/// @param parent index of parent body (external)
void addBody(const int body, const int parent);
/// build mapping from external to internal indexing
/// @return 0 on success, -1 on failure
int buildMapping();
/// get internal index from external index
/// @param user external (user) index
/// @param internal pointer for storage of corresponding internal index
/// @return 0 on success, -1 on failure
int user2internal(const int user, int *internal) const;
/// get internal index from external index
/// @param user external (user) index
/// @param internal pointer for storage of corresponding internal index
/// @return 0 on success, -1 on failure
int internal2user(const int internal, int *user) const;
/// Ctor
User2InternalIndex();
/// add body to index maps
/// @param body index of body to add (external)
/// @param parent index of parent body (external)
void addBody(const int body, const int parent);
/// build mapping from external to internal indexing
/// @return 0 on success, -1 on failure
int buildMapping();
/// get internal index from external index
/// @param user external (user) index
/// @param internal pointer for storage of corresponding internal index
/// @return 0 on success, -1 on failure
int user2internal(const int user, int *internal) const;
/// get internal index from external index
/// @param user external (user) index
/// @param internal pointer for storage of corresponding internal index
/// @return 0 on success, -1 on failure
int internal2user(const int internal, int *user) const;
private:
int findRoot(int index);
void recurseIndexSets(const int user_body_index);
bool m_map_built;
std::map<int, int> m_user_parent_index_map;
std::map<int, int> m_user_to_internal;
std::map<int, int> m_internal_to_user;
std::map<int, std::vector<int> > m_user_child_indices;
int m_current_index;
int findRoot(int index);
void recurseIndexSets(const int user_body_index);
bool m_map_built;
std::map<int, int> m_user_parent_index_map;
std::map<int, int> m_user_to_internal;
std::map<int, int> m_internal_to_user;
std::map<int, std::vector<int> > m_user_child_indices;
int m_current_index;
};
}
} // namespace btInverseDynamics
#endif // USER2INTERNALINDEX_HPP

140
Extras/InverseDynamics/btMultiBodyFromURDF.hpp
View File

@ -14,78 +14,84 @@
/// Create a btMultiBody model from URDF.
/// This is adapted from Bullet URDF loader example
class MyBtMultiBodyFromURDF {
class MyBtMultiBodyFromURDF
{
public:
/// ctor
/// @param gravity gravitational acceleration (in world frame)
/// @param base_fixed if true, the root body is treated as fixed,
/// if false, it is treated as floating
MyBtMultiBodyFromURDF(const btVector3 &gravity, const bool base_fixed)
: m_gravity(gravity), m_base_fixed(base_fixed) {
m_broadphase = 0x0;
m_dispatcher = 0x0;
m_solver = 0x0;
m_collisionConfiguration = 0x0;
m_dynamicsWorld = 0x0;
m_multibody = 0x0;
}
/// dtor
~MyBtMultiBodyFromURDF() {
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
delete m_multibody;
}
/// @param name path to urdf file
void setFileName(const std::string name) { m_filename = name; }
/// load urdf file and build btMultiBody model
void init() {
this->createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(m_gravity);
BulletURDFImporter urdf_importer(&m_nogfx,0,1,0);
URDFImporterInterface &u2b(urdf_importer);
bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed);
/// ctor
/// @param gravity gravitational acceleration (in world frame)
/// @param base_fixed if true, the root body is treated as fixed,
/// if false, it is treated as floating
MyBtMultiBodyFromURDF(const btVector3 &gravity, const bool base_fixed)
: m_gravity(gravity), m_base_fixed(base_fixed)
{
m_broadphase = 0x0;
m_dispatcher = 0x0;
m_solver = 0x0;
m_collisionConfiguration = 0x0;
m_dynamicsWorld = 0x0;
m_multibody = 0x0;
}
/// dtor
~MyBtMultiBodyFromURDF()
{
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
delete m_multibody;
}
/// @param name path to urdf file
void setFileName(const std::string name) { m_filename = name; }
/// load urdf file and build btMultiBody model
void init()
{
this->createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(m_gravity);
BulletURDFImporter urdf_importer(&m_nogfx, 0, 1, 0);
URDFImporterInterface &u2b(urdf_importer);
bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed);
if (loadOk) {
btTransform identityTrans;
identityTrans.setIdentity();
MyMultiBodyCreator creation(&m_nogfx);
const bool use_multibody = true;
ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, use_multibody,
u2b.getPathPrefix());
m_multibody = creation.getBulletMultiBody();
m_dynamicsWorld->stepSimulation(1. / 240., 0);
}
}
/// @return pointer to the btMultiBody model
btMultiBody *getBtMultiBody() { return m_multibody; }
if (loadOk)
{
btTransform identityTrans;
identityTrans.setIdentity();
MyMultiBodyCreator creation(&m_nogfx);
const bool use_multibody = true;
ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, use_multibody,
u2b.getPathPrefix());
m_multibody = creation.getBulletMultiBody();
m_dynamicsWorld->stepSimulation(1. / 240., 0);
}
}
/// @return pointer to the btMultiBody model
btMultiBody *getBtMultiBody() { return m_multibody; }
private:
// internal utility function
void createEmptyDynamicsWorld() {
m_collisionConfiguration = new btDefaultCollisionConfiguration();
// internal utility function
void createEmptyDynamicsWorld()
{
m_collisionConfiguration = new btDefaultCollisionConfiguration();
/// use the default collision dispatcher. For parallel processing you can use a diffent
/// dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
m_solver = new btMultiBodyConstraintSolver;
m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver,
m_collisionConfiguration);
m_dynamicsWorld->setGravity(m_gravity);
}
/// use the default collision dispatcher. For parallel processing you can use a diffent
/// dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
m_solver = new btMultiBodyConstraintSolver;
m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver,
m_collisionConfiguration);
m_dynamicsWorld->setGravity(m_gravity);
}
btBroadphaseInterface *m_broadphase;
btCollisionDispatcher *m_dispatcher;
btMultiBodyConstraintSolver *m_solver;
btDefaultCollisionConfiguration *m_collisionConfiguration;
btMultiBodyDynamicsWorld *m_dynamicsWorld;
std::string m_filename;
DummyGUIHelper m_nogfx;
btMultiBody *m_multibody;
const btVector3 m_gravity;
const bool m_base_fixed;
btBroadphaseInterface *m_broadphase;
btCollisionDispatcher *m_dispatcher;
btMultiBodyConstraintSolver *m_solver;
btDefaultCollisionConfiguration *m_collisionConfiguration;
btMultiBodyDynamicsWorld *m_dynamicsWorld;
std::string m_filename;
DummyGUIHelper m_nogfx;
btMultiBody *m_multibody;
const btVector3 m_gravity;
const bool m_base_fixed;
};
#endif // BTMULTIBODYFROMURDF_HPP

497
Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
View File

@ -1,272 +1,299 @@
#include "btMultiBodyTreeCreator.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
btMultiBodyTreeCreator::btMultiBodyTreeCreator() : m_initialized(false) {}
int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const bool verbose) {
if (0x0 == btmb) {
bt_id_error_message("cannot create MultiBodyTree from null pointer\n");
return -1;
}
int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const bool verbose)
{
if (0x0 == btmb)
{
bt_id_error_message("cannot create MultiBodyTree from null pointer\n");
return -1;
}
// in case this is a second call, discard old data
m_data.clear();
m_initialized = false;
// in case this is a second call, discard old data
m_data.clear();
m_initialized = false;
// btMultiBody treats base link separately
m_data.resize(1 + btmb->getNumLinks());
// btMultiBody treats base link separately
m_data.resize(1 + btmb->getNumLinks());
// add base link data
{
LinkData &link = m_data[0];
// add base link data
{
LinkData &link = m_data[0];
link.parent_index = -1;
if (btmb->hasFixedBase()) {
link.joint_type = FIXED;
} else {
link.joint_type = FLOATING;
}
btTransform transform=(btmb->getBaseWorldTransform());
link.parent_index = -1;
if (btmb->hasFixedBase())
{
link.joint_type = FIXED;
}
else
{
link.joint_type = FLOATING;
}
btTransform transform = (btmb->getBaseWorldTransform());
//compute inverse dynamics in body-fixed frame
transform.setIdentity();
link.parent_r_parent_body_ref(0) = transform.getOrigin()[0];
link.parent_r_parent_body_ref(1) = transform.getOrigin()[1];
link.parent_r_parent_body_ref(2) = transform.getOrigin()[2];
link.parent_r_parent_body_ref(0) = transform.getOrigin()[0];
link.parent_r_parent_body_ref(1) = transform.getOrigin()[1];
link.parent_r_parent_body_ref(2) = transform.getOrigin()[2];
link.body_T_parent_ref(0, 0) = transform.getBasis()[0][0];
link.body_T_parent_ref(0, 1) = transform.getBasis()[0][1];
link.body_T_parent_ref(0, 2) = transform.getBasis()[0][2];
link.body_T_parent_ref(0, 0) = transform.getBasis()[0][0];
link.body_T_parent_ref(0, 1) = transform.getBasis()[0][1];
link.body_T_parent_ref(0, 2) = transform.getBasis()[0][2];
link.body_T_parent_ref(1, 0) = transform.getBasis()[1][0];
link.body_T_parent_ref(1, 1) = transform.getBasis()[1][1];
link.body_T_parent_ref(1, 2) = transform.getBasis()[1][2];
link.body_T_parent_ref(1, 0) = transform.getBasis()[1][0];
link.body_T_parent_ref(1, 1) = transform.getBasis()[1][1];
link.body_T_parent_ref(1, 2) = transform.getBasis()[1][2];
link.body_T_parent_ref(2, 0) = transform.getBasis()[2][0];
link.body_T_parent_ref(2, 1) = transform.getBasis()[2][1];
link.body_T_parent_ref(2, 2) = transform.getBasis()[2][2];
link.body_T_parent_ref(2, 0) = transform.getBasis()[2][0];
link.body_T_parent_ref(2, 1) = transform.getBasis()[2][1];
link.body_T_parent_ref(2, 2) = transform.getBasis()[2][2];
// random unit vector. value not used for fixed or floating joints.
link.body_axis_of_motion(0) = 0;
link.body_axis_of_motion(1) = 0;
link.body_axis_of_motion(2) = 1;
// random unit vector. value not used for fixed or floating joints.
link.body_axis_of_motion(0) = 0;
link.body_axis_of_motion(1) = 0;
link.body_axis_of_motion(2) = 1;
link.mass = btmb->getBaseMass();
// link frame in the center of mass
link.body_r_body_com(0) = 0;
link.body_r_body_com(1) = 0;
link.body_r_body_com(2) = 0;
// BulletDynamics uses body-fixed frame in the cog, aligned with principal axes
link.body_I_body(0, 0) = btmb->getBaseInertia()[0];
link.body_I_body(0, 1) = 0.0;
link.body_I_body(0, 2) = 0.0;
link.body_I_body(1, 0) = 0.0;
link.body_I_body(1, 1) = btmb->getBaseInertia()[1];
link.body_I_body(1, 2) = 0.0;
link.body_I_body(2, 0) = 0.0;
link.body_I_body(2, 1) = 0.0;
link.body_I_body(2, 2) = btmb->getBaseInertia()[2];
// shift reference point to link origin (in joint axis)
mat33 tilde_r_com = tildeOperator(link.body_r_body_com);
link.body_I_body = link.body_I_body - link.mass * tilde_r_com * tilde_r_com;
if (verbose) {
id_printf("base: mass= %f, bt_inertia= [%f %f %f]\n"
"Io= [%f %f %f;\n"
" %f %f %f;\n"
" %f %f %f]\n",
link.mass, btmb->getBaseInertia()[0], btmb->getBaseInertia()[1],
btmb->getBaseInertia()[2], link.body_I_body(0, 0), link.body_I_body(0, 1),
link.body_I_body(0, 2), link.body_I_body(1, 0), link.body_I_body(1, 1),
link.body_I_body(1, 2), link.body_I_body(2, 0), link.body_I_body(2, 1),
link.body_I_body(2, 2));
}
}
link.mass = btmb->getBaseMass();
// link frame in the center of mass
link.body_r_body_com(0) = 0;
link.body_r_body_com(1) = 0;
link.body_r_body_com(2) = 0;
// BulletDynamics uses body-fixed frame in the cog, aligned with principal axes
link.body_I_body(0, 0) = btmb->getBaseInertia()[0];
link.body_I_body(0, 1) = 0.0;
link.body_I_body(0, 2) = 0.0;
link.body_I_body(1, 0) = 0.0;
link.body_I_body(1, 1) = btmb->getBaseInertia()[1];
link.body_I_body(1, 2) = 0.0;
link.body_I_body(2, 0) = 0.0;
link.body_I_body(2, 1) = 0.0;
link.body_I_body(2, 2) = btmb->getBaseInertia()[2];
// shift reference point to link origin (in joint axis)
mat33 tilde_r_com = tildeOperator(link.body_r_body_com);
link.body_I_body = link.body_I_body - link.mass * tilde_r_com * tilde_r_com;
if (verbose)
{
id_printf(
"base: mass= %f, bt_inertia= [%f %f %f]\n"
"Io= [%f %f %f;\n"
" %f %f %f;\n"
" %f %f %f]\n",
link.mass, btmb->getBaseInertia()[0], btmb->getBaseInertia()[1],
btmb->getBaseInertia()[2], link.body_I_body(0, 0), link.body_I_body(0, 1),
link.body_I_body(0, 2), link.body_I_body(1, 0), link.body_I_body(1, 1),
link.body_I_body(1, 2), link.body_I_body(2, 0), link.body_I_body(2, 1),
link.body_I_body(2, 2));
}
}
for (int bt_index = 0; bt_index < btmb->getNumLinks(); bt_index++) {
if (verbose) {
id_printf("bt->id: converting link %d\n", bt_index);
}
const btMultibodyLink &bt_link = btmb->getLink(bt_index);
LinkData &link = m_data[bt_index + 1];
for (int bt_index = 0; bt_index < btmb->getNumLinks(); bt_index++)
{
if (verbose)
{
id_printf("bt->id: converting link %d\n", bt_index);
}
const btMultibodyLink &bt_link = btmb->getLink(bt_index);
LinkData &link = m_data[bt_index + 1];
link.parent_index = bt_link.m_parent + 1;
link.parent_index = bt_link.m_parent + 1;
link.mass = bt_link.m_mass;
if (verbose) {
id_printf("mass= %f\n", link.mass);
}
// from this body's pivot to this body's com in this body's frame
link.body_r_body_com[0] = bt_link.m_dVector[0];
link.body_r_body_com[1] = bt_link.m_dVector[1];
link.body_r_body_com[2] = bt_link.m_dVector[2];
if (verbose) {
id_printf("com= %f %f %f\n", link.body_r_body_com[0], link.body_r_body_com[1],
link.body_r_body_com[2]);
}
// BulletDynamics uses a body-fixed frame in the CoM, aligned with principal axes
link.body_I_body(0, 0) = bt_link.m_inertiaLocal[0];
link.body_I_body(0, 1) = 0.0;
link.body_I_body(0, 2) = 0.0;
link.body_I_body(1, 0) = 0.0;
link.body_I_body(1, 1) = bt_link.m_inertiaLocal[1];
link.body_I_body(1, 2) = 0.0;
link.body_I_body(2, 0) = 0.0;
link.body_I_body(2, 1) = 0.0;
link.body_I_body(2, 2) = bt_link.m_inertiaLocal[2];
// shift reference point to link origin (in joint axis)
mat33 tilde_r_com = tildeOperator(link.body_r_body_com);
link.body_I_body = link.body_I_body - link.mass * tilde_r_com * tilde_r_com;
link.mass = bt_link.m_mass;
if (verbose)
{
id_printf("mass= %f\n", link.mass);
}
// from this body's pivot to this body's com in this body's frame
link.body_r_body_com[0] = bt_link.m_dVector[0];
link.body_r_body_com[1] = bt_link.m_dVector[1];
link.body_r_body_com[2] = bt_link.m_dVector[2];
if (verbose)
{
id_printf("com= %f %f %f\n", link.body_r_body_com[0], link.body_r_body_com[1],
link.body_r_body_com[2]);
}
// BulletDynamics uses a body-fixed frame in the CoM, aligned with principal axes
link.body_I_body(0, 0) = bt_link.m_inertiaLocal[0];
link.body_I_body(0, 1) = 0.0;
link.body_I_body(0, 2) = 0.0;
link.body_I_body(1, 0) = 0.0;
link.body_I_body(1, 1) = bt_link.m_inertiaLocal[1];
link.body_I_body(1, 2) = 0.0;
link.body_I_body(2, 0) = 0.0;
link.body_I_body(2, 1) = 0.0;
link.body_I_body(2, 2) = bt_link.m_inertiaLocal[2];
// shift reference point to link origin (in joint axis)
mat33 tilde_r_com = tildeOperator(link.body_r_body_com);
link.body_I_body = link.body_I_body - link.mass * tilde_r_com * tilde_r_com;
if (verbose) {
id_printf("link %d: mass= %f, bt_inertia= [%f %f %f]\n"
"Io= [%f %f %f;\n"
" %f %f %f;\n"
" %f %f %f]\n",
bt_index, link.mass, bt_link.m_inertiaLocal[0], bt_link.m_inertiaLocal[1],
bt_link.m_inertiaLocal[2], link.body_I_body(0, 0), link.body_I_body(0, 1),
link.body_I_body(0, 2), link.body_I_body(1, 0), link.body_I_body(1, 1),
link.body_I_body(1, 2), link.body_I_body(2, 0), link.body_I_body(2, 1),
link.body_I_body(2, 2));
}
// transform for vectors written in parent frame to this link's body-fixed frame
btMatrix3x3 basis = btTransform(bt_link.m_zeroRotParentToThis).getBasis();
link.body_T_parent_ref(0, 0) = basis[0][0];
link.body_T_parent_ref(0, 1) = basis[0][1];
link.body_T_parent_ref(0, 2) = basis[0][2];
link.body_T_parent_ref(1, 0) = basis[1][0];
link.body_T_parent_ref(1, 1) = basis[1][1];
link.body_T_parent_ref(1, 2) = basis[1][2];
link.body_T_parent_ref(2, 0) = basis[2][0];
link.body_T_parent_ref(2, 1) = basis[2][1];
link.body_T_parent_ref(2, 2) = basis[2][2];
if (verbose) {
id_printf("body_T_parent_ref= %f %f %f\n"
" %f %f %f\n"
" %f %f %f\n",
basis[0][0], basis[0][1], basis[0][2], basis[1][0], basis[1][1], basis[1][2],
basis[2][0], basis[2][1], basis[2][2]);
}
switch (bt_link.m_jointType) {
case btMultibodyLink::eRevolute:
link.joint_type = REVOLUTE;
if (verbose) {
id_printf("type= revolute\n");
}
link.body_axis_of_motion(0) = bt_link.m_axes[0].m_topVec[0];
link.body_axis_of_motion(1) = bt_link.m_axes[0].m_topVec[1];
link.body_axis_of_motion(2) = bt_link.m_axes[0].m_topVec[2];
if (verbose)
{
id_printf(
"link %d: mass= %f, bt_inertia= [%f %f %f]\n"
"Io= [%f %f %f;\n"
" %f %f %f;\n"
" %f %f %f]\n",
bt_index, link.mass, bt_link.m_inertiaLocal[0], bt_link.m_inertiaLocal[1],
bt_link.m_inertiaLocal[2], link.body_I_body(0, 0), link.body_I_body(0, 1),
link.body_I_body(0, 2), link.body_I_body(1, 0), link.body_I_body(1, 1),
link.body_I_body(1, 2), link.body_I_body(2, 0), link.body_I_body(2, 1),
link.body_I_body(2, 2));
}
// transform for vectors written in parent frame to this link's body-fixed frame
btMatrix3x3 basis = btTransform(bt_link.m_zeroRotParentToThis).getBasis();
link.body_T_parent_ref(0, 0) = basis[0][0];
link.body_T_parent_ref(0, 1) = basis[0][1];
link.body_T_parent_ref(0, 2) = basis[0][2];
link.body_T_parent_ref(1, 0) = basis[1][0];
link.body_T_parent_ref(1, 1) = basis[1][1];
link.body_T_parent_ref(1, 2) = basis[1][2];
link.body_T_parent_ref(2, 0) = basis[2][0];
link.body_T_parent_ref(2, 1) = basis[2][1];
link.body_T_parent_ref(2, 2) = basis[2][2];
if (verbose)
{
id_printf(
"body_T_parent_ref= %f %f %f\n"
" %f %f %f\n"
" %f %f %f\n",
basis[0][0], basis[0][1], basis[0][2], basis[1][0], basis[1][1], basis[1][2],
basis[2][0], basis[2][1], basis[2][2]);
}
switch (bt_link.m_jointType)
{
case btMultibodyLink::eRevolute:
link.joint_type = REVOLUTE;
if (verbose)
{
id_printf("type= revolute\n");
}
link.body_axis_of_motion(0) = bt_link.m_axes[0].m_topVec[0];
link.body_axis_of_motion(1) = bt_link.m_axes[0].m_topVec[1];
link.body_axis_of_motion(2) = bt_link.m_axes[0].m_topVec[2];
// for revolute joints, m_eVector = parentComToThisPivotOffset
// m_dVector = thisPivotToThisComOffset
// from parent com to pivot, in parent frame
link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
case btMultibodyLink::ePrismatic:
link.joint_type = PRISMATIC;
if (verbose) {
id_printf("type= prismatic\n");
}
link.body_axis_of_motion(0) = bt_link.m_axes[0].m_bottomVec[0];
link.body_axis_of_motion(1) = bt_link.m_axes[0].m_bottomVec[1];
link.body_axis_of_motion(2) = bt_link.m_axes[0].m_bottomVec[2];
// for revolute joints, m_eVector = parentComToThisPivotOffset
// m_dVector = thisPivotToThisComOffset
// from parent com to pivot, in parent frame
link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
case btMultibodyLink::ePrismatic:
link.joint_type = PRISMATIC;
if (verbose)
{
id_printf("type= prismatic\n");
}
link.body_axis_of_motion(0) = bt_link.m_axes[0].m_bottomVec[0];
link.body_axis_of_motion(1) = bt_link.m_axes[0].m_bottomVec[1];
link.body_axis_of_motion(2) = bt_link.m_axes[0].m_bottomVec[2];
// for prismatic joints, eVector
// according to documentation :
// parentComToThisComOffset
// but seems to be: from parent's com to parent's
// pivot ??
// m_dVector = thisPivotToThisComOffset
link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
case btMultibodyLink::eSpherical:
bt_id_error_message("spherical joints not implemented\n");
return -1;
case btMultibodyLink::ePlanar:
bt_id_error_message("planar joints not implemented\n");
return -1;
case btMultibodyLink::eFixed:
link.joint_type = FIXED;
// random unit vector
link.body_axis_of_motion(0) = 0;
link.body_axis_of_motion(1) = 0;
link.body_axis_of_motion(2) = 1;
// for prismatic joints, eVector
// according to documentation :
// parentComToThisComOffset
// but seems to be: from parent's com to parent's
// pivot ??
// m_dVector = thisPivotToThisComOffset
link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
case btMultibodyLink::eSpherical:
bt_id_error_message("spherical joints not implemented\n");
return -1;
case btMultibodyLink::ePlanar:
bt_id_error_message("planar joints not implemented\n");
return -1;
case btMultibodyLink::eFixed:
link.joint_type = FIXED;
// random unit vector
link.body_axis_of_motion(0) = 0;
link.body_axis_of_motion(1) = 0;
link.body_axis_of_motion(2) = 1;
// for fixed joints, m_dVector = thisPivotToThisComOffset;
// m_eVector = parentComToThisPivotOffset;
link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
default:
bt_id_error_message("unknown btMultiBody::eFeatherstoneJointType %d\n",
bt_link.m_jointType);
return -1;
}
if (link.parent_index > 0) { // parent body isn't the root
const btMultibodyLink &bt_parent_link = btmb->getLink(link.parent_index - 1);
// from parent pivot to parent com, in parent frame
link.parent_r_parent_body_ref(0) += bt_parent_link.m_dVector[0];
link.parent_r_parent_body_ref(1) += bt_parent_link.m_dVector[1];
link.parent_r_parent_body_ref(2) += bt_parent_link.m_dVector[2];
} else {
// parent is root body. btMultiBody only knows 6-DoF or 0-DoF root bodies,
// whose link frame is in the CoM (ie, no notion of a pivot point)
}
// for fixed joints, m_dVector = thisPivotToThisComOffset;
// m_eVector = parentComToThisPivotOffset;
link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
default:
bt_id_error_message("unknown btMultiBody::eFeatherstoneJointType %d\n",
bt_link.m_jointType);
return -1;
}
if (link.parent_index > 0)
{ // parent body isn't the root
const btMultibodyLink &bt_parent_link = btmb->getLink(link.parent_index - 1);
// from parent pivot to parent com, in parent frame
link.parent_r_parent_body_ref(0) += bt_parent_link.m_dVector[0];
link.parent_r_parent_body_ref(1) += bt_parent_link.m_dVector[1];
link.parent_r_parent_body_ref(2) += bt_parent_link.m_dVector[2];
}
else
{
// parent is root body. btMultiBody only knows 6-DoF or 0-DoF root bodies,
// whose link frame is in the CoM (ie, no notion of a pivot point)
}
if (verbose) {
id_printf("parent_r_parent_body_ref= %f %f %f\n", link.parent_r_parent_body_ref[0],
link.parent_r_parent_body_ref[1], link.parent_r_parent_body_ref[2]);
}
}
if (verbose)
{
id_printf("parent_r_parent_body_ref= %f %f %f\n", link.parent_r_parent_body_ref[0],
link.parent_r_parent_body_ref[1], link.parent_r_parent_body_ref[2]);
}
}
m_initialized = true;
m_initialized = true;
return 0;
return 0;
}
int btMultiBodyTreeCreator::getNumBodies(int *num_bodies) const {
if (false == m_initialized) {
bt_id_error_message("btMultiBody not converted yet\n");
return -1;
}
int btMultiBodyTreeCreator::getNumBodies(int *num_bodies) const
{
if (false == m_initialized)
{
bt_id_error_message("btMultiBody not converted yet\n");
return -1;
}
*num_bodies = static_cast<int>(m_data.size());
return 0;
*num_bodies = static_cast<int>(m_data.size());
return 0;
}
int btMultiBodyTreeCreator::getBody(const int body_index, int *parent_index, JointType *joint_type,
vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref,
vec3 *body_axis_of_motion, idScalar *mass,
vec3 *body_r_body_com, mat33 *body_I_body, int *user_int,
void **user_ptr) const {
if (false == m_initialized) {
bt_id_error_message("MultiBodyTree not created yet\n");
return -1;
}
vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref,
vec3 *body_axis_of_motion, idScalar *mass,
vec3 *body_r_body_com, mat33 *body_I_body, int *user_int,
void **user_ptr) const
{
if (false == m_initialized)
{
bt_id_error_message("MultiBodyTree not created yet\n");
return -1;
}
if (body_index < 0 || body_index >= static_cast<int>(m_data.size())) {
bt_id_error_message("index out of range (got %d but only %zu bodies)\n", body_index,
m_data.size());
return -1;
}
if (body_index < 0 || body_index >= static_cast<int>(m_data.size()))
{
bt_id_error_message("index out of range (got %d but only %zu bodies)\n", body_index,
m_data.size());
return -1;
}
*parent_index = m_data[body_index].parent_index;
*joint_type = m_data[body_index].joint_type;
*parent_r_parent_body_ref = m_data[body_index].parent_r_parent_body_ref;
*body_T_parent_ref = m_data[body_index].body_T_parent_ref;
*body_axis_of_motion = m_data[body_index].body_axis_of_motion;
*mass = m_data[body_index].mass;
*body_r_body_com = m_data[body_index].body_r_body_com;
*body_I_body = m_data[body_index].body_I_body;
*parent_index = m_data[body_index].parent_index;
*joint_type = m_data[body_index].joint_type;
*parent_r_parent_body_ref = m_data[body_index].parent_r_parent_body_ref;
*body_T_parent_ref = m_data[body_index].body_T_parent_ref;
*body_axis_of_motion = m_data[body_index].body_axis_of_motion;
*mass = m_data[body_index].mass;
*body_r_body_com = m_data[body_index].body_r_body_com;
*body_I_body = m_data[body_index].body_I_body;
*user_int = -1;
*user_ptr = 0x0;
*user_int = -1;
*user_ptr = 0x0;
return 0;
}
return 0;
}
} // namespace btInverseDynamics

66
Extras/InverseDynamics/btMultiBodyTreeCreator.hpp
View File

@ -7,44 +7,46 @@
#include "MultiBodyTreeCreator.hpp"
#include "BulletDynamics/Featherstone/btMultiBody.h"
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// MultiBodyTreeCreator implementation for converting
/// a btMultiBody forward dynamics model into a MultiBodyTree inverse dynamics model
class btMultiBodyTreeCreator : public MultiBodyTreeCreator {
class btMultiBodyTreeCreator : public MultiBodyTreeCreator
{
public:
/// ctor
btMultiBodyTreeCreator();
/// dtor
~btMultiBodyTreeCreator() {}
/// extract model data from a btMultiBody
/// @param btmb pointer to btMultiBody to convert
/// @param verbose if true, some information is printed
/// @return -1 on error, 0 on success
int createFromBtMultiBody(const btMultiBody *btmb, const bool verbose = false);
/// \copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int *num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
/// ctor
btMultiBodyTreeCreator();
/// dtor
~btMultiBodyTreeCreator() {}
/// extract model data from a btMultiBody
/// @param btmb pointer to btMultiBody to convert
/// @param verbose if true, some information is printed
/// @return -1 on error, 0 on success
int createFromBtMultiBody(const btMultiBody *btmb, const bool verbose = false);
/// \copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int *num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(const int body_index, int *parent_index, JointType *joint_type,
vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref,
vec3 *body_axis_of_motion, idScalar *mass, vec3 *body_r_body_com,
mat33 *body_I_body, int *user_int, void **user_ptr) const;
vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref,
vec3 *body_axis_of_motion, idScalar *mass, vec3 *body_r_body_com,
mat33 *body_I_body, int *user_int, void **user_ptr) const;
private:
// internal struct holding data extracted from btMultiBody
struct LinkData {
int parent_index;
JointType joint_type;
vec3 parent_r_parent_body_ref;
mat33 body_T_parent_ref;
vec3 body_axis_of_motion;
idScalar mass;
vec3 body_r_body_com;
mat33 body_I_body;
};
idArray<LinkData>::type m_data;
bool m_initialized;
// internal struct holding data extracted from btMultiBody
struct LinkData
{
int parent_index;
JointType joint_type;
vec3 parent_r_parent_body_ref;
mat33 body_T_parent_ref;
vec3 body_axis_of_motion;
idScalar mass;
vec3 body_r_body_com;
mat33 body_I_body;
};
idArray<LinkData>::type m_data;
bool m_initialized;
};
}
} // namespace btInverseDynamics
#endif // BTMULTIBODYTREECREATOR_HPP_

588
Extras/InverseDynamics/invdyn_bullet_comparison.cpp
View File

@ -8,300 +8,354 @@
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
namespace btInverseDynamics {
namespace btInverseDynamics
{
int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &gravity, bool verbose,
btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error,
double *acc_error) {
btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error,
double *acc_error)
{
// call function and return -1 if it does, printing an bt_id_error_message
#define RETURN_ON_FAILURE(x) \
do { \
if (-1 == x) { \
bt_id_error_message("calling " #x "\n"); \
return -1; \
} \
} while (0)
#define RETURN_ON_FAILURE(x) \
do \
{ \
if (-1 == x) \
{ \
bt_id_error_message("calling " #x "\n"); \
return -1; \
} \
} while (0)
if (verbose) {
printf("\n ===================================== \n");
}
vecx joint_forces(q.size());
if (verbose)
{
printf("\n ===================================== \n");
}
vecx joint_forces(q.size());
// set positions and velocities for btMultiBody
// base link
mat33 world_T_base;
vec3 world_pos_base;
btTransform base_transform;
vec3 base_velocity;
vec3 base_angular_velocity;
// set positions and velocities for btMultiBody
// base link
mat33 world_T_base;
vec3 world_pos_base;
btTransform base_transform;
vec3 base_velocity;
vec3 base_angular_velocity;
RETURN_ON_FAILURE(id_tree->setGravityInWorldFrame(gravity));
RETURN_ON_FAILURE(id_tree->getBodyOrigin(0, &world_pos_base));
RETURN_ON_FAILURE(id_tree->getBodyTransform(0, &world_T_base));
RETURN_ON_FAILURE(id_tree->getBodyAngularVelocity(0, &base_angular_velocity));
RETURN_ON_FAILURE(id_tree->getBodyLinearVelocityCoM(0, &base_velocity));
RETURN_ON_FAILURE(id_tree->setGravityInWorldFrame(gravity));
RETURN_ON_FAILURE(id_tree->getBodyOrigin(0, &world_pos_base));
RETURN_ON_FAILURE(id_tree->getBodyTransform(0, &world_T_base));
RETURN_ON_FAILURE(id_tree->getBodyAngularVelocity(0, &base_angular_velocity));
RETURN_ON_FAILURE(id_tree->getBodyLinearVelocityCoM(0, &base_velocity));
base_transform.setBasis(world_T_base);
base_transform.setOrigin(world_pos_base);
btmb->setBaseWorldTransform(base_transform);
btmb->setBaseOmega(base_angular_velocity);
btmb->setBaseVel(base_velocity);
btmb->setLinearDamping(0);
btmb->setAngularDamping(0);
base_transform.setBasis(world_T_base);
base_transform.setOrigin(world_pos_base);
btmb->setBaseWorldTransform(base_transform);
btmb->setBaseOmega(base_angular_velocity);
btmb->setBaseVel(base_velocity);
btmb->setLinearDamping(0);
btmb->setAngularDamping(0);
// remaining links
int q_index;
if (btmb->hasFixedBase()) {
q_index = 0;
} else {
q_index = 6;
}
if (verbose) {
printf("bt:num_links= %d, num_dofs= %d\n", btmb->getNumLinks(), btmb->getNumDofs());
}
for (int l = 0; l < btmb->getNumLinks(); l++) {
const btMultibodyLink &link = btmb->getLink(l);
if (verbose) {
printf("link %d, pos_var_count= %d, dof_count= %d\n", l, link.m_posVarCount,
link.m_dofCount);
}
if (link.m_posVarCount == 1) {
btmb->setJointPosMultiDof(l, &q(q_index));
btmb->setJointVelMultiDof(l, &u(q_index));
if (verbose) {
printf("set q[%d]= %f, u[%d]= %f\n", q_index, q(q_index), q_index, u(q_index));
}
q_index++;
}
}
// sanity check
if (q_index != q.size()) {
bt_id_error_message("error in number of dofs for btMultibody and MultiBodyTree\n");
return -1;
}
// remaining links
int q_index;
if (btmb->hasFixedBase())
{
q_index = 0;
}
else
{
q_index = 6;
}
if (verbose)
{
printf("bt:num_links= %d, num_dofs= %d\n", btmb->getNumLinks(), btmb->getNumDofs());
}
for (int l = 0; l < btmb->getNumLinks(); l++)
{
const btMultibodyLink &link = btmb->getLink(l);
if (verbose)
{
printf("link %d, pos_var_count= %d, dof_count= %d\n", l, link.m_posVarCount,
link.m_dofCount);
}
if (link.m_posVarCount == 1)
{
btmb->setJointPosMultiDof(l, &q(q_index));
btmb->setJointVelMultiDof(l, &u(q_index));
if (verbose)
{
printf("set q[%d]= %f, u[%d]= %f\n", q_index, q(q_index), q_index, u(q_index));
}
q_index++;
}
}
// sanity check
if (q_index != q.size())
{
bt_id_error_message("error in number of dofs for btMultibody and MultiBodyTree\n");
return -1;
}
// run inverse dynamics to determine joint_forces for given q, u, dot_u
if (-1 == id_tree->calculateInverseDynamics(q, u, dot_u, &joint_forces)) {
bt_id_error_message("calculating inverse dynamics\n");
return -1;
}
// run inverse dynamics to determine joint_forces for given q, u, dot_u
if (-1 == id_tree->calculateInverseDynamics(q, u, dot_u, &joint_forces))
{
bt_id_error_message("calculating inverse dynamics\n");
return -1;
}
// set up bullet forward dynamics model
btScalar dt = 0;
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;
// this triggers switch between using either appliedConstraintForce or appliedForce
bool isConstraintPass = false;
// apply gravity forces for btMultiBody model. Must be done manually.
btmb->addBaseForce(btmb->getBaseMass() * gravity);
// set up bullet forward dynamics model
btScalar dt = 0;
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;
// this triggers switch between using either appliedConstraintForce or appliedForce
bool isConstraintPass = false;
// apply gravity forces for btMultiBody model. Must be done manually.
btmb->addBaseForce(btmb->getBaseMass() * gravity);
for (int link = 0; link < btmb->getNumLinks(); link++) {
btmb->addLinkForce(link, gravity * btmb->getLinkMass(link));
if (verbose) {
printf("link %d, applying gravity %f %f %f\n", link,
gravity[0] * btmb->getLinkMass(link), gravity[1] * btmb->getLinkMass(link),
gravity[2] * btmb->getLinkMass(link));
}
}
for (int link = 0; link < btmb->getNumLinks(); link++)
{
btmb->addLinkForce(link, gravity * btmb->getLinkMass(link));
if (verbose)
{
printf("link %d, applying gravity %f %f %f\n", link,
gravity[0] * btmb->getLinkMass(link), gravity[1] * btmb->getLinkMass(link),
gravity[2] * btmb->getLinkMass(link));
}
}
// apply generalized forces
if (btmb->hasFixedBase()) {
q_index = 0;
} else {
vec3 base_force;
base_force(0) = joint_forces(3);
base_force(1) = joint_forces(4);
base_force(2) = joint_forces(5);
// apply generalized forces
if (btmb->hasFixedBase())
{
q_index = 0;
}
else
{
vec3 base_force;
base_force(0) = joint_forces(3);
base_force(1) = joint_forces(4);
base_force(2) = joint_forces(5);
vec3 base_moment;
base_moment(0) = joint_forces(0);
base_moment(1) = joint_forces(1);
base_moment(2) = joint_forces(2);
vec3 base_moment;
base_moment(0) = joint_forces(0);
base_moment(1) = joint_forces(1);
base_moment(2) = joint_forces(2);
btmb->addBaseForce(world_T_base * base_force);
btmb->addBaseTorque(world_T_base * base_moment);
if (verbose) {
printf("base force from id: %f %f %f\n", joint_forces(3), joint_forces(4),
joint_forces(5));
printf("base moment from id: %f %f %f\n", joint_forces(0), joint_forces(1),
joint_forces(2));
}
q_index = 6;
}
btmb->addBaseForce(world_T_base * base_force);
btmb->addBaseTorque(world_T_base * base_moment);
if (verbose)
{
printf("base force from id: %f %f %f\n", joint_forces(3), joint_forces(4),
joint_forces(5));
printf("base moment from id: %f %f %f\n", joint_forces(0), joint_forces(1),
joint_forces(2));
}
q_index = 6;
}
for (int l = 0; l < btmb->getNumLinks(); l++) {
const btMultibodyLink &link = btmb->getLink(l);
if (link.m_posVarCount == 1) {
if (verbose) {
printf("id:joint_force[%d]= %f, applied to link %d\n", q_index,
joint_forces(q_index), l);
}
btmb->addJointTorque(l, joint_forces(q_index));
q_index++;
}
}
for (int l = 0; l < btmb->getNumLinks(); l++)
{
const btMultibodyLink &link = btmb->getLink(l);
if (link.m_posVarCount == 1)
{
if (verbose)
{
printf("id:joint_force[%d]= %f, applied to link %d\n", q_index,
joint_forces(q_index), l);
}
btmb->addJointTorque(l, joint_forces(q_index));
q_index++;
}
}
// sanity check
if (q_index != q.size()) {
bt_id_error_message("error in number of dofs for btMultibody and MultiBodyTree\n");
return -1;
}
// sanity check
if (q_index != q.size())
{
bt_id_error_message("error in number of dofs for btMultibody and MultiBodyTree\n");
return -1;
}
// run forward kinematics & forward dynamics
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
btmb->forwardKinematics(world_to_local, local_origin);
btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass);
// run forward kinematics & forward dynamics
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
btmb->forwardKinematics(world_to_local, local_origin);
btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass);
// read generalized accelerations back from btMultiBody
// the mapping from scratch variables to accelerations is taken from the implementation
// of stepVelocitiesMultiDof
btScalar *base_accel = &scratch_r[btmb->getNumDofs()];
btScalar *joint_accel = base_accel + 6;
*acc_error = 0;
int dot_u_offset = 0;
if (btmb->hasFixedBase()) {
dot_u_offset = 0;
} else {
dot_u_offset = 6;
}
// read generalized accelerations back from btMultiBody
// the mapping from scratch variables to accelerations is taken from the implementation
// of stepVelocitiesMultiDof
btScalar *base_accel = &scratch_r[btmb->getNumDofs()];
btScalar *joint_accel = base_accel + 6;
*acc_error = 0;
int dot_u_offset = 0;
if (btmb->hasFixedBase())
{
dot_u_offset = 0;
}
else
{
dot_u_offset = 6;
}
if (true == btmb->hasFixedBase()) {
for (int i = 0; i < btmb->getNumDofs(); i++) {
if (verbose) {
printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i],
dot_u(i + dot_u_offset), joint_accel[i] - dot_u(i));
}
*acc_error += BT_ID_POW(joint_accel[i] - dot_u(i + dot_u_offset), 2);
}
} else {
vec3 base_dot_omega;
vec3 world_dot_omega;
world_dot_omega(0) = base_accel[0];
world_dot_omega(1) = base_accel[1];
world_dot_omega(2) = base_accel[2];
base_dot_omega = world_T_base.transpose() * world_dot_omega;
if (true == btmb->hasFixedBase())
{
for (int i = 0; i < btmb->getNumDofs(); i++)
{
if (verbose)
{
printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i],
dot_u(i + dot_u_offset), joint_accel[i] - dot_u(i));
}
*acc_error += BT_ID_POW(joint_accel[i] - dot_u(i + dot_u_offset), 2);
}
}
else
{
vec3 base_dot_omega;
vec3 world_dot_omega;
world_dot_omega(0) = base_accel[0];
world_dot_omega(1) = base_accel[1];
world_dot_omega(2) = base_accel[2];
base_dot_omega = world_T_base.transpose() * world_dot_omega;
// com happens to coincide with link origin here. If that changes, we need to calculate
// ddot_com
vec3 base_ddot_com;
vec3 world_ddot_com;
world_ddot_com(0) = base_accel[3];
world_ddot_com(1) = base_accel[4];
world_ddot_com(2) = base_accel[5];
base_ddot_com = world_T_base.transpose()*world_ddot_com;
// com happens to coincide with link origin here. If that changes, we need to calculate
// ddot_com
vec3 base_ddot_com;
vec3 world_ddot_com;
world_ddot_com(0) = base_accel[3];
world_ddot_com(1) = base_accel[4];
world_ddot_com(2) = base_accel[5];
base_ddot_com = world_T_base.transpose() * world_ddot_com;
for (int i = 0; i < 3; i++) {
if (verbose) {
printf("bt::base_dot_omega(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_dot_omega(i),
i, dot_u[i], base_dot_omega(i) - dot_u[i]);
}
*acc_error += BT_ID_POW(base_dot_omega(i) - dot_u(i), 2);
}
for (int i = 0; i < 3; i++) {
if (verbose) {
printf("bt::base_ddot_com(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_ddot_com(i),
i, dot_u[i + 3], base_ddot_com(i) - dot_u[i + 3]);
}
*acc_error += BT_ID_POW(base_ddot_com(i) - dot_u(i + 3), 2);
}
for (int i = 0; i < 3; i++)
{
if (verbose)
{
printf("bt::base_dot_omega(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_dot_omega(i),
i, dot_u[i], base_dot_omega(i) - dot_u[i]);
}
*acc_error += BT_ID_POW(base_dot_omega(i) - dot_u(i), 2);
}
for (int i = 0; i < 3; i++)
{
if (verbose)
{
printf("bt::base_ddot_com(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_ddot_com(i),
i, dot_u[i + 3], base_ddot_com(i) - dot_u[i + 3]);
}
*acc_error += BT_ID_POW(base_ddot_com(i) - dot_u(i + 3), 2);
}
for (int i = 0; i < btmb->getNumDofs(); i++) {
if (verbose) {
printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i],
dot_u(i + 6), joint_accel[i] - dot_u(i + 6));
}
*acc_error += BT_ID_POW(joint_accel[i] - dot_u(i + 6), 2);
}
}
*acc_error = std::sqrt(*acc_error);
if (verbose) {
printf("======dynamics-err: %e\n", *acc_error);
}
*pos_error = 0.0;
for (int i = 0; i < btmb->getNumDofs(); i++)
{
if (verbose)
{
printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i],
dot_u(i + 6), joint_accel[i] - dot_u(i + 6));
}
*acc_error += BT_ID_POW(joint_accel[i] - dot_u(i + 6), 2);
}
}
*acc_error = std::sqrt(*acc_error);
if (verbose)
{
printf("======dynamics-err: %e\n", *acc_error);
}
*pos_error = 0.0;
{
mat33 world_T_body;
if (-1 == id_tree->getBodyTransform(0, &world_T_body)) {
bt_id_error_message("getting transform for body %d\n", 0);
return -1;
}
vec3 world_com;
if (-1 == id_tree->getBodyCoM(0, &world_com)) {
bt_id_error_message("getting com for body %d\n", 0);
return -1;
}
if (verbose) {
printf("id:com: %f %f %f\n", world_com(0), world_com(1), world_com(2));
{
mat33 world_T_body;
if (-1 == id_tree->getBodyTransform(0, &world_T_body))
{
bt_id_error_message("getting transform for body %d\n", 0);
return -1;
}
vec3 world_com;
if (-1 == id_tree->getBodyCoM(0, &world_com))
{
bt_id_error_message("getting com for body %d\n", 0);
return -1;
}
if (verbose)
{
printf("id:com: %f %f %f\n", world_com(0), world_com(1), world_com(2));
printf("id:transform: %f %f %f\n"
" %f %f %f\n"
" %f %f %f\n",
world_T_body(0, 0), world_T_body(0, 1), world_T_body(0, 2), world_T_body(1, 0),
world_T_body(1, 1), world_T_body(1, 2), world_T_body(2, 0), world_T_body(2, 1),
world_T_body(2, 2));
}
}
printf(
"id:transform: %f %f %f\n"
" %f %f %f\n"
" %f %f %f\n",
world_T_body(0, 0), world_T_body(0, 1), world_T_body(0, 2), world_T_body(1, 0),
world_T_body(1, 1), world_T_body(1, 2), world_T_body(2, 0), world_T_body(2, 1),
world_T_body(2, 2));
}
}
for (int l = 0; l < btmb->getNumLinks(); l++) {
const btMultibodyLink &bt_link = btmb->getLink(l);
for (int l = 0; l < btmb->getNumLinks(); l++)
{
const btMultibodyLink &bt_link = btmb->getLink(l);
vec3 bt_origin = bt_link.m_cachedWorldTransform.getOrigin();
mat33 bt_basis = bt_link.m_cachedWorldTransform.getBasis();
if (verbose) {
printf("------------- link %d\n", l + 1);
printf("bt:com: %f %f %f\n", bt_origin(0), bt_origin(1), bt_origin(2));
printf("bt:transform: %f %f %f\n"
" %f %f %f\n"
" %f %f %f\n",
bt_basis(0, 0), bt_basis(0, 1), bt_basis(0, 2), bt_basis(1, 0), bt_basis(1, 1),
bt_basis(1, 2), bt_basis(2, 0), bt_basis(2, 1), bt_basis(2, 2));
}
mat33 id_world_T_body;
vec3 id_world_com;
vec3 bt_origin = bt_link.m_cachedWorldTransform.getOrigin();
mat33 bt_basis = bt_link.m_cachedWorldTransform.getBasis();
if (verbose)
{
printf("------------- link %d\n", l + 1);
printf("bt:com: %f %f %f\n", bt_origin(0), bt_origin(1), bt_origin(2));
printf(
"bt:transform: %f %f %f\n"
" %f %f %f\n"
" %f %f %f\n",
bt_basis(0, 0), bt_basis(0, 1), bt_basis(0, 2), bt_basis(1, 0), bt_basis(1, 1),
bt_basis(1, 2), bt_basis(2, 0), bt_basis(2, 1), bt_basis(2, 2));
}
mat33 id_world_T_body;
vec3 id_world_com;
if (-1 == id_tree->getBodyTransform(l + 1, &id_world_T_body)) {
bt_id_error_message("getting transform for body %d\n", l);
return -1;
}
if (-1 == id_tree->getBodyCoM(l + 1, &id_world_com)) {
bt_id_error_message("getting com for body %d\n", l);
return -1;
}
if (verbose) {
printf("id:com: %f %f %f\n", id_world_com(0), id_world_com(1), id_world_com(2));
if (-1 == id_tree->getBodyTransform(l + 1, &id_world_T_body))
{
bt_id_error_message("getting transform for body %d\n", l);
return -1;
}
if (-1 == id_tree->getBodyCoM(l + 1, &id_world_com))
{
bt_id_error_message("getting com for body %d\n", l);
return -1;
}
if (verbose)
{
printf("id:com: %f %f %f\n", id_world_com(0), id_world_com(1), id_world_com(2));
printf("id:transform: %f %f %f\n"
" %f %f %f\n"
" %f %f %f\n",
id_world_T_body(0, 0), id_world_T_body(0, 1), id_world_T_body(0, 2),
id_world_T_body(1, 0), id_world_T_body(1, 1), id_world_T_body(1, 2),
id_world_T_body(2, 0), id_world_T_body(2, 1), id_world_T_body(2, 2));
}
vec3 diff_com = bt_origin - id_world_com;
mat33 diff_basis = bt_basis - id_world_T_body;
if (verbose) {
printf("diff-com: %e %e %e\n", diff_com(0), diff_com(1), diff_com(2));
printf(
"id:transform: %f %f %f\n"
" %f %f %f\n"
" %f %f %f\n",
id_world_T_body(0, 0), id_world_T_body(0, 1), id_world_T_body(0, 2),
id_world_T_body(1, 0), id_world_T_body(1, 1), id_world_T_body(1, 2),
id_world_T_body(2, 0), id_world_T_body(2, 1), id_world_T_body(2, 2));
}
vec3 diff_com = bt_origin - id_world_com;
mat33 diff_basis = bt_basis - id_world_T_body;
if (verbose)
{
printf("diff-com: %e %e %e\n", diff_com(0), diff_com(1), diff_com(2));
printf("diff-transform: %e %e %e %e %e %e %e %e %e\n", diff_basis(0, 0),
diff_basis(0, 1), diff_basis(0, 2), diff_basis(1, 0), diff_basis(1, 1),
diff_basis(1, 2), diff_basis(2, 0), diff_basis(2, 1), diff_basis(2, 2));
}
double total_pos_err =
BT_ID_SQRT(BT_ID_POW(diff_com(0), 2) + BT_ID_POW(diff_com(1), 2) +
BT_ID_POW(diff_com(2), 2) + BT_ID_POW(diff_basis(0, 0), 2) +
BT_ID_POW(diff_basis(0, 1), 2) + BT_ID_POW(diff_basis(0, 2), 2) +
BT_ID_POW(diff_basis(1, 0), 2) + BT_ID_POW(diff_basis(1, 1), 2) +
BT_ID_POW(diff_basis(1, 2), 2) + BT_ID_POW(diff_basis(2, 0), 2) +
BT_ID_POW(diff_basis(2, 1), 2) + BT_ID_POW(diff_basis(2, 2), 2));
if (verbose) {
printf("======kin-pos-err: %e\n", total_pos_err);
}
if (total_pos_err > *pos_error) {
*pos_error = total_pos_err;
}
}
printf("diff-transform: %e %e %e %e %e %e %e %e %e\n", diff_basis(0, 0),
diff_basis(0, 1), diff_basis(0, 2), diff_basis(1, 0), diff_basis(1, 1),
diff_basis(1, 2), diff_basis(2, 0), diff_basis(2, 1), diff_basis(2, 2));
}
double total_pos_err =
BT_ID_SQRT(BT_ID_POW(diff_com(0), 2) + BT_ID_POW(diff_com(1), 2) +
BT_ID_POW(diff_com(2), 2) + BT_ID_POW(diff_basis(0, 0), 2) +
BT_ID_POW(diff_basis(0, 1), 2) + BT_ID_POW(diff_basis(0, 2), 2) +
BT_ID_POW(diff_basis(1, 0), 2) + BT_ID_POW(diff_basis(1, 1), 2) +
BT_ID_POW(diff_basis(1, 2), 2) + BT_ID_POW(diff_basis(2, 0), 2) +
BT_ID_POW(diff_basis(2, 1), 2) + BT_ID_POW(diff_basis(2, 2), 2));
if (verbose)
{
printf("======kin-pos-err: %e\n", total_pos_err);
}
if (total_pos_err > *pos_error)
{
*pos_error = total_pos_err;
}
}
return 0;
}
return 0;
}
} // namespace btInverseDynamics

9
Extras/InverseDynamics/invdyn_bullet_comparison.hpp
View File

@ -6,7 +6,8 @@
class btMultiBody;
class btVector3;
namespace btInverseDynamics {
namespace btInverseDynamics
{
class MultiBodyTree;
/// this function compares the forward dynamics computations implemented in btMultiBody to
@ -29,7 +30,7 @@ class MultiBodyTree;
/// computed in step 3 relative to dot_u
/// @return -1 on error, 0 on success
int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &gravity, bool verbose,
btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error,
double *acc_error);
}
btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error,
double *acc_error);
} // namespace btInverseDynamics
#endif // INVDYN_BULLET_COMPARISON_HPP

104
Extras/Serialize/BlenderSerialize/bBlenderFile.cpp
View File

@ -27,32 +27,27 @@ extern int DNAlen;
extern unsigned char DNAstr64[];
extern int DNAlen64;
using namespace bParse;
bBlenderFile::bBlenderFile(const char* fileName)
:bFile(fileName, "BLENDER")
bBlenderFile::bBlenderFile(const char *fileName)
: bFile(fileName, "BLENDER")
{
mMain= new bMain(this, fileName, mVersion);
mMain = new bMain(this, fileName, mVersion);
}
bBlenderFile::bBlenderFile(char *memoryBuffer, int len)
:bFile(memoryBuffer,len, "BLENDER"),
mMain(0)
: bFile(memoryBuffer, len, "BLENDER"),
mMain(0)
{
mMain= new bMain(this, "memoryBuf", mVersion);
mMain = new bMain(this, "memoryBuf", mVersion);
}
bBlenderFile::~bBlenderFile()
{
delete mMain;
}
bMain* bBlenderFile::getMain()
bMain *bBlenderFile::getMain()
{
return mMain;
}
@ -60,20 +55,17 @@ bMain* bBlenderFile::getMain()
// ----------------------------------------------------- //
void bBlenderFile::parseData()
{
// printf ("Building datablocks\n");
// printf ("Chunk size = %d\n",CHUNK_HEADER_LEN);
// printf ("File chunk size = %d\n", ChunkUtils::getOffset(mFlags));
// printf ("Building datablocks\n");
// printf ("Chunk size = %d\n",CHUNK_HEADER_LEN);
// printf ("File chunk size = %d\n", ChunkUtils::getOffset(mFlags));
const bool swap = (mFlags&FD_ENDIAN_SWAP)!=0;
const bool swap = (mFlags & FD_ENDIAN_SWAP) != 0;
char *dataPtr = mFileBuffer+mDataStart;
char *dataPtr = mFileBuffer + mDataStart;
bChunkInd dataChunk;
dataChunk.code = 0;
//dataPtr += ChunkUtils::getNextBlock(&dataChunk, dataPtr, mFlags);
int seek = getNextBlock(&dataChunk, dataPtr, mFlags);
//dataPtr += ChunkUtils::getOffset(mFlags);
@ -81,55 +73,46 @@ void bBlenderFile::parseData()
while (dataChunk.code != DNA1)
{
// one behind
if (dataChunk.code == SDNA) break;
//if (dataChunk.code == DNA1) break;
// same as (BHEAD+DATA dependency)
dataPtrHead = dataPtr+ChunkUtils::getOffset(mFlags);
dataPtrHead = dataPtr + ChunkUtils::getOffset(mFlags);
char *id = readStruct(dataPtrHead, dataChunk);
// lookup maps
if (id)
{
m_chunkPtrPtrMap.insert(dataChunk.oldPtr, dataChunk);
mLibPointers.insert(dataChunk.oldPtr, (bStructHandle*)id);
m_chunkPtrPtrMap.insert(dataChunk.oldPtr, dataChunk);
mLibPointers.insert(dataChunk.oldPtr, (bStructHandle *)id);
m_chunks.push_back(dataChunk);
// block it
bListBasePtr *listID = mMain->getListBasePtr(dataChunk.code);
if (listID)
listID->push_back((bStructHandle*)id);
listID->push_back((bStructHandle *)id);
}
if (dataChunk.code == GLOB)
{
m_glob = (bStructHandle*) id;
m_glob = (bStructHandle *)id;
}
// next please!
dataPtr += seek;
seek = getNextBlock(&dataChunk, dataPtr, mFlags);
seek = getNextBlock(&dataChunk, dataPtr, mFlags);
if (seek < 0)
break;
}
}
void bBlenderFile::addDataBlock(char* dataBlock)
void bBlenderFile::addDataBlock(char *dataBlock)
{
mMain->addDatablock(dataBlock);
}
// 32 && 64 bit versions
extern unsigned char DNAstr[];
extern int DNAlen;
@ -137,88 +120,87 @@ extern int DNAlen;
//unsigned char DNAstr[]={0};
//int DNAlen=0;
extern unsigned char DNAstr64[];
extern int DNAlen64;
void bBlenderFile::writeDNA(FILE* fp)
void bBlenderFile::writeDNA(FILE *fp)
{
bChunkInd dataChunk;
dataChunk.code = DNA1;
dataChunk.dna_nr = 0;
dataChunk.nr = 1;
if (VOID_IS_8)
{
dataChunk.len = DNAlen64;
dataChunk.oldPtr = DNAstr64;
fwrite(&dataChunk,sizeof(bChunkInd),1,fp);
fwrite(DNAstr64, DNAlen64,1,fp);
fwrite(&dataChunk, sizeof(bChunkInd), 1, fp);
fwrite(DNAstr64, DNAlen64, 1, fp);
}
else
{
dataChunk.len = DNAlen;
dataChunk.oldPtr = DNAstr;
fwrite(&dataChunk,sizeof(bChunkInd),1,fp);
fwrite(DNAstr, DNAlen,1,fp);
fwrite(&dataChunk, sizeof(bChunkInd), 1, fp);
fwrite(DNAstr, DNAlen, 1, fp);
}
}
void bBlenderFile::parse(int verboseMode)
void bBlenderFile::parse(int verboseMode)
{
if (VOID_IS_8)
{
parseInternal(verboseMode,(char*)DNAstr64,DNAlen64);
parseInternal(verboseMode, (char *)DNAstr64, DNAlen64);
}
else
{
parseInternal(verboseMode,(char*)DNAstr,DNAlen);
parseInternal(verboseMode, (char *)DNAstr, DNAlen);
}
}
// experimental
int bBlenderFile::write(const char* fileName, bool fixupPointers)
int bBlenderFile::write(const char *fileName, bool fixupPointers)
{
FILE *fp = fopen(fileName, "wb");
if (fp)
{
char header[SIZEOFBLENDERHEADER] ;
char header[SIZEOFBLENDERHEADER];
memcpy(header, m_headerString, 7);
int endian= 1;
endian= ((char*)&endian)[0];
int endian = 1;
endian = ((char *)&endian)[0];
if (endian)
{
header[7] = '_';
} else
}
else
{
header[7] = '-';
}
if (VOID_IS_8)
{
header[8]='V';
} else
header[8] = 'V';
}
else
{
header[8]='v';
header[8] = 'v';
}
header[9] = '2';
header[10] = '4';
header[11] = '9';
fwrite(header,SIZEOFBLENDERHEADER,1,fp);
fwrite(header, SIZEOFBLENDERHEADER, 1, fp);
writeChunks(fp, fixupPointers);
writeDNA(fp);
fclose(fp);
} else
}
else
{
printf("Error: cannot open file %s for writing\n",fileName);
printf("Error: cannot open file %s for writing\n", fileName);
return 0;
}
return 1;

65
Extras/Serialize/BlenderSerialize/bBlenderFile.h
View File

@ -16,48 +16,43 @@ subject to the following restrictions:
#ifndef B_BLENDER_FILE_H
#define B_BLENDER_FILE_H
#include "bFile.h"
namespace bParse {
namespace bParse
{
// ----------------------------------------------------- //
class bBlenderFile : public bFile
{
protected:
bMain* mMain;
// ----------------------------------------------------- //
class bBlenderFile : public bFile
bStructHandle* m_glob;
public:
bBlenderFile(const char* fileName);
bBlenderFile(char* memoryBuffer, int len);
virtual ~bBlenderFile();
bMain* getMain();
virtual void addDataBlock(char* dataBlock);
bStructHandle* getFileGlobal()
{
return m_glob;
}
protected:
bMain* mMain;
// experimental
virtual int write(const char* fileName, bool fixupPointers = false);
bStructHandle* m_glob;
virtual void parse(int verboseMode);
public:
virtual void parseData();
bBlenderFile(const char* fileName);
bBlenderFile(char *memoryBuffer, int len);
virtual ~bBlenderFile();
bMain* getMain();
virtual void addDataBlock(char* dataBlock);
bStructHandle* getFileGlobal()
{
return m_glob;
}
// experimental
virtual int write(const char* fileName, bool fixupPointers = false);
virtual void parse(int verboseMode);
virtual void parseData();
virtual void writeDNA(FILE* fp);
};
virtual void writeDNA(FILE* fp);
};
}; // namespace bParse
#endif //B_BLENDER_FILE_H
#endif //B_BLENDER_FILE_H

92
Extras/Serialize/BlenderSerialize/bMain.cpp
View File

@ -21,51 +21,49 @@ subject to the following restrictions:
using namespace bParse;
// ----------------------------------------------------- //
bMain::bMain(bBlenderFile *filePtr, const char *baseName, int fileVersion)
: mFP(filePtr),
mVersion(fileVersion),
mName(baseName)
: mFP(filePtr),
mVersion(fileVersion),
mName(baseName)
{
mData.insert(ID_SCE,bListBasePtr());
mData.insert(ID_LI,bListBasePtr());
mData.insert(ID_OB,bListBasePtr());
mData.insert(ID_ME,bListBasePtr());
mData.insert(ID_CU,bListBasePtr());
mData.insert(ID_MB,bListBasePtr());
mData.insert(ID_MA,bListBasePtr());
mData.insert(ID_TE,bListBasePtr());
mData.insert(ID_IM,bListBasePtr());
mData.insert(ID_WV,bListBasePtr());
mData.insert(ID_LT,bListBasePtr());
mData.insert(ID_LA,bListBasePtr());
mData.insert(ID_CA,bListBasePtr());
mData.insert(ID_IP,bListBasePtr());
mData.insert(ID_KE,bListBasePtr());
mData.insert(ID_WO,bListBasePtr());
mData.insert(ID_SCR,bListBasePtr());
mData.insert(ID_VF,bListBasePtr());
mData.insert(ID_TXT,bListBasePtr());
mData.insert(ID_SO,bListBasePtr());
mData.insert(ID_GR,bListBasePtr());
mData.insert(ID_AR,bListBasePtr());
mData.insert(ID_AC,bListBasePtr());
mData.insert(ID_NT,bListBasePtr());
mData.insert(ID_BR,bListBasePtr());
mData.insert(ID_SCE, bListBasePtr());
mData.insert(ID_LI, bListBasePtr());
mData.insert(ID_OB, bListBasePtr());
mData.insert(ID_ME, bListBasePtr());
mData.insert(ID_CU, bListBasePtr());
mData.insert(ID_MB, bListBasePtr());
mData.insert(ID_MA, bListBasePtr());
mData.insert(ID_TE, bListBasePtr());
mData.insert(ID_IM, bListBasePtr());
mData.insert(ID_WV, bListBasePtr());
mData.insert(ID_LT, bListBasePtr());
mData.insert(ID_LA, bListBasePtr());
mData.insert(ID_CA, bListBasePtr());
mData.insert(ID_IP, bListBasePtr());
mData.insert(ID_KE, bListBasePtr());
mData.insert(ID_WO, bListBasePtr());
mData.insert(ID_SCR, bListBasePtr());
mData.insert(ID_VF, bListBasePtr());
mData.insert(ID_TXT, bListBasePtr());
mData.insert(ID_SO, bListBasePtr());
mData.insert(ID_GR, bListBasePtr());
mData.insert(ID_AR, bListBasePtr());
mData.insert(ID_AC, bListBasePtr());
mData.insert(ID_NT, bListBasePtr());
mData.insert(ID_BR, bListBasePtr());
mData.insert(ID_SCRIPT, bListBasePtr());
}
// ----------------------------------------------------- //
bMain::~bMain()
{
// allocated data blocks!
int sz = mPool.size();
for (int i=0;i<sz;i++)
for (int i = 0; i < sz; i++)
{
delete [] mPool[i];
delete[] mPool[i];
}
}
@ -85,30 +83,25 @@ const char *bMain::getName()
void bMain::addDatablock(void *allocated)
{
assert(allocated);
mPool.push_back((bStructHandle*)allocated);
mPool.push_back((bStructHandle *)allocated);
}
// ------------------------------------------------------------//
void bMain::linkList(void *listBasePtr)
{
struct ListBase // local Blender::ListBase
struct ListBase // local Blender::ListBase
{
void *first;
void *last;
};
struct Link // local Blender::Link
struct Link // local Blender::Link
{
void *next;
void *prev;
};
ListBase *base = (ListBase*)listBasePtr;
ListBase *base = (ListBase *)listBasePtr;
if (!base || !base->first)
return;
@ -121,18 +114,18 @@ void bMain::linkList(void *listBasePtr)
}
void *prev = 0;
Link *l = (Link*)base->first;
Link *l = (Link *)base->first;
while (l)
{
l->next = mFP->findLibPointer(l->next);
l->prev = l->next;
prev = l->next;
l = (Link*)l->next;
l = (Link *)l->next;
}
}
// ------------------------------------------------------------//
bListBasePtr* bMain::getListBasePtr(int listBaseCode)
bListBasePtr *bMain::getListBasePtr(int listBaseCode)
{
bListBasePtr *ptr = _findCode(listBaseCode);
if (!ptr)
@ -143,12 +136,10 @@ bListBasePtr* bMain::getListBasePtr(int listBaseCode)
// ------------------------------------------------------------//
bListBasePtr *bMain::_findCode(int code)
{
bListBasePtr* lbPtr = mData.find(code);
bListBasePtr *lbPtr = mData.find(code);
return lbPtr;
}
// ------------------------------------------------------------//
bListBasePtr *bMain::getScene()
{
@ -193,8 +184,6 @@ bListBasePtr *bMain::getCurve()
return ptr;
}
// ------------------------------------------------------------//
bListBasePtr *bMain::getMball()
{
@ -222,7 +211,6 @@ bListBasePtr *bMain::getTex()
return ptr;
}
// ------------------------------------------------------------//
bListBasePtr *bMain::getImage()
{
@ -295,7 +283,6 @@ bListBasePtr *bMain::getWorld()
return ptr;
}
// ------------------------------------------------------------//
bListBasePtr *bMain::getScreen()
{
@ -368,7 +355,6 @@ bListBasePtr *bMain::getAction()
return ptr;
}
// ------------------------------------------------------------//
bListBasePtr *bMain::getNodetree()
{
@ -387,6 +373,4 @@ bListBasePtr *bMain::getBrush()
return ptr;
}
//eof

128
Extras/Serialize/BlenderSerialize/bMain.h
View File

@ -20,91 +20,77 @@ subject to the following restrictions:
#include "bChunk.h"
#include "LinearMath/btHashMap.h"
namespace bParse
{
class bDNA;
class bBlenderFile;
}; // namespace bParse
namespace bParse
{
class bDNA;
// ----------------------------------------------------- //
class bBlenderFile;
};
typedef btHashMap<btHashInt, bListBasePtr> bMainDataMap;
namespace bParse {
// ----------------------------------------------------- //
typedef btHashMap<btHashInt,bListBasePtr> bMainDataMap;
// ----------------------------------------------------- //
class bMain
{
// ----------------------------------------------------- //
class bMain
{
//private:
public:
bBlenderFile* mFP;
bListBasePtr mPool;
public:
bBlenderFile *mFP;
bListBasePtr mPool;
int mVersion;
const char* mName;
int mVersion;
const char *mName;
bMainDataMap mData;
bMainDataMap mData;
bListBasePtr *_findCode(int code);
public:
bMain(bBlenderFile *filePtr, const char *baseName, int fileVersion);
~bMain();
bListBasePtr *_findCode(int code);
int getVersion();
const char *getName();
public:
bMain(bBlenderFile *filePtr, const char *baseName, int fileVersion);
~bMain();
bListBasePtr *getListBasePtr(int listBaseCode);
int getVersion();
const char *getName();
bListBasePtr *getScene();
bListBasePtr *getLibrary();
bListBasePtr *getObject();
bListBasePtr *getMesh();
bListBasePtr *getCurve();
bListBasePtr *getMball();
bListBasePtr *getMat();
bListBasePtr *getTex();
bListBasePtr *getImage();
bListBasePtr *getWave();
bListBasePtr *getLatt();
bListBasePtr *getLamp();
bListBasePtr *getCamera();
bListBasePtr *getIpo();
bListBasePtr *getKey();
bListBasePtr *getWorld();
bListBasePtr *getScreen();
bListBasePtr *getScript();
bListBasePtr *getVfont();
bListBasePtr *getText();
bListBasePtr *getSound();
bListBasePtr *getGroup();
bListBasePtr *getArmature();
bListBasePtr *getAction();
bListBasePtr *getNodetree();
bListBasePtr *getBrush();
bListBasePtr *getListBasePtr(int listBaseCode);
// tracking allocated memory
void addDatablock(void *allocated);
// --
bListBasePtr *getScene();
bListBasePtr *getLibrary();
bListBasePtr *getObject();
bListBasePtr *getMesh();
bListBasePtr *getCurve();
bListBasePtr *getMball();
bListBasePtr *getMat();
bListBasePtr *getTex();
bListBasePtr *getImage();
bListBasePtr *getWave();
bListBasePtr *getLatt();
bListBasePtr *getLamp();
bListBasePtr *getCamera();
bListBasePtr *getIpo();
bListBasePtr *getKey();
bListBasePtr *getWorld();
bListBasePtr *getScreen();
bListBasePtr *getScript();
bListBasePtr *getVfont();
bListBasePtr *getText();
bListBasePtr *getSound();
bListBasePtr *getGroup();
bListBasePtr *getArmature();
bListBasePtr *getAction();
bListBasePtr *getNodetree();
bListBasePtr *getBrush();
void linkList(void *listBasePtr);
};
} // namespace bParse
// tracking allocated memory
void addDatablock(void *allocated);
// --
void linkList(void *listBasePtr);
};
}
#endif//__BMAIN_H__
#endif //__BMAIN_H__

2793
Extras/Serialize/BlenderSerialize/dna249-64bit.cpp
View File

File diff suppressed because it is too large Load Diff

2793
Extras/Serialize/BlenderSerialize/dna249.cpp
View File

File diff suppressed because it is too large Load Diff

3021
Extras/Serialize/BulletFileLoader/autogenerated/bullet.h
View File

File diff suppressed because it is too large Load Diff

12
Extras/Serialize/BulletFileLoader/bChunk.cpp
View File

@ -17,15 +17,13 @@ subject to the following restrictions:
#include "bDefines.h"
#include "bFile.h"
#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__)
#if !defined(__CELLOS_LV2__) && !defined(__MWERKS__)
#include <memory.h>
#endif
#include <string.h>
using namespace bParse;
// ----------------------------------------------------- //
short ChunkUtils::swapShort(short sht)
{
@ -57,19 +55,15 @@ int ChunkUtils::getOffset(int flags)
if (VOID_IS_8)
{
if (flags &FD_BITS_VARIES)
if (flags & FD_BITS_VARIES)
res = sizeof(bChunkPtr4);
}
else
{
if (flags &FD_BITS_VARIES)
if (flags & FD_BITS_VARIES)
res = sizeof(bChunkPtr8);
}
return res;
}
//eof

118
Extras/Serialize/BulletFileLoader/bChunk.h
View File

@ -16,77 +16,69 @@ subject to the following restrictions:
#ifndef __BCHUNK_H__
#define __BCHUNK_H__
#if defined (_WIN32) && ! defined (__MINGW32__)
#define long64 __int64
#elif defined (__MINGW32__)
#include <stdint.h>
#define long64 int64_t
#if defined(_WIN32) && !defined(__MINGW32__)
#define long64 __int64
#elif defined(__MINGW32__)
#include <stdint.h>
#define long64 int64_t
#else
#define long64 long long
#define long64 long long
#endif
namespace bParse {
// ----------------------------------------------------- //
class bChunkPtr4
{
public:
bChunkPtr4(){}
int code;
int len;
union
{
int m_uniqueInt;
};
int dna_nr;
int nr;
namespace bParse
{
// ----------------------------------------------------- //
class bChunkPtr4
{
public:
bChunkPtr4() {}
int code;
int len;
union {
int m_uniqueInt;
};
int dna_nr;
int nr;
};
// ----------------------------------------------------- //
class bChunkPtr8
{
public:
bChunkPtr8(){}
int code, len;
union
{
long64 oldPrev;
int m_uniqueInts[2];
};
int dna_nr, nr;
// ----------------------------------------------------- //
class bChunkPtr8
{
public:
bChunkPtr8() {}
int code, len;
union {
long64 oldPrev;
int m_uniqueInts[2];
};
int dna_nr, nr;
};
// ----------------------------------------------------- //
class bChunkInd
{
public:
bChunkInd(){}
int code, len;
void *oldPtr;
int dna_nr, nr;
};
// ----------------------------------------------------- //
class bChunkInd
{
public:
bChunkInd() {}
int code, len;
void *oldPtr;
int dna_nr, nr;
};
// ----------------------------------------------------- //
class ChunkUtils
{
public:
// file chunk offset
static int getOffset(int flags);
// ----------------------------------------------------- //
class ChunkUtils
{
public:
// file chunk offset
static int getOffset(int flags);
// endian utils
static short swapShort(short sht);
static int swapInt(int inte);
static long64 swapLong64(long64 lng);
};
// endian utils
static short swapShort(short sht);
static int swapInt(int inte);
static long64 swapLong64(long64 lng);
const int CHUNK_HEADER_LEN = ((sizeof(bChunkInd)));
const bool VOID_IS_8 = ((sizeof(void *) == 8));
} // namespace bParse
};
const int CHUNK_HEADER_LEN = ((sizeof(bChunkInd)));
const bool VOID_IS_8 = ((sizeof(void*)==8));
}
#endif//__BCHUNK_H__
#endif //__BCHUNK_H__

29
Extras/Serialize/BulletFileLoader/bCommon.h
View File

@ -16,24 +16,25 @@ subject to the following restrictions:
#ifndef __BCOMMON_H__
#define __BCOMMON_H__
#include <assert.h>
//#include "bLog.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btHashMap.h"
namespace bParse {
namespace bParse
{
class bMain;
class bFileData;
class bFile;
class bDNA;
class bMain;
class bFileData;
class bFile;
class bDNA;
// delete void* undefined
typedef struct bStructHandle
{
int unused;
} bStructHandle;
typedef btAlignedObjectArray<bStructHandle*> bListBasePtr;
typedef btHashMap<btHashPtr, bStructHandle*> bPtrMap;
} // namespace bParse
// delete void* undefined
typedef struct bStructHandle {int unused;}bStructHandle;
typedef btAlignedObjectArray<bStructHandle*> bListBasePtr;
typedef btHashMap<btHashPtr, bStructHandle*> bPtrMap;
}
#endif//__BCOMMON_H__
#endif //__BCOMMON_H__

360
Extras/Serialize/BulletFileLoader/bDNA.cpp
View File

@ -23,13 +23,11 @@ subject to the following restrictions:
//this define will force traversal of structures, to check backward (and forward) compatibility
//#define TEST_BACKWARD_FORWARD_COMPATIBILITY
using namespace bParse;
// ----------------------------------------------------- //
bDNA::bDNA()
: mPtrLen(0)
: mPtrLen(0)
{
// --
}
@ -43,7 +41,7 @@ bDNA::~bDNA()
// ----------------------------------------------------- //
bool bDNA::lessThan(bDNA *file)
{
return ( m_Names.size() < file->m_Names.size());
return (m_Names.size() < file->m_Names.size());
}
// ----------------------------------------------------- //
@ -53,36 +51,31 @@ char *bDNA::getName(int ind)
return m_Names[ind].m_name;
}
// ----------------------------------------------------- //
char *bDNA::getType(int ind)
{
assert(ind<= (int)mTypes.size());
assert(ind <= (int)mTypes.size());
return mTypes[ind];
}
// ----------------------------------------------------- //
short *bDNA::getStruct(int ind)
{
assert(ind <= (int)mStructs.size());
assert(ind <= (int)mStructs.size());
return mStructs[ind];
}
// ----------------------------------------------------- //
short bDNA::getLength(int ind)
{
assert(ind <= (int)mTlens.size());
assert(ind <= (int)mTlens.size());
return mTlens[ind];
}
// ----------------------------------------------------- //
int bDNA::getReverseType(short type)
{
int* intPtr = mStructReverse.find(type);
int *intPtr = mStructReverse.find(type);
if (intPtr)
return *intPtr;
@ -92,12 +85,11 @@ int bDNA::getReverseType(short type)
// ----------------------------------------------------- //
int bDNA::getReverseType(const char *type)
{
btHashString key(type);
int* valuePtr = mTypeLookup.find(key);
int *valuePtr = mTypeLookup.find(key);
if (valuePtr)
return *valuePtr;
return -1;
}
@ -110,22 +102,22 @@ int bDNA::getNumStructs()
// ----------------------------------------------------- //
bool bDNA::flagNotEqual(int dna_nr)
{
assert(dna_nr <= (int)mCMPFlags.size());
assert(dna_nr <= (int)mCMPFlags.size());
return mCMPFlags[dna_nr] == FDF_STRUCT_NEQU;
}
// ----------------------------------------------------- //
bool bDNA::flagEqual(int dna_nr)
{
assert(dna_nr <= (int)mCMPFlags.size());
assert(dna_nr <= (int)mCMPFlags.size());
int flag = mCMPFlags[dna_nr];
return flag == FDF_STRUCT_EQU;
return flag == FDF_STRUCT_EQU;
}
// ----------------------------------------------------- //
bool bDNA::flagNone(int dna_nr)
{
assert(dna_nr <= (int)mCMPFlags.size());
assert(dna_nr <= (int)mCMPFlags.size());
return mCMPFlags[dna_nr] == FDF_NONE;
}
@ -143,15 +135,15 @@ void bDNA::initRecurseCmpFlags(int iter)
short *oldStrc = mStructs[iter];
short type = oldStrc[0];
for (int i=0; i<(int)mStructs.size(); i++)
for (int i = 0; i < (int)mStructs.size(); i++)
{
if (i != iter && mCMPFlags[i] == FDF_STRUCT_EQU )
if (i != iter && mCMPFlags[i] == FDF_STRUCT_EQU)
{
short *curStruct = mStructs[i];
int eleLen = curStruct[1];
curStruct+=2;
curStruct += 2;
for (int j=0; j<eleLen; j++, curStruct+=2)
for (int j = 0; j < eleLen; j++, curStruct += 2)
{
if (curStruct[0] == type)
{
@ -171,19 +163,15 @@ void bDNA::initRecurseCmpFlags(int iter)
// ----------------------------------------------------- //
void bDNA::initCmpFlags(bDNA *memDNA)
{
// compare the file to memory
// compare the file to memory
// this ptr should be the file data
assert(!(m_Names.size() == 0));//DNA empty!
assert(!(m_Names.size() == 0)); //DNA empty!
mCMPFlags.resize(mStructs.size(), FDF_NONE);
int i;
for ( i=0; i<(int)mStructs.size(); i++)
for (i = 0; i < (int)mStructs.size(); i++)
{
short *oldStruct = mStructs[i];
@ -197,7 +185,7 @@ void bDNA::initCmpFlags(bDNA *memDNA)
//#define SLOW_FORWARD_COMPATIBLE 1
#ifdef SLOW_FORWARD_COMPATIBLE
char* typeName = mTypes[oldLookup];
char *typeName = mTypes[oldLookup];
int newLookup = memDNA->getReverseType(typeName);
if (newLookup == -1)
{
@ -211,71 +199,61 @@ void bDNA::initCmpFlags(bDNA *memDNA)
if (oldLookup < memDNA->mStructs.size())
{
short *curStruct = memDNA->mStructs[oldLookup];
#endif
#endif
// rebuild...
mCMPFlags[i] = FDF_STRUCT_NEQU;
// rebuild...
mCMPFlags[i] = FDF_STRUCT_NEQU;
#ifndef TEST_BACKWARD_FORWARD_COMPATIBILITY
if (curStruct[1] == oldStruct[1])
if (curStruct[1] == oldStruct[1])
{
// type len same ...
if (mTlens[oldStruct[0]] == memDNA->mTlens[curStruct[0]])
{
// type len same ...
if (mTlens[oldStruct[0]] == memDNA->mTlens[curStruct[0]])
bool isSame = true;
int elementLength = oldStruct[1];
curStruct += 2;
oldStruct += 2;
for (int j = 0; j < elementLength; j++, curStruct += 2, oldStruct += 2)
{
bool isSame = true;
int elementLength = oldStruct[1];
curStruct+=2;
oldStruct+=2;
for (int j=0; j<elementLength; j++, curStruct+=2, oldStruct+=2)
// type the same
//const char* typeFileDNA = mTypes[oldStruct[0]];
//const char* typeMemDNA = mTypes[curStruct[0]];
if (strcmp(mTypes[oldStruct[0]], memDNA->mTypes[curStruct[0]]) != 0)
{
// type the same
//const char* typeFileDNA = mTypes[oldStruct[0]];
//const char* typeMemDNA = mTypes[curStruct[0]];
if (strcmp(mTypes[oldStruct[0]], memDNA->mTypes[curStruct[0]])!=0)
{
isSame=false;
break;
}
// name the same
if (strcmp(m_Names[oldStruct[1]].m_name, memDNA->m_Names[curStruct[1]].m_name)!=0)
{
isSame=false;
break;
}
isSame = false;
break;
}
// name the same
if (strcmp(m_Names[oldStruct[1]].m_name, memDNA->m_Names[curStruct[1]].m_name) != 0)
{
isSame = false;
break;
}
// flag valid ==
if (isSame)
mCMPFlags[i] = FDF_STRUCT_EQU;
}
// flag valid ==
if (isSame)
mCMPFlags[i] = FDF_STRUCT_EQU;
}
#endif
}
}
// recurse in
for ( i=0; i<(int)mStructs.size(); i++)
{
if (mCMPFlags[i] == FDF_STRUCT_NEQU)
initRecurseCmpFlags(i);
#endif
}
}
// recurse in
for (i = 0; i < (int)mStructs.size(); i++)
{
if (mCMPFlags[i] == FDF_STRUCT_NEQU)
initRecurseCmpFlags(i);
}
}
static int name_is_array(char* name, int* dim1, int* dim2) {
static int name_is_array(char *name, int *dim1, int *dim2)
{
int len = strlen(name);
/*fprintf(stderr,"[%s]",name);*/
/*if (len >= 1) {
@ -285,58 +263,77 @@ static int name_is_array(char* name, int* dim1, int* dim2) {
return 0;*/
char *bp;
int num;
if (dim1) {
if (dim1)
{
*dim1 = 1;
}
if (dim2) {
if (dim2)
{
*dim2 = 1;
}
bp = strchr(name, '[');
if (!bp) {
if (!bp)
{
return 0;
}
num = 0;
while (++bp < name+len-1) {
while (++bp < name + len - 1)
{
const char c = *bp;
if (c == ']') {
if (c == ']')
{
break;
}
if (c <= '9' && c >= '0') {
if (c <= '9' && c >= '0')
{
num *= 10;
num += (c - '0');
} else {
}
else
{
printf("array parse error.\n");
return 0;
}
}
if (dim2) {
if (dim2)
{
*dim2 = num;
}
/* find second dim, if any. */
bp = strchr(bp, '[');
if (!bp) {
if (!bp)
{
return 1; /* at least we got the first dim. */
}
num = 0;
while (++bp < name+len-1) {
while (++bp < name + len - 1)
{
const char c = *bp;
if (c == ']') {
if (c == ']')
{
break;
}
if (c <= '9' && c >= '0') {
if (c <= '9' && c >= '0')
{
num *= 10;
num += (c - '0');
} else {
}
else
{
printf("array2 parse error.\n");
return 1;
}
}
if (dim1) {
if (dim2) {
if (dim1)
{
if (dim2)
{
*dim1 = *dim2;
*dim2 = num;
} else {
}
else
{
*dim1 = num;
}
}
@ -344,14 +341,15 @@ static int name_is_array(char* name, int* dim1, int* dim2) {
return 1;
}
// ----------------------------------------------------- //
void bDNA::init(char *data, int len, bool swap)
{
int *intPtr=0;short *shtPtr=0;
char *cp = 0;int dataLen =0;
int *intPtr = 0;
short *shtPtr = 0;
char *cp = 0;
int dataLen = 0;
//long nr=0;
intPtr = (int*)data;
intPtr = (int *)data;
/*
SDNA (4 bytes) (magic number)
@ -361,39 +359,36 @@ void bDNA::init(char *data, int len, bool swap)
<string>
*/
if (strncmp(data, "SDNA", 4)==0)
if (strncmp(data, "SDNA", 4) == 0)
{
// skip ++ NAME
intPtr++; intPtr++;
intPtr++;
intPtr++;
}
// Parse names
if (swap)
if (swap)
{
*intPtr = ChunkUtils::swapInt(*intPtr);
}
dataLen = *intPtr;
intPtr++;
cp = (char*)intPtr;
cp = (char *)intPtr;
int i;
for ( i=0; i<dataLen; i++)
for (i = 0; i < dataLen; i++)
{
bNameInfo info;
info.m_name = cp;
info.m_isPointer = (info.m_name[0] == '*') || (info.m_name[1] == '*');
name_is_array(info.m_name,&info.m_dim0,&info.m_dim1);
name_is_array(info.m_name, &info.m_dim0, &info.m_dim1);
m_Names.push_back(info);
while (*cp)cp++;
while (*cp) cp++;
cp++;
}
cp = btAlignPointer(cp, 4);
cp = btAlignPointer(cp,4);
/*
TYPE (4 bytes)
<nr> amount of types (int)
@ -401,26 +396,27 @@ void bDNA::init(char *data, int len, bool swap)
<string>
*/
intPtr = (int*)cp;
assert(strncmp(cp, "TYPE", 4)==0); intPtr++;
intPtr = (int *)cp;
assert(strncmp(cp, "TYPE", 4) == 0);
intPtr++;
if (swap)
if (swap)
{
*intPtr = ChunkUtils::swapInt(*intPtr);
}
dataLen = *intPtr;
intPtr++;
cp = (char*)intPtr;
for ( i=0; i<dataLen; i++)
cp = (char *)intPtr;
for (i = 0; i < dataLen; i++)
{
mTypes.push_back(cp);
while (*cp)cp++;
while (*cp) cp++;
cp++;
}
cp = btAlignPointer(cp,4);
cp = btAlignPointer(cp, 4);
/*
TLEN (4 bytes)
<len> (short) the lengths of types
@ -428,13 +424,14 @@ void bDNA::init(char *data, int len, bool swap)
*/
// Parse type lens
intPtr = (int*)cp;
assert(strncmp(cp, "TLEN", 4)==0); intPtr++;
intPtr = (int *)cp;
assert(strncmp(cp, "TLEN", 4) == 0);
intPtr++;
dataLen = (int)mTypes.size();
shtPtr = (short*)intPtr;
for ( i=0; i<dataLen; i++, shtPtr++)
shtPtr = (short *)intPtr;
for (i = 0; i < dataLen; i++, shtPtr++)
{
if (swap)
shtPtr[0] = ChunkUtils::swapShort(shtPtr[0]);
@ -454,94 +451,89 @@ void bDNA::init(char *data, int len, bool swap)
<namenr>
*/
intPtr = (int*)shtPtr;
cp = (char*)intPtr;
assert(strncmp(cp, "STRC", 4)==0); intPtr++;
intPtr = (int *)shtPtr;
cp = (char *)intPtr;
assert(strncmp(cp, "STRC", 4) == 0);
intPtr++;
if (swap)
if (swap)
{
*intPtr = ChunkUtils::swapInt(*intPtr);
}
dataLen = *intPtr;
intPtr++;
shtPtr = (short*)intPtr;
for ( i=0; i<dataLen; i++)
shtPtr = (short *)intPtr;
for (i = 0; i < dataLen; i++)
{
mStructs.push_back (shtPtr);
mStructs.push_back(shtPtr);
if (swap)
{
shtPtr[0]= ChunkUtils::swapShort(shtPtr[0]);
shtPtr[1]= ChunkUtils::swapShort(shtPtr[1]);
shtPtr[0] = ChunkUtils::swapShort(shtPtr[0]);
shtPtr[1] = ChunkUtils::swapShort(shtPtr[1]);
int len = shtPtr[1];
shtPtr+= 2;
shtPtr += 2;
for (int a=0; a<len; a++, shtPtr+=2)
for (int a = 0; a < len; a++, shtPtr += 2)
{
shtPtr[0]= ChunkUtils::swapShort(shtPtr[0]);
shtPtr[1]= ChunkUtils::swapShort(shtPtr[1]);
shtPtr[0] = ChunkUtils::swapShort(shtPtr[0]);
shtPtr[1] = ChunkUtils::swapShort(shtPtr[1]);
}
}
else
shtPtr+= (2*shtPtr[1])+2;
shtPtr += (2 * shtPtr[1]) + 2;
}
// build reverse lookups
for ( i=0; i<(int)mStructs.size(); i++)
for (i = 0; i < (int)mStructs.size(); i++)
{
short *strc = mStructs.at(i);
if (!mPtrLen && strcmp(mTypes[strc[0]],"ListBase")==0)
if (!mPtrLen && strcmp(mTypes[strc[0]], "ListBase") == 0)
{
mPtrLen = mTlens[strc[0]]/2;
mPtrLen = mTlens[strc[0]] / 2;
}
mStructReverse.insert(strc[0], i);
mTypeLookup.insert(btHashString(mTypes[strc[0]]),i);
mTypeLookup.insert(btHashString(mTypes[strc[0]]), i);
}
}
// ----------------------------------------------------- //
int bDNA::getArraySize(char* string)
int bDNA::getArraySize(char *string)
{
int ret = 1;
int len = strlen(string);
char* next = 0;
for (int i=0; i<len; i++)
char *next = 0;
for (int i = 0; i < len; i++)
{
char c = string[i];
if (c == '[')
next = &string[i+1];
else if (c==']')
next = &string[i + 1];
else if (c == ']')
if (next)
ret *= atoi(next);
}
// print (string << ' ' << ret);
// print (string << ' ' << ret);
return ret;
}
void bDNA::dumpTypeDefinitions()
{
int i;
int numTypes = mTypes.size();
for (i=0;i<numTypes;i++)
{
for (i = 0; i < numTypes; i++)
{
}
for ( i=0; i<(int)mStructs.size(); i++)
for (i = 0; i < (int)mStructs.size(); i++)
{
int totalBytes=0;
int totalBytes = 0;
short *oldStruct = mStructs[i];
int oldLookup = getReverseType(oldStruct[0]);
@ -551,44 +543,46 @@ void bDNA::dumpTypeDefinitions()
continue;
}
short* newStruct = mStructs[oldLookup];
char* typeName = mTypes[newStruct[0]];
printf("%3d: %s ",i,typeName);
short *newStruct = mStructs[oldLookup];
char *typeName = mTypes[newStruct[0]];
printf("%3d: %s ", i, typeName);
//char *name = mNames[oldStruct[1]];
int len = oldStruct[1];
printf(" (%d fields) ",len);
oldStruct+=2;
printf(" (%d fields) ", len);
oldStruct += 2;
printf("{");
int j;
for (j=0; j<len; ++j,oldStruct+=2) {
const char* name = m_Names[oldStruct[1]].m_name;
printf("%s %s", mTypes[oldStruct[0]],name);
int elemNumBytes= 0;
for (j = 0; j < len; ++j, oldStruct += 2)
{
const char *name = m_Names[oldStruct[1]].m_name;
printf("%s %s", mTypes[oldStruct[0]], name);
int elemNumBytes = 0;
int arrayDimensions = getArraySizeNew(oldStruct[1]);
if (m_Names[oldStruct[1]].m_isPointer)
{
elemNumBytes = VOID_IS_8 ? 8 : 4;
} else
}
else
{
elemNumBytes = getLength(oldStruct[0]);
}
printf(" /* %d bytes */",elemNumBytes*arrayDimensions);
if (j == len-1) {
printf(" /* %d bytes */", elemNumBytes * arrayDimensions);
if (j == len - 1)
{
printf(";}");
} else {
}
else
{
printf("; ");
}
totalBytes+=elemNumBytes*arrayDimensions;
totalBytes += elemNumBytes * arrayDimensions;
}
printf("\ntotalBytes=%d\n\n",totalBytes);
printf("\ntotalBytes=%d\n\n", totalBytes);
}
#if 0
/* dump out display of types and their sizes */
@ -618,12 +612,6 @@ void bDNA::dumpTypeDefinitions()
}
}
#endif
}
//eof

155
Extras/Serialize/BulletFileLoader/bDNA.h
View File

@ -16,95 +16,86 @@ subject to the following restrictions:
#ifndef __BDNA_H__
#define __BDNA_H__
#include "bCommon.h"
namespace bParse {
namespace bParse
{
struct bNameInfo
{
char *m_name;
bool m_isPointer;
int m_dim0;
int m_dim1;
};
struct bNameInfo
class bDNA
{
public:
bDNA();
~bDNA();
void init(char *data, int len, bool swap = false);
int getArraySize(char *str);
int getArraySizeNew(short name)
{
char* m_name;
bool m_isPointer;
int m_dim0;
int m_dim1;
const bNameInfo &nameInfo = m_Names[name];
return nameInfo.m_dim0 * nameInfo.m_dim1;
}
int getElementSize(short type, short name)
{
const bNameInfo &nameInfo = m_Names[name];
int size = nameInfo.m_isPointer ? mPtrLen * nameInfo.m_dim0 * nameInfo.m_dim1 : mTlens[type] * nameInfo.m_dim0 * nameInfo.m_dim1;
return size;
}
int getNumNames() const
{
return m_Names.size();
}
char *getName(int ind);
char *getType(int ind);
short *getStruct(int ind);
short getLength(int ind);
int getReverseType(short type);
int getReverseType(const char *type);
int getNumStructs();
//
bool lessThan(bDNA *other);
void initCmpFlags(bDNA *memDNA);
bool flagNotEqual(int dna_nr);
bool flagEqual(int dna_nr);
bool flagNone(int dna_nr);
int getPointerSize();
void dumpTypeDefinitions();
private:
enum FileDNAFlags
{
FDF_NONE = 0,
FDF_STRUCT_NEQU,
FDF_STRUCT_EQU
};
class bDNA
{
public:
bDNA();
~bDNA();
void initRecurseCmpFlags(int i);
void init(char *data, int len, bool swap=false);
btAlignedObjectArray<int> mCMPFlags;
int getArraySize(char* str);
int getArraySizeNew(short name)
{
const bNameInfo& nameInfo = m_Names[name];
return nameInfo.m_dim0*nameInfo.m_dim1;
}
int getElementSize(short type, short name)
{
const bNameInfo& nameInfo = m_Names[name];
int size = nameInfo.m_isPointer ? mPtrLen*nameInfo.m_dim0*nameInfo.m_dim1 : mTlens[type]*nameInfo.m_dim0*nameInfo.m_dim1;
return size;
}
btAlignedObjectArray<bNameInfo> m_Names;
btAlignedObjectArray<char *> mTypes;
btAlignedObjectArray<short *> mStructs;
btAlignedObjectArray<short> mTlens;
btHashMap<btHashInt, int> mStructReverse;
btHashMap<btHashString, int> mTypeLookup;
int getNumNames() const
{
return m_Names.size();
}
int mPtrLen;
};
} // namespace bParse
char *getName(int ind);
char *getType(int ind);
short *getStruct(int ind);
short getLength(int ind);
int getReverseType(short type);
int getReverseType(const char *type);
int getNumStructs();
//
bool lessThan(bDNA* other);
void initCmpFlags(bDNA *memDNA);
bool flagNotEqual(int dna_nr);
bool flagEqual(int dna_nr);
bool flagNone(int dna_nr);
int getPointerSize();
void dumpTypeDefinitions();
private:
enum FileDNAFlags
{
FDF_NONE=0,
FDF_STRUCT_NEQU,
FDF_STRUCT_EQU
};
void initRecurseCmpFlags(int i);
btAlignedObjectArray<int> mCMPFlags;
btAlignedObjectArray<bNameInfo> m_Names;
btAlignedObjectArray<char*> mTypes;
btAlignedObjectArray<short*> mStructs;
btAlignedObjectArray<short> mTlens;
btHashMap<btHashInt, int> mStructReverse;
btHashMap<btHashString,int> mTypeLookup;
int mPtrLen;
};
}
#endif//__BDNA_H__
#endif //__BDNA_H__

209
Extras/Serialize/BulletFileLoader/bDefines.h
View File

@ -19,121 +19,134 @@
#ifndef __B_DEFINES_H__
#define __B_DEFINES_H__
// MISC defines, see BKE_global.h, BKE_utildefines.h
#define SIZEOFBLENDERHEADER 12
// ------------------------------------------------------------
#if defined(__sgi) || defined (__sparc) || defined (__sparc__) || defined (__PPC__) || defined (__ppc__) || defined (__BIG_ENDIAN__)
# define MAKE_ID(a,b,c,d) ( (int)(a)<<24 | (int)(b)<<16 | (c)<<8 | (d) )
#if defined(__sgi) || defined(__sparc) || defined(__sparc__) || defined(__PPC__) || defined(__ppc__) || defined(__BIG_ENDIAN__)
#define MAKE_ID(a, b, c, d) ((int)(a) << 24 | (int)(b) << 16 | (c) << 8 | (d))
#else
# define MAKE_ID(a,b,c,d) ( (int)(d)<<24 | (int)(c)<<16 | (b)<<8 | (a) )
#endif
// ------------------------------------------------------------
#if defined(__sgi) || defined(__sparc) || defined(__sparc__) || defined (__PPC__) || defined (__ppc__) || defined (__BIG_ENDIAN__)
# define MAKE_ID2(c, d) ( (c)<<8 | (d) )
# define MOST_SIG_BYTE 0
# define BBIG_ENDIAN
#else
# define MAKE_ID2(c, d) ( (d)<<8 | (c) )
# define MOST_SIG_BYTE 1
# define BLITTLE_ENDIAN
#define MAKE_ID(a, b, c, d) ((int)(d) << 24 | (int)(c) << 16 | (b) << 8 | (a))
#endif
// ------------------------------------------------------------
#define ID_SCE MAKE_ID2('S', 'C')
#define ID_LI MAKE_ID2('L', 'I')
#define ID_OB MAKE_ID2('O', 'B')
#define ID_ME MAKE_ID2('M', 'E')
#define ID_CU MAKE_ID2('C', 'U')
#define ID_MB MAKE_ID2('M', 'B')
#define ID_MA MAKE_ID2('M', 'A')
#define ID_TE MAKE_ID2('T', 'E')
#define ID_IM MAKE_ID2('I', 'M')
#define ID_IK MAKE_ID2('I', 'K')
#define ID_WV MAKE_ID2('W', 'V')
#define ID_LT MAKE_ID2('L', 'T')
#define ID_SE MAKE_ID2('S', 'E')
#define ID_LF MAKE_ID2('L', 'F')
#define ID_LA MAKE_ID2('L', 'A')
#define ID_CA MAKE_ID2('C', 'A')
#define ID_IP MAKE_ID2('I', 'P')
#define ID_KE MAKE_ID2('K', 'E')
#define ID_WO MAKE_ID2('W', 'O')
#define ID_SCR MAKE_ID2('S', 'R')
#define ID_VF MAKE_ID2('V', 'F')
#define ID_TXT MAKE_ID2('T', 'X')
#define ID_SO MAKE_ID2('S', 'O')
#define ID_SAMPLE MAKE_ID2('S', 'A')
#define ID_GR MAKE_ID2('G', 'R')
#define ID_ID MAKE_ID2('I', 'D')
#define ID_AR MAKE_ID2('A', 'R')
#define ID_AC MAKE_ID2('A', 'C')
#define ID_SCRIPT MAKE_ID2('P', 'Y')
#define ID_FLUIDSIM MAKE_ID2('F', 'S')
#define ID_NT MAKE_ID2('N', 'T')
#define ID_BR MAKE_ID2('B', 'R')
#define ID_SEQ MAKE_ID2('S', 'Q')
#define ID_CO MAKE_ID2('C', 'O')
#define ID_PO MAKE_ID2('A', 'C')
#define ID_NLA MAKE_ID2('N', 'L')
#define ID_VS MAKE_ID2('V', 'S')
#define ID_VN MAKE_ID2('V', 'N')
#if defined(__sgi) || defined(__sparc) || defined(__sparc__) || defined(__PPC__) || defined(__ppc__) || defined(__BIG_ENDIAN__)
#define MAKE_ID2(c, d) ((c) << 8 | (d))
#define MOST_SIG_BYTE 0
#define BBIG_ENDIAN
#else
#define MAKE_ID2(c, d) ((d) << 8 | (c))
#define MOST_SIG_BYTE 1
#define BLITTLE_ENDIAN
#endif
// ------------------------------------------------------------
#define FORM MAKE_ID('F','O','R','M')
#define DDG1 MAKE_ID('3','D','G','1')
#define DDG2 MAKE_ID('3','D','G','2')
#define DDG3 MAKE_ID('3','D','G','3')
#define DDG4 MAKE_ID('3','D','G','4')
#define GOUR MAKE_ID('G','O','U','R')
#define BLEN MAKE_ID('B','L','E','N')
#define DER_ MAKE_ID('D','E','R','_')
#define V100 MAKE_ID('V','1','0','0')
#define DATA MAKE_ID('D','A','T','A')
#define GLOB MAKE_ID('G','L','O','B')
#define IMAG MAKE_ID('I','M','A','G')
#define USER MAKE_ID('U','S','E','R')
#define ID_SCE MAKE_ID2('S', 'C')
#define ID_LI MAKE_ID2('L', 'I')
#define ID_OB MAKE_ID2('O', 'B')
#define ID_ME MAKE_ID2('M', 'E')
#define ID_CU MAKE_ID2('C', 'U')
#define ID_MB MAKE_ID2('M', 'B')
#define ID_MA MAKE_ID2('M', 'A')
#define ID_TE MAKE_ID2('T', 'E')
#define ID_IM MAKE_ID2('I', 'M')
#define ID_IK MAKE_ID2('I', 'K')
#define ID_WV MAKE_ID2('W', 'V')
#define ID_LT MAKE_ID2('L', 'T')
#define ID_SE MAKE_ID2('S', 'E')
#define ID_LF MAKE_ID2('L', 'F')
#define ID_LA MAKE_ID2('L', 'A')
#define ID_CA MAKE_ID2('C', 'A')
#define ID_IP MAKE_ID2('I', 'P')
#define ID_KE MAKE_ID2('K', 'E')
#define ID_WO MAKE_ID2('W', 'O')
#define ID_SCR MAKE_ID2('S', 'R')
#define ID_VF MAKE_ID2('V', 'F')
#define ID_TXT MAKE_ID2('T', 'X')
#define ID_SO MAKE_ID2('S', 'O')
#define ID_SAMPLE MAKE_ID2('S', 'A')
#define ID_GR MAKE_ID2('G', 'R')
#define ID_ID MAKE_ID2('I', 'D')
#define ID_AR MAKE_ID2('A', 'R')
#define ID_AC MAKE_ID2('A', 'C')
#define ID_SCRIPT MAKE_ID2('P', 'Y')
#define ID_FLUIDSIM MAKE_ID2('F', 'S')
#define ID_NT MAKE_ID2('N', 'T')
#define ID_BR MAKE_ID2('B', 'R')
#define ID_SEQ MAKE_ID2('S', 'Q')
#define ID_CO MAKE_ID2('C', 'O')
#define ID_PO MAKE_ID2('A', 'C')
#define ID_NLA MAKE_ID2('N', 'L')
#define ID_VS MAKE_ID2('V', 'S')
#define ID_VN MAKE_ID2('V', 'N')
// ------------------------------------------------------------
#define DNA1 MAKE_ID('D','N','A','1')
#define REND MAKE_ID('R','E','N','D')
#define ENDB MAKE_ID('E','N','D','B')
#define NAME MAKE_ID('N','A','M','E')
#define SDNA MAKE_ID('S','D','N','A')
#define TYPE MAKE_ID('T','Y','P','E')
#define TLEN MAKE_ID('T','L','E','N')
#define STRC MAKE_ID('S','T','R','C')
#define FORM MAKE_ID('F', 'O', 'R', 'M')
#define DDG1 MAKE_ID('3', 'D', 'G', '1')
#define DDG2 MAKE_ID('3', 'D', 'G', '2')
#define DDG3 MAKE_ID('3', 'D', 'G', '3')
#define DDG4 MAKE_ID('3', 'D', 'G', '4')
#define GOUR MAKE_ID('G', 'O', 'U', 'R')
#define BLEN MAKE_ID('B', 'L', 'E', 'N')
#define DER_ MAKE_ID('D', 'E', 'R', '_')
#define V100 MAKE_ID('V', '1', '0', '0')
#define DATA MAKE_ID('D', 'A', 'T', 'A')
#define GLOB MAKE_ID('G', 'L', 'O', 'B')
#define IMAG MAKE_ID('I', 'M', 'A', 'G')
#define USER MAKE_ID('U', 'S', 'E', 'R')
// ------------------------------------------------------------
#define SWITCH_INT(a) { \
char s_i, *p_i; \
p_i= (char *)&(a); \
s_i=p_i[0]; p_i[0]=p_i[3]; p_i[3]=s_i; \
s_i=p_i[1]; p_i[1]=p_i[2]; p_i[2]=s_i; }
#define DNA1 MAKE_ID('D', 'N', 'A', '1')
#define REND MAKE_ID('R', 'E', 'N', 'D')
#define ENDB MAKE_ID('E', 'N', 'D', 'B')
#define NAME MAKE_ID('N', 'A', 'M', 'E')
#define SDNA MAKE_ID('S', 'D', 'N', 'A')
#define TYPE MAKE_ID('T', 'Y', 'P', 'E')
#define TLEN MAKE_ID('T', 'L', 'E', 'N')
#define STRC MAKE_ID('S', 'T', 'R', 'C')
// ------------------------------------------------------------
#define SWITCH_SHORT(a) { \
char s_i, *p_i; \
p_i= (char *)&(a); \
s_i=p_i[0]; p_i[0]=p_i[1]; p_i[1]=s_i; }
#define SWITCH_INT(a) \
{ \
char s_i, *p_i; \
p_i = (char *)&(a); \
s_i = p_i[0]; \
p_i[0] = p_i[3]; \
p_i[3] = s_i; \
s_i = p_i[1]; \
p_i[1] = p_i[2]; \
p_i[2] = s_i; \
}
// ------------------------------------------------------------
#define SWITCH_LONGINT(a) { \
char s_i, *p_i; \
p_i= (char *)&(a); \
s_i=p_i[0]; p_i[0]=p_i[7]; p_i[7]=s_i; \
s_i=p_i[1]; p_i[1]=p_i[6]; p_i[6]=s_i; \
s_i=p_i[2]; p_i[2]=p_i[5]; p_i[5]=s_i; \
s_i=p_i[3]; p_i[3]=p_i[4]; p_i[4]=s_i; }
#define SWITCH_SHORT(a) \
{ \
char s_i, *p_i; \
p_i = (char *)&(a); \
s_i = p_i[0]; \
p_i[0] = p_i[1]; \
p_i[1] = s_i; \
}
#endif//__B_DEFINES_H__
// ------------------------------------------------------------
#define SWITCH_LONGINT(a) \
{ \
char s_i, *p_i; \
p_i = (char *)&(a); \
s_i = p_i[0]; \
p_i[0] = p_i[7]; \
p_i[7] = s_i; \
s_i = p_i[1]; \
p_i[1] = p_i[6]; \
p_i[6] = s_i; \
s_i = p_i[2]; \
p_i[2] = p_i[5]; \
p_i[5] = s_i; \
s_i = p_i[3]; \
p_i[3] = p_i[4]; \
p_i[4] = s_i; \
}
#endif //__B_DEFINES_H__

1098
Extras/Serialize/BulletFileLoader/bFile.cpp
View File

File diff suppressed because it is too large Load Diff

263
Extras/Serialize/BulletFileLoader/bFile.h
View File

@ -20,156 +20,147 @@ subject to the following restrictions:
#include "bChunk.h"
#include <stdio.h>
namespace bParse {
namespace bParse
{
// ----------------------------------------------------- //
enum bFileFlags
{
FD_INVALID = 0,
FD_OK = 1,
FD_VOID_IS_8 = 2,
FD_ENDIAN_SWAP = 4,
FD_FILE_64 = 8,
FD_BITS_VARIES = 16,
FD_VERSION_VARIES = 32,
FD_DOUBLE_PRECISION = 64,
FD_BROKEN_DNA = 128,
FD_FILEDNA_IS_MEMDNA = 256
};
// ----------------------------------------------------- //
enum bFileFlags
enum bFileVerboseMode
{
FD_VERBOSE_EXPORT_XML = 1,
FD_VERBOSE_DUMP_DNA_TYPE_DEFINITIONS = 2,
FD_VERBOSE_DUMP_CHUNKS = 4,
FD_VERBOSE_DUMP_FILE_INFO = 8,
};
// ----------------------------------------------------- //
class bFile
{
protected:
char m_headerString[7];
bool mOwnsBuffer;
char* mFileBuffer;
int mFileLen;
int mVersion;
bPtrMap mLibPointers;
int mDataStart;
bDNA* mFileDNA;
bDNA* mMemoryDNA;
btAlignedObjectArray<char*> m_pointerFixupArray;
btAlignedObjectArray<char*> m_pointerPtrFixupArray;
btAlignedObjectArray<bChunkInd> m_chunks;
btHashMap<btHashPtr, bChunkInd> m_chunkPtrPtrMap;
//
bPtrMap mDataPointers;
int mFlags;
// ////////////////////////////////////////////////////////////////////////////
// buffer offset util
int getNextBlock(bChunkInd* dataChunk, const char* dataPtr, const int flags);
void safeSwapPtr(char* dst, const char* src);
virtual void parseHeader();
virtual void parseData() = 0;
void resolvePointersMismatch();
void resolvePointersChunk(const bChunkInd& dataChunk, int verboseMode);
int resolvePointersStructRecursive(char* strcPtr, int old_dna, int verboseMode, int recursion);
//void swapPtr(char *dst, char *src);
void parseStruct(char* strcPtr, char* dtPtr, int old_dna, int new_dna, bool fixupPointers);
void getMatchingFileDNA(short* old, const char* lookupName, const char* lookupType, char* strcData, char* data, bool fixupPointers);
char* getFileElement(short* firstStruct, char* lookupName, char* lookupType, char* data, short** foundPos);
void swap(char* head, class bChunkInd& ch, bool ignoreEndianFlag);
void swapData(char* data, short type, int arraySize, bool ignoreEndianFlag);
void swapStruct(int dna_nr, char* data, bool ignoreEndianFlag);
void swapLen(char* dataPtr);
void swapDNA(char* ptr);
char* readStruct(char* head, class bChunkInd& chunk);
char* getAsString(int code);
virtual void parseInternal(int verboseMode, char* memDna, int memDnaLength);
public:
bFile(const char* filename, const char headerString[7]);
//todo: make memoryBuffer const char
//bFile( const char *memoryBuffer, int len);
bFile(char* memoryBuffer, int len, const char headerString[7]);
virtual ~bFile();
bDNA* getFileDNA()
{
FD_INVALID =0,
FD_OK =1,
FD_VOID_IS_8 =2,
FD_ENDIAN_SWAP =4,
FD_FILE_64 =8,
FD_BITS_VARIES =16,
FD_VERSION_VARIES = 32,
FD_DOUBLE_PRECISION =64,
FD_BROKEN_DNA = 128,
FD_FILEDNA_IS_MEMDNA = 256
};
return mFileDNA;
}
enum bFileVerboseMode
virtual void addDataBlock(char* dataBlock) = 0;
int getFlags() const
{
FD_VERBOSE_EXPORT_XML = 1,
FD_VERBOSE_DUMP_DNA_TYPE_DEFINITIONS = 2,
FD_VERBOSE_DUMP_CHUNKS = 4,
FD_VERBOSE_DUMP_FILE_INFO=8,
};
// ----------------------------------------------------- //
class bFile
return mFlags;
}
void setFileDNAisMemoryDNA()
{
protected:
char m_headerString[7];
mFlags |= FD_FILEDNA_IS_MEMDNA;
}
bool mOwnsBuffer;
char* mFileBuffer;
int mFileLen;
int mVersion;
bPtrMap& getLibPointers()
{
return mLibPointers;
}
void* findLibPointer(void* ptr);
bPtrMap mLibPointers;
bool ok();
int mDataStart;
bDNA* mFileDNA;
bDNA* mMemoryDNA;
virtual void parse(int verboseMode) = 0;
btAlignedObjectArray<char*> m_pointerFixupArray;
btAlignedObjectArray<char*> m_pointerPtrFixupArray;
btAlignedObjectArray<bChunkInd> m_chunks;
btHashMap<btHashPtr, bChunkInd> m_chunkPtrPtrMap;
virtual int write(const char* fileName, bool fixupPointers = false) = 0;
//
bPtrMap mDataPointers;
virtual void writeChunks(FILE* fp, bool fixupPointers);
int mFlags;
virtual void writeDNA(FILE* fp) = 0;
// ////////////////////////////////////////////////////////////////////////////
void updateOldPointers();
void resolvePointers(int verboseMode);
// buffer offset util
int getNextBlock(bChunkInd *dataChunk, const char *dataPtr, const int flags);
void safeSwapPtr(char *dst, const char *src);
void dumpChunks(bDNA* dna);
virtual void parseHeader();
virtual void parseData() = 0;
virtual void setFileDNA(int verboseMode, char* buffer, int len);
void resolvePointersMismatch();
void resolvePointersChunk(const bChunkInd& dataChunk, int verboseMode);
int getVersion() const
{
return mVersion;
}
//pre-swap the endianness, so that data loaded on a target with different endianness doesn't need to be swapped
void preSwap();
void writeFile(const char* fileName);
};
} // namespace bParse
int resolvePointersStructRecursive(char *strcPtr, int old_dna, int verboseMode, int recursion);
//void swapPtr(char *dst, char *src);
void parseStruct(char *strcPtr, char *dtPtr, int old_dna, int new_dna, bool fixupPointers);
void getMatchingFileDNA(short* old, const char* lookupName, const char* lookupType, char *strcData, char *data, bool fixupPointers);
char* getFileElement(short *firstStruct, char *lookupName, char *lookupType, char *data, short **foundPos);
void swap(char *head, class bChunkInd& ch, bool ignoreEndianFlag);
void swapData(char *data, short type, int arraySize, bool ignoreEndianFlag);
void swapStruct(int dna_nr, char *data, bool ignoreEndianFlag);
void swapLen(char *dataPtr);
void swapDNA(char* ptr);
char* readStruct(char *head, class bChunkInd& chunk);
char *getAsString(int code);
virtual void parseInternal(int verboseMode, char* memDna,int memDnaLength);
public:
bFile(const char *filename, const char headerString[7]);
//todo: make memoryBuffer const char
//bFile( const char *memoryBuffer, int len);
bFile( char *memoryBuffer, int len, const char headerString[7]);
virtual ~bFile();
bDNA* getFileDNA()
{
return mFileDNA;
}
virtual void addDataBlock(char* dataBlock) = 0;
int getFlags() const
{
return mFlags;
}
void setFileDNAisMemoryDNA()
{
mFlags |= FD_FILEDNA_IS_MEMDNA;
}
bPtrMap& getLibPointers()
{
return mLibPointers;
}
void* findLibPointer(void *ptr);
bool ok();
virtual void parse(int verboseMode) = 0;
virtual int write(const char* fileName, bool fixupPointers=false) = 0;
virtual void writeChunks(FILE* fp, bool fixupPointers );
virtual void writeDNA(FILE* fp) = 0;
void updateOldPointers();
void resolvePointers(int verboseMode);
void dumpChunks(bDNA* dna);
virtual void setFileDNA(int verboseMode, char* buffer, int len);
int getVersion() const
{
return mVersion;
}
//pre-swap the endianness, so that data loaded on a target with different endianness doesn't need to be swapped
void preSwap();
void writeFile(const char* fileName);
};
}
#endif//__BFILE_H__
#endif //__BFILE_H__

264
Extras/Serialize/BulletFileLoader/btBulletFile.cpp
View File

@ -17,12 +17,11 @@ subject to the following restrictions:
#include "bDefines.h"
#include "bDNA.h"
#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__)
#if !defined(__CELLOS_LV2__) && !defined(__MWERKS__)
#include <memory.h>
#endif
#include <string.h>
// 32 && 64 bit versions
#ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#ifdef _WIN64
@ -31,137 +30,123 @@ extern int sBulletDNAlen64;
#else
extern char sBulletDNAstr[];
extern int sBulletDNAlen;
#endif //_WIN64
#else//BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#endif //_WIN64
#else //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
extern char sBulletDNAstr64[];
extern int sBulletDNAlen64;
extern char sBulletDNAstr[];
extern int sBulletDNAlen;
#endif //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#endif //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
using namespace bParse;
btBulletFile::btBulletFile()
:bFile("", "BULLET ")
: bFile("", "BULLET ")
{
mMemoryDNA = new bDNA(); //this memory gets released in the bFile::~bFile destructor,@todo not consistent with the rule 'who allocates it, has to deallocate it"
mMemoryDNA = new bDNA(); //this memory gets released in the bFile::~bFile destructor,@todo not consistent with the rule 'who allocates it, has to deallocate it"
m_DnaCopy = 0;
#ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#ifdef _WIN64
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen64,16);
memcpy(m_DnaCopy,sBulletDNAstr64,sBulletDNAlen64);
mMemoryDNA->init(m_DnaCopy,sBulletDNAlen64);
#else//_WIN64
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen,16);
memcpy(m_DnaCopy,sBulletDNAstr,sBulletDNAlen);
mMemoryDNA->init(m_DnaCopy,sBulletDNAlen);
#endif//_WIN64
#else//BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen64, 16);
memcpy(m_DnaCopy, sBulletDNAstr64, sBulletDNAlen64);
mMemoryDNA->init(m_DnaCopy, sBulletDNAlen64);
#else //_WIN64
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen, 16);
memcpy(m_DnaCopy, sBulletDNAstr, sBulletDNAlen);
mMemoryDNA->init(m_DnaCopy, sBulletDNAlen);
#endif //_WIN64
#else //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
if (VOID_IS_8)
{
m_DnaCopy = (char*) btAlignedAlloc(sBulletDNAlen64,16);
memcpy(m_DnaCopy,sBulletDNAstr64,sBulletDNAlen64);
mMemoryDNA->init(m_DnaCopy,sBulletDNAlen64);
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen64, 16);
memcpy(m_DnaCopy, sBulletDNAstr64, sBulletDNAlen64);
mMemoryDNA->init(m_DnaCopy, sBulletDNAlen64);
}
else
{
m_DnaCopy =(char*) btAlignedAlloc(sBulletDNAlen,16);
memcpy(m_DnaCopy,sBulletDNAstr,sBulletDNAlen);
mMemoryDNA->init(m_DnaCopy,sBulletDNAlen);
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen, 16);
memcpy(m_DnaCopy, sBulletDNAstr, sBulletDNAlen);
mMemoryDNA->init(m_DnaCopy, sBulletDNAlen);
}
#endif//BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#endif //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
}
btBulletFile::btBulletFile(const char* fileName)
:bFile(fileName, "BULLET ")
: bFile(fileName, "BULLET ")
{
m_DnaCopy = 0;
}
btBulletFile::btBulletFile(char *memoryBuffer, int len)
:bFile(memoryBuffer,len, "BULLET ")
btBulletFile::btBulletFile(char* memoryBuffer, int len)
: bFile(memoryBuffer, len, "BULLET ")
{
m_DnaCopy = 0;
}
btBulletFile::~btBulletFile()
{
if (m_DnaCopy)
btAlignedFree(m_DnaCopy);
while (m_dataBlocks.size())
{
char* dataBlock = m_dataBlocks[m_dataBlocks.size()-1];
char* dataBlock = m_dataBlocks[m_dataBlocks.size() - 1];
delete[] dataBlock;
m_dataBlocks.pop_back();
}
}
// ----------------------------------------------------- //
void btBulletFile::parseData()
{
// printf ("Building datablocks");
// printf ("Chunk size = %d",CHUNK_HEADER_LEN);
// printf ("File chunk size = %d",ChunkUtils::getOffset(mFlags));
// printf ("Building datablocks");
// printf ("Chunk size = %d",CHUNK_HEADER_LEN);
// printf ("File chunk size = %d",ChunkUtils::getOffset(mFlags));
const bool brokenDNA = (mFlags&FD_BROKEN_DNA)!=0;
const bool brokenDNA = (mFlags & FD_BROKEN_DNA) != 0;
//const bool swap = (mFlags&FD_ENDIAN_SWAP)!=0;
int remain = mFileLen;
mDataStart = 12;
remain-=12;
remain -= 12;
//invalid/empty file?
if (remain < sizeof(bChunkInd))
return;
char *dataPtr = mFileBuffer+mDataStart;
char* dataPtr = mFileBuffer + mDataStart;
bChunkInd dataChunk;
dataChunk.code = 0;
//dataPtr += ChunkUtils::getNextBlock(&dataChunk, dataPtr, mFlags);
int seek = getNextBlock(&dataChunk, dataPtr, mFlags);
if (mFlags &FD_ENDIAN_SWAP)
if (mFlags & FD_ENDIAN_SWAP)
swapLen(dataPtr);
//dataPtr += ChunkUtils::getOffset(mFlags);
char *dataPtrHead = 0;
char* dataPtrHead = 0;
while (dataChunk.code != DNA1)
{
if (!brokenDNA || (dataChunk.code != BT_QUANTIZED_BVH_CODE) )
if (!brokenDNA || (dataChunk.code != BT_QUANTIZED_BVH_CODE))
{
// one behind
if (dataChunk.code == SDNA) break;
//if (dataChunk.code == DNA1) break;
// same as (BHEAD+DATA dependency)
dataPtrHead = dataPtr+ChunkUtils::getOffset(mFlags);
if (dataChunk.dna_nr>=0)
dataPtrHead = dataPtr + ChunkUtils::getOffset(mFlags);
if (dataChunk.dna_nr >= 0)
{
char *id = readStruct(dataPtrHead, dataChunk);
char* id = readStruct(dataPtrHead, dataChunk);
// lookup maps
if (id)
@ -192,99 +177,94 @@ void btBulletFile::parseData()
if (dataChunk.code == BT_SOFTBODY_CODE)
{
m_softBodies.push_back((bStructHandle*) id);
m_softBodies.push_back((bStructHandle*)id);
}
if (dataChunk.code == BT_RIGIDBODY_CODE)
{
m_rigidBodies.push_back((bStructHandle*) id);
m_rigidBodies.push_back((bStructHandle*)id);
}
if (dataChunk.code == BT_DYNAMICSWORLD_CODE)
{
m_dynamicsWorldInfo.push_back((bStructHandle*) id);
m_dynamicsWorldInfo.push_back((bStructHandle*)id);
}
if (dataChunk.code == BT_CONSTRAINT_CODE)
{
m_constraints.push_back((bStructHandle*) id);
m_constraints.push_back((bStructHandle*)id);
}
if (dataChunk.code == BT_QUANTIZED_BVH_CODE)
{
m_bvhs.push_back((bStructHandle*) id);
m_bvhs.push_back((bStructHandle*)id);
}
if (dataChunk.code == BT_TRIANLGE_INFO_MAP)
{
m_triangleInfoMaps.push_back((bStructHandle*) id);
m_triangleInfoMaps.push_back((bStructHandle*)id);
}
if (dataChunk.code == BT_COLLISIONOBJECT_CODE)
{
m_collisionObjects.push_back((bStructHandle*) id);
m_collisionObjects.push_back((bStructHandle*)id);
}
if (dataChunk.code == BT_SHAPE_CODE)
{
m_collisionShapes.push_back((bStructHandle*) id);
m_collisionShapes.push_back((bStructHandle*)id);
}
// if (dataChunk.code == GLOB)
// {
// m_glob = (bStructHandle*) id;
// }
} else
// if (dataChunk.code == GLOB)
// {
// m_glob = (bStructHandle*) id;
// }
}
else
{
//printf("unknown chunk\n");
mLibPointers.insert(dataChunk.oldPtr, (bStructHandle*)dataPtrHead);
}
} else
}
else
{
printf("skipping BT_QUANTIZED_BVH_CODE due to broken DNA\n");
}
dataPtr += seek;
remain-=seek;
if (remain<=0)
remain -= seek;
if (remain <= 0)
break;
seek = getNextBlock(&dataChunk, dataPtr, mFlags);
if (mFlags &FD_ENDIAN_SWAP)
seek = getNextBlock(&dataChunk, dataPtr, mFlags);
if (mFlags & FD_ENDIAN_SWAP)
swapLen(dataPtr);
if (seek < 0)
break;
}
}
void btBulletFile::addDataBlock(char* dataBlock)
void btBulletFile::addDataBlock(char* dataBlock)
{
m_dataBlocks.push_back(dataBlock);
}
void btBulletFile::writeDNA(FILE* fp)
void btBulletFile::writeDNA(FILE* fp)
{
bChunkInd dataChunk;
dataChunk.code = DNA1;
dataChunk.dna_nr = 0;
dataChunk.nr = 1;
#ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
if (VOID_IS_8)
{
#ifdef _WIN64
dataChunk.len = sBulletDNAlen64;
dataChunk.oldPtr = sBulletDNAstr64;
fwrite(&dataChunk,sizeof(bChunkInd),1,fp);
fwrite(sBulletDNAstr64, sBulletDNAlen64,1,fp);
fwrite(&dataChunk, sizeof(bChunkInd), 1, fp);
fwrite(sBulletDNAstr64, sBulletDNAlen64, 1, fp);
#else
btAssert(0);
#endif
@ -294,43 +274,42 @@ void btBulletFile::writeDNA(FILE* fp)
#ifndef _WIN64
dataChunk.len = sBulletDNAlen;
dataChunk.oldPtr = sBulletDNAstr;
fwrite(&dataChunk,sizeof(bChunkInd),1,fp);
fwrite(sBulletDNAstr, sBulletDNAlen,1,fp);
#else//_WIN64
fwrite(&dataChunk, sizeof(bChunkInd), 1, fp);
fwrite(sBulletDNAstr, sBulletDNAlen, 1, fp);
#else //_WIN64
btAssert(0);
#endif//_WIN64
#endif //_WIN64
}
#else//BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#else //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
if (VOID_IS_8)
{
dataChunk.len = sBulletDNAlen64;
dataChunk.oldPtr = sBulletDNAstr64;
fwrite(&dataChunk,sizeof(bChunkInd),1,fp);
fwrite(sBulletDNAstr64, sBulletDNAlen64,1,fp);
fwrite(&dataChunk, sizeof(bChunkInd), 1, fp);
fwrite(sBulletDNAstr64, sBulletDNAlen64, 1, fp);
}
else
{
dataChunk.len = sBulletDNAlen;
dataChunk.oldPtr = sBulletDNAstr;
fwrite(&dataChunk,sizeof(bChunkInd),1,fp);
fwrite(sBulletDNAstr, sBulletDNAlen,1,fp);
fwrite(&dataChunk, sizeof(bChunkInd), 1, fp);
fwrite(sBulletDNAstr, sBulletDNAlen, 1, fp);
}
#endif//BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#endif //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
}
void btBulletFile::parse(int verboseMode)
void btBulletFile::parse(int verboseMode)
{
#ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
if (VOID_IS_8)
{
#ifdef _WIN64
if (m_DnaCopy)
delete m_DnaCopy;
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen64,16);
memcpy(m_DnaCopy,sBulletDNAstr64,sBulletDNAlen64);
parseInternal(verboseMode,(char*)sBulletDNAstr64,sBulletDNAlen64);
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen64, 16);
memcpy(m_DnaCopy, sBulletDNAstr64, sBulletDNAlen64);
parseInternal(verboseMode, (char*)sBulletDNAstr64, sBulletDNAlen64);
#else
btAssert(0);
#endif
@ -341,93 +320,91 @@ void btBulletFile::parse(int verboseMode)
if (m_DnaCopy)
delete m_DnaCopy;
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen,16);
memcpy(m_DnaCopy,sBulletDNAstr,sBulletDNAlen);
parseInternal(verboseMode,m_DnaCopy,sBulletDNAlen);
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen, 16);
memcpy(m_DnaCopy, sBulletDNAstr, sBulletDNAlen);
parseInternal(verboseMode, m_DnaCopy, sBulletDNAlen);
#else
btAssert(0);
#endif
}
#else//BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#else //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
if (VOID_IS_8)
{
if (m_DnaCopy)
delete m_DnaCopy;
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen64,16);
memcpy(m_DnaCopy,sBulletDNAstr64,sBulletDNAlen64);
parseInternal(verboseMode,m_DnaCopy,sBulletDNAlen64);
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen64, 16);
memcpy(m_DnaCopy, sBulletDNAstr64, sBulletDNAlen64);
parseInternal(verboseMode, m_DnaCopy, sBulletDNAlen64);
}
else
{
if (m_DnaCopy)
delete m_DnaCopy;
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen,16);
memcpy(m_DnaCopy,sBulletDNAstr,sBulletDNAlen);
parseInternal(verboseMode,m_DnaCopy,sBulletDNAlen);
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen, 16);
memcpy(m_DnaCopy, sBulletDNAstr, sBulletDNAlen);
parseInternal(verboseMode, m_DnaCopy, sBulletDNAlen);
}
#endif//BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
//the parsing will convert to cpu endian
mFlags &=~FD_ENDIAN_SWAP;
#endif //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
int littleEndian= 1;
littleEndian= ((char*)&littleEndian)[0];
mFileBuffer[8] = littleEndian?'v':'V';
//the parsing will convert to cpu endian
mFlags &= ~FD_ENDIAN_SWAP;
int littleEndian = 1;
littleEndian = ((char*)&littleEndian)[0];
mFileBuffer[8] = littleEndian ? 'v' : 'V';
}
// experimental
int btBulletFile::write(const char* fileName, bool fixupPointers)
int btBulletFile::write(const char* fileName, bool fixupPointers)
{
FILE *fp = fopen(fileName, "wb");
FILE* fp = fopen(fileName, "wb");
if (fp)
{
char header[SIZEOFBLENDERHEADER] ;
char header[SIZEOFBLENDERHEADER];
memcpy(header, m_headerString, 7);
int endian= 1;
endian= ((char*)&endian)[0];
int endian = 1;
endian = ((char*)&endian)[0];
if (endian)
{
header[7] = '_';
} else
}
else
{
header[7] = '-';
}
if (VOID_IS_8)
{
header[8]='V';
} else
header[8] = 'V';
}
else
{
header[8]='v';
header[8] = 'v';
}
header[9] = '2';
header[10] = '7';
header[11] = '5';
fwrite(header,SIZEOFBLENDERHEADER,1,fp);
fwrite(header, SIZEOFBLENDERHEADER, 1, fp);
writeChunks(fp, fixupPointers);
writeDNA(fp);
fclose(fp);
} else
}
else
{
printf("Error: cannot open file %s for writing\n",fileName);
printf("Error: cannot open file %s for writing\n", fileName);
return 0;
}
return 1;
}
void btBulletFile::addStruct(const char* structType,void* data, int len, void* oldPtr, int code)
void btBulletFile::addStruct(const char* structType, void* data, int len, void* oldPtr, int code)
{
bParse::bChunkInd dataChunk;
dataChunk.code = code;
dataChunk.nr = 1;
@ -436,13 +413,12 @@ void btBulletFile::addStruct(const char* structType,void* data, int len, void* o
dataChunk.oldPtr = oldPtr;
///Perform structure size validation
short* structInfo= mMemoryDNA->getStruct(dataChunk.dna_nr);
short* structInfo = mMemoryDNA->getStruct(dataChunk.dna_nr);
int elemBytes;
elemBytes= mMemoryDNA->getLength(structInfo[0]);
// int elemBytes = mMemoryDNA->getElementSize(structInfo[0],structInfo[1]);
assert(len==elemBytes);
elemBytes = mMemoryDNA->getLength(structInfo[0]);
// int elemBytes = mMemoryDNA->getElementSize(structInfo[0],structInfo[1]);
assert(len == elemBytes);
mLibPointers.insert(dataChunk.oldPtr, (bStructHandle*)data);
m_chunks.push_back(dataChunk);
}

76
Extras/Serialize/BulletFileLoader/btBulletFile.h
View File

@ -16,75 +16,65 @@ subject to the following restrictions:
#ifndef BT_BULLET_FILE_H
#define BT_BULLET_FILE_H
#include "bFile.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "bDefines.h"
#include "LinearMath/btSerializer.h"
namespace bParse
{
// ----------------------------------------------------- //
class btBulletFile : public bFile
{
protected:
char* m_DnaCopy;
public:
btAlignedObjectArray<bStructHandle*> m_multiBodies;
namespace bParse {
btAlignedObjectArray<bStructHandle*> m_multiBodyLinkColliders;
// ----------------------------------------------------- //
class btBulletFile : public bFile
{
btAlignedObjectArray<bStructHandle*> m_softBodies;
protected:
char* m_DnaCopy;
public:
btAlignedObjectArray<bStructHandle*> m_rigidBodies;
btAlignedObjectArray<bStructHandle*> m_multiBodies;
btAlignedObjectArray<bStructHandle*> m_collisionObjects;
btAlignedObjectArray<bStructHandle*> m_multiBodyLinkColliders;
btAlignedObjectArray<bStructHandle*> m_collisionShapes;
btAlignedObjectArray<bStructHandle*> m_softBodies;
btAlignedObjectArray<bStructHandle*> m_constraints;
btAlignedObjectArray<bStructHandle*> m_rigidBodies;
btAlignedObjectArray<bStructHandle*> m_bvhs;
btAlignedObjectArray<bStructHandle*> m_collisionObjects;
btAlignedObjectArray<bStructHandle*> m_triangleInfoMaps;
btAlignedObjectArray<bStructHandle*> m_collisionShapes;
btAlignedObjectArray<bStructHandle*> m_dynamicsWorldInfo;
btAlignedObjectArray<bStructHandle*> m_constraints;
btAlignedObjectArray<bStructHandle*> m_contactManifolds;
btAlignedObjectArray<bStructHandle*> m_bvhs;
btAlignedObjectArray<char*> m_dataBlocks;
btBulletFile();
btAlignedObjectArray<bStructHandle*> m_triangleInfoMaps;
btBulletFile(const char* fileName);
btAlignedObjectArray<bStructHandle*> m_dynamicsWorldInfo;
btBulletFile(char* memoryBuffer, int len);
btAlignedObjectArray<bStructHandle*> m_contactManifolds;
virtual ~btBulletFile();
btAlignedObjectArray<char*> m_dataBlocks;
btBulletFile();
virtual void addDataBlock(char* dataBlock);
btBulletFile(const char* fileName);
// experimental
virtual int write(const char* fileName, bool fixupPointers = false);
btBulletFile(char *memoryBuffer, int len);
virtual void parse(int verboseMode);
virtual ~btBulletFile();
virtual void parseData();
virtual void addDataBlock(char* dataBlock);
// experimental
virtual int write(const char* fileName, bool fixupPointers=false);
virtual void writeDNA(FILE* fp);
virtual void parse(int verboseMode);
virtual void parseData();
virtual void writeDNA(FILE* fp);
void addStruct(const char* structType,void* data, int len, void* oldPtr, int code);
};
void addStruct(const char* structType, void* data, int len, void* oldPtr, int code);
};
}; // namespace bParse
#endif //BT_BULLET_FILE_H
#endif //BT_BULLET_FILE_H

158
Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp
View File

@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btBulletWorldImporter.h"
#include "../BulletFileLoader/btBulletFile.h"
@ -22,14 +21,13 @@ subject to the following restrictions:
#include "BulletCollision/Gimpact/btGImpactShape.h"
#endif
//#define USE_INTERNAL_EDGE_UTILITY
#ifdef USE_INTERNAL_EDGE_UTILITY
#include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h"
#endif //USE_INTERNAL_EDGE_UTILITY
#endif //USE_INTERNAL_EDGE_UTILITY
btBulletWorldImporter::btBulletWorldImporter(btDynamicsWorld* world)
:btWorldImporter(world)
: btWorldImporter(world)
{
}
@ -37,12 +35,10 @@ btBulletWorldImporter::~btBulletWorldImporter()
{
}
bool btBulletWorldImporter::loadFile( const char* fileName, const char* preSwapFilenameOut)
bool btBulletWorldImporter::loadFile(const char* fileName, const char* preSwapFilenameOut)
{
bParse::btBulletFile* bulletFile2 = new bParse::btBulletFile(fileName);
bool result = loadFileFromMemory(bulletFile2);
//now you could save the file in 'native' format using
//bulletFile2->writeFile("native.bullet");
@ -53,19 +49,15 @@ bool btBulletWorldImporter::loadFile( const char* fileName, const char* preSwapF
bulletFile2->preSwap();
bulletFile2->writeFile(preSwapFilenameOut);
}
}
delete bulletFile2;
return result;
return result;
}
bool btBulletWorldImporter::loadFileFromMemory( char* memoryBuffer, int len)
bool btBulletWorldImporter::loadFileFromMemory(char* memoryBuffer, int len)
{
bParse::btBulletFile* bulletFile2 = new bParse::btBulletFile(memoryBuffer,len);
bParse::btBulletFile* bulletFile2 = new bParse::btBulletFile(memoryBuffer, len);
bool result = loadFileFromMemory(bulletFile2);
@ -74,36 +66,31 @@ bool btBulletWorldImporter::loadFileFromMemory( char* memoryBuffer, int len)
return result;
}
bool btBulletWorldImporter::loadFileFromMemory( bParse::btBulletFile* bulletFile2)
bool btBulletWorldImporter::loadFileFromMemory(bParse::btBulletFile* bulletFile2)
{
bool ok = (bulletFile2->getFlags()& bParse::FD_OK)!=0;
bool ok = (bulletFile2->getFlags() & bParse::FD_OK) != 0;
if (ok)
bulletFile2->parse(m_verboseMode);
else
else
return false;
if (m_verboseMode & bParse::FD_VERBOSE_DUMP_CHUNKS)
{
bulletFile2->dumpChunks(bulletFile2->getFileDNA());
}
return convertAllObjects(bulletFile2);
}
bool btBulletWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile2)
bool btBulletWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFile2)
{
m_shapeMap.clear();
m_bodyMap.clear();
int i;
for (i=0;i<bulletFile2->m_bvhs.size();i++)
for (i = 0; i < bulletFile2->m_bvhs.size(); i++)
{
btOptimizedBvh* bvh = createOptimizedBvh();
@ -111,41 +98,34 @@ bool btBulletWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile
{
btQuantizedBvhDoubleData* bvhData = (btQuantizedBvhDoubleData*)bulletFile2->m_bvhs[i];
bvh->deSerializeDouble(*bvhData);
} else
}
else
{
btQuantizedBvhFloatData* bvhData = (btQuantizedBvhFloatData*)bulletFile2->m_bvhs[i];
bvh->deSerializeFloat(*bvhData);
}
m_bvhMap.insert(bulletFile2->m_bvhs[i],bvh);
m_bvhMap.insert(bulletFile2->m_bvhs[i], bvh);
}
for (i=0;i<bulletFile2->m_collisionShapes.size();i++)
for (i = 0; i < bulletFile2->m_collisionShapes.size(); i++)
{
btCollisionShapeData* shapeData = (btCollisionShapeData*)bulletFile2->m_collisionShapes[i];
btCollisionShape* shape = convertCollisionShape(shapeData);
if (shape)
{
// printf("shapeMap.insert(%x,%x)\n",shapeData,shape);
m_shapeMap.insert(shapeData,shape);
// printf("shapeMap.insert(%x,%x)\n",shapeData,shape);
m_shapeMap.insert(shapeData, shape);
}
if (shape&& shapeData->m_name)
if (shape && shapeData->m_name)
{
char* newname = duplicateName(shapeData->m_name);
m_objectNameMap.insert(shape,newname);
m_nameShapeMap.insert(newname,shape);
m_objectNameMap.insert(shape, newname);
m_nameShapeMap.insert(newname, shape);
}
}
for (int i=0;i<bulletFile2->m_dynamicsWorldInfo.size();i++)
for (int i = 0; i < bulletFile2->m_dynamicsWorldInfo.size(); i++)
{
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
@ -169,21 +149,22 @@ bool btBulletWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile
solverInfo.m_globalCfm = btScalar(solverInfoData->m_solverInfo.m_globalCfm);
solverInfo.m_splitImpulsePenetrationThreshold = btScalar(solverInfoData->m_solverInfo.m_splitImpulsePenetrationThreshold);
solverInfo.m_splitImpulseTurnErp = btScalar(solverInfoData->m_solverInfo.m_splitImpulseTurnErp);
solverInfo.m_linearSlop = btScalar(solverInfoData->m_solverInfo.m_linearSlop);
solverInfo.m_warmstartingFactor = btScalar(solverInfoData->m_solverInfo.m_warmstartingFactor);
solverInfo.m_maxGyroscopicForce = btScalar(solverInfoData->m_solverInfo.m_maxGyroscopicForce);
solverInfo.m_singleAxisRollingFrictionThreshold = btScalar(solverInfoData->m_solverInfo.m_singleAxisRollingFrictionThreshold);
solverInfo.m_numIterations = solverInfoData->m_solverInfo.m_numIterations;
solverInfo.m_solverMode = solverInfoData->m_solverInfo.m_solverMode;
solverInfo.m_restingContactRestitutionThreshold = solverInfoData->m_solverInfo.m_restingContactRestitutionThreshold;
solverInfo.m_minimumSolverBatchSize = solverInfoData->m_solverInfo.m_minimumSolverBatchSize;
solverInfo.m_splitImpulse = solverInfoData->m_solverInfo.m_splitImpulse;
setDynamicsWorldInfo(gravity,solverInfo);
} else
setDynamicsWorldInfo(gravity, solverInfo);
}
else
{
btDynamicsWorldFloatData* solverInfoData = (btDynamicsWorldFloatData*)bulletFile2->m_dynamicsWorldInfo[i];
btContactSolverInfo solverInfo;
@ -205,40 +186,38 @@ bool btBulletWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile
solverInfo.m_globalCfm = solverInfoData->m_solverInfo.m_globalCfm;
solverInfo.m_splitImpulsePenetrationThreshold = solverInfoData->m_solverInfo.m_splitImpulsePenetrationThreshold;
solverInfo.m_splitImpulseTurnErp = solverInfoData->m_solverInfo.m_splitImpulseTurnErp;
solverInfo.m_linearSlop = solverInfoData->m_solverInfo.m_linearSlop;
solverInfo.m_warmstartingFactor = solverInfoData->m_solverInfo.m_warmstartingFactor;
solverInfo.m_maxGyroscopicForce = solverInfoData->m_solverInfo.m_maxGyroscopicForce;
solverInfo.m_singleAxisRollingFrictionThreshold = solverInfoData->m_solverInfo.m_singleAxisRollingFrictionThreshold;
solverInfo.m_numIterations = solverInfoData->m_solverInfo.m_numIterations;
solverInfo.m_solverMode = solverInfoData->m_solverInfo.m_solverMode;
solverInfo.m_restingContactRestitutionThreshold = solverInfoData->m_solverInfo.m_restingContactRestitutionThreshold;
solverInfo.m_minimumSolverBatchSize = solverInfoData->m_solverInfo.m_minimumSolverBatchSize;
solverInfo.m_splitImpulse = solverInfoData->m_solverInfo.m_splitImpulse;
setDynamicsWorldInfo(gravity,solverInfo);
setDynamicsWorldInfo(gravity, solverInfo);
}
}
for (i=0;i<bulletFile2->m_rigidBodies.size();i++)
for (i = 0; i < bulletFile2->m_rigidBodies.size(); i++)
{
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
btRigidBodyDoubleData* colObjData = (btRigidBodyDoubleData*)bulletFile2->m_rigidBodies[i];
convertRigidBodyDouble(colObjData);
} else
}
else
{
btRigidBodyFloatData* colObjData = (btRigidBodyFloatData*)bulletFile2->m_rigidBodies[i];
convertRigidBodyFloat(colObjData);
}
}
for (i=0;i<bulletFile2->m_collisionObjects.size();i++)
for (i = 0; i < bulletFile2->m_collisionObjects.size(); i++)
{
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
@ -249,29 +228,30 @@ bool btBulletWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile
btTransform startTransform;
colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f;
startTransform.deSerializeDouble(colObjData->m_worldTransform);
btCollisionShape* shape = (btCollisionShape*)*shapePtr;
btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name);
btCollisionObject* body = createCollisionObject(startTransform, shape, colObjData->m_name);
body->setFriction(btScalar(colObjData->m_friction));
body->setRestitution(btScalar(colObjData->m_restitution));
#ifdef USE_INTERNAL_EDGE_UTILITY
if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape;
if (trimesh->getTriangleInfoMap())
{
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
}
}
#endif //USE_INTERNAL_EDGE_UTILITY
m_bodyMap.insert(colObjData,body);
} else
#endif //USE_INTERNAL_EDGE_UTILITY
m_bodyMap.insert(colObjData, body);
}
else
{
printf("error: no shape found\n");
}
} else
}
else
{
btCollisionObjectFloatData* colObjData = (btCollisionObjectFloatData*)bulletFile2->m_collisionObjects[i];
btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape);
@ -280,9 +260,9 @@ bool btBulletWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile
btTransform startTransform;
colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f;
startTransform.deSerializeFloat(colObjData->m_worldTransform);
btCollisionShape* shape = (btCollisionShape*)*shapePtr;
btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name);
btCollisionObject* body = createCollisionObject(startTransform, shape, colObjData->m_name);
#ifdef USE_INTERNAL_EDGE_UTILITY
if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
@ -290,21 +270,20 @@ bool btBulletWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile
btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape;
if (trimesh->getTriangleInfoMap())
{
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
}
}
#endif //USE_INTERNAL_EDGE_UTILITY
m_bodyMap.insert(colObjData,body);
} else
#endif //USE_INTERNAL_EDGE_UTILITY
m_bodyMap.insert(colObjData, body);
}
else
{
printf("error: no shape found\n");
}
}
}
for (i=0;i<bulletFile2->m_constraints.size();i++)
for (i = 0; i < bulletFile2->m_constraints.size(); i++)
{
btTypedConstraintData2* constraintData = (btTypedConstraintData2*)bulletFile2->m_constraints[i];
@ -328,34 +307,31 @@ bool btBulletWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile
}
if (!rbA && !rbB)
continue;
bool isDoublePrecisionData = (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)!=0;
bool isDoublePrecisionData = (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) != 0;
if (isDoublePrecisionData)
{
if (bulletFile2->getVersion()>=282)
if (bulletFile2->getVersion() >= 282)
{
btTypedConstraintDoubleData* dc = (btTypedConstraintDoubleData*)constraintData;
convertConstraintDouble(dc, rbA,rbB, bulletFile2->getVersion());
} else
convertConstraintDouble(dc, rbA, rbB, bulletFile2->getVersion());
}
else
{
//double-precision constraints were messed up until 2.82, try to recover data...
btTypedConstraintData* oldData = (btTypedConstraintData*)constraintData;
convertConstraintBackwardsCompatible281(oldData, rbA,rbB, bulletFile2->getVersion());
btTypedConstraintData* oldData = (btTypedConstraintData*)constraintData;
convertConstraintBackwardsCompatible281(oldData, rbA, rbB, bulletFile2->getVersion());
}
}
else
{
btTypedConstraintFloatData* dc = (btTypedConstraintFloatData*)constraintData;
convertConstraintFloat(dc, rbA,rbB, bulletFile2->getVersion());
convertConstraintFloat(dc, rbA, rbB, bulletFile2->getVersion());
}
}
return true;
}

34
Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h
View File

@ -13,56 +13,40 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BULLET_WORLD_IMPORTER_H
#define BULLET_WORLD_IMPORTER_H
#include "btWorldImporter.h"
class btBulletFile;
namespace bParse
{
class btBulletFile;
class btBulletFile;
};
///The btBulletWorldImporter is a starting point to import .bullet files.
///note that not all data is converted yet. You are expected to override or modify this class.
///See Bullet/Demos/SerializeDemo for a derived class that extract btSoftBody objects too.
class btBulletWorldImporter : public btWorldImporter
{
public:
btBulletWorldImporter(btDynamicsWorld* world=0);
btBulletWorldImporter(btDynamicsWorld* world = 0);
virtual ~btBulletWorldImporter();
///if you pass a valid preSwapFilenameOut, it will save a new file with a different endianness
///if you pass a valid preSwapFilenameOut, it will save a new file with a different endianness
///this pre-swapped file can be loaded without swapping on a target platform of different endianness
bool loadFile(const char* fileName, const char* preSwapFilenameOut=0);
bool loadFile(const char* fileName, const char* preSwapFilenameOut = 0);
///the memoryBuffer might be modified (for example if endian swaps are necessary)
bool loadFileFromMemory(char *memoryBuffer, int len);
bool loadFileFromMemory(char* memoryBuffer, int len);
bool loadFileFromMemory(bParse::btBulletFile* file);
bool loadFileFromMemory(bParse::btBulletFile* file);
//call make sure bulletFile2 has been parsed, either using btBulletFile::parse or btBulletWorldImporter::loadFileFromMemory
virtual bool convertAllObjects(bParse::btBulletFile* file);
virtual bool convertAllObjects(bParse::btBulletFile* file);
};
#endif //BULLET_WORLD_IMPORTER_H
#endif //BULLET_WORLD_IMPORTER_H

200
Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp
View File

@ -15,12 +15,11 @@ struct btMultiBodyWorldImporterInternalData
};
btMultiBodyWorldImporter::btMultiBodyWorldImporter(btMultiBodyDynamicsWorld* world)
:btBulletWorldImporter(world)
: btBulletWorldImporter(world)
{
m_data = new btMultiBodyWorldImporterInternalData;
m_data->m_mbDynamicsWorld = world;
}
btMultiBodyWorldImporter::~btMultiBodyWorldImporter()
{
@ -32,7 +31,6 @@ void btMultiBodyWorldImporter::deleteAllData()
btBulletWorldImporter::deleteAllData();
}
static btCollisionObjectDoubleData* getBody0FromContactManifold(btPersistentManifoldDoubleData* manifold)
{
return (btCollisionObjectDoubleData*)manifold->m_body0;
@ -50,8 +48,8 @@ static btCollisionObjectFloatData* getBody1FromContactManifold(btPersistentManif
return (btCollisionObjectFloatData*)manifold->m_body1;
}
template<class T> void syncContactManifolds(T** contactManifolds, int numContactManifolds, btMultiBodyWorldImporterInternalData* m_data)
template <class T>
void syncContactManifolds(T** contactManifolds, int numContactManifolds, btMultiBodyWorldImporterInternalData* m_data)
{
m_data->m_mbDynamicsWorld->updateAabbs();
m_data->m_mbDynamicsWorld->computeOverlappingPairs();
@ -59,7 +57,6 @@ template<class T> void syncContactManifolds(T** contactManifolds, int numContact
btDispatcherInfo& dispatchInfo = m_data->m_mbDynamicsWorld->getDispatchInfo();
if (dispatcher)
{
btOverlappingPairCache* pairCache = m_data->m_mbDynamicsWorld->getBroadphase()->getOverlappingPairCache();
@ -104,10 +101,10 @@ template<class T> void syncContactManifolds(T** contactManifolds, int numContact
}
}
}
}
template<class T> void syncMultiBody(T* mbd, btMultiBody* mb, btMultiBodyWorldImporterInternalData* m_data, btAlignedObjectArray<btQuaternion>& scratchQ, btAlignedObjectArray<btVector3>& scratchM)
template <class T>
void syncMultiBody(T* mbd, btMultiBody* mb, btMultiBodyWorldImporterInternalData* m_data, btAlignedObjectArray<btQuaternion>& scratchQ, btAlignedObjectArray<btVector3>& scratchM)
{
bool isFixedBase = mbd->m_baseMass == 0;
bool canSleep = false;
@ -129,7 +126,6 @@ template<class T> void syncMultiBody(T* mbd, btMultiBody* mb, btMultiBodyWorldI
for (int i = 0; i < mbd->m_numLinks; i++)
{
mb->getLink(i).m_absFrameTotVelocity.m_topVec.deSerialize(mbd->m_links[i].m_absFrameTotVelocityTop);
mb->getLink(i).m_absFrameTotVelocity.m_bottomVec.deSerialize(mbd->m_links[i].m_absFrameTotVelocityBottom);
mb->getLink(i).m_absFrameLocVelocity.m_topVec.deSerialize(mbd->m_links[i].m_absFrameLocVelocityTop);
@ -137,45 +133,46 @@ template<class T> void syncMultiBody(T* mbd, btMultiBody* mb, btMultiBodyWorldI
switch (mbd->m_links[i].m_jointType)
{
case btMultibodyLink::eFixed:
{
break;
}
case btMultibodyLink::ePrismatic:
{
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
break;
}
case btMultibodyLink::eRevolute:
{
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
break;
}
case btMultibodyLink::eSpherical:
{
btScalar jointPos[4] = { (btScalar)mbd->m_links[i].m_jointPos[0], (btScalar)mbd->m_links[i].m_jointPos[1], (btScalar)mbd->m_links[i].m_jointPos[2], (btScalar)mbd->m_links[i].m_jointPos[3] };
btScalar jointVel[3] = { (btScalar)mbd->m_links[i].m_jointVel[0], (btScalar)mbd->m_links[i].m_jointVel[1], (btScalar)mbd->m_links[i].m_jointVel[2] };
mb->setJointPosMultiDof(i, jointPos);
mb->setJointVelMultiDof(i, jointVel);
case btMultibodyLink::eFixed:
{
break;
}
case btMultibodyLink::ePrismatic:
{
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
break;
}
case btMultibodyLink::eRevolute:
{
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
break;
}
case btMultibodyLink::eSpherical:
{
btScalar jointPos[4] = {(btScalar)mbd->m_links[i].m_jointPos[0], (btScalar)mbd->m_links[i].m_jointPos[1], (btScalar)mbd->m_links[i].m_jointPos[2], (btScalar)mbd->m_links[i].m_jointPos[3]};
btScalar jointVel[3] = {(btScalar)mbd->m_links[i].m_jointVel[0], (btScalar)mbd->m_links[i].m_jointVel[1], (btScalar)mbd->m_links[i].m_jointVel[2]};
mb->setJointPosMultiDof(i, jointPos);
mb->setJointVelMultiDof(i, jointVel);
break;
}
case btMultibodyLink::ePlanar:
{
break;
}
default:
{
}
break;
}
case btMultibodyLink::ePlanar:
{
break;
}
default:
{
}
}
}
mb->forwardKinematics(scratchQ, scratchM);
mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
}
template<class T> void convertMultiBody(T* mbd, btMultiBodyWorldImporterInternalData* m_data)
template <class T>
void convertMultiBody(T* mbd, btMultiBodyWorldImporterInternalData* m_data)
{
bool isFixedBase = mbd->m_baseMass == 0;
bool canSleep = false;
@ -206,71 +203,69 @@ template<class T> void convertMultiBody(T* mbd, btMultiBodyWorldImporterInterna
switch (mbd->m_links[i].m_jointType)
{
case btMultibodyLink::eFixed:
{
case btMultibodyLink::eFixed:
{
mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset);
//search for the collider
//mbd->m_links[i].m_linkCollider
break;
}
case btMultibodyLink::ePrismatic:
{
btVector3 jointAxis;
jointAxis.deSerialize(mbd->m_links[i].m_jointAxisBottom[0]);
bool disableParentCollision = true; //todo
mb->setupPrismatic(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
break;
}
case btMultibodyLink::eRevolute:
{
btVector3 jointAxis;
jointAxis.deSerialize(mbd->m_links[i].m_jointAxisTop[0]);
bool disableParentCollision = true; //todo
mb->setupRevolute(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
break;
}
case btMultibodyLink::eSpherical:
{
btAssert(0);
bool disableParentCollision = true; //todo
mb->setupSpherical(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
btScalar jointPos[4] = {(btScalar)mbd->m_links[i].m_jointPos[0], (btScalar)mbd->m_links[i].m_jointPos[1], (btScalar)mbd->m_links[i].m_jointPos[2], (btScalar)mbd->m_links[i].m_jointPos[3]};
btScalar jointVel[3] = {(btScalar)mbd->m_links[i].m_jointVel[0], (btScalar)mbd->m_links[i].m_jointVel[1], (btScalar)mbd->m_links[i].m_jointVel[2]};
mb->setJointPosMultiDof(i, jointPos);
mb->setJointVelMultiDof(i, jointVel);
mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset);
//search for the collider
//mbd->m_links[i].m_linkCollider
break;
}
case btMultibodyLink::ePrismatic:
{
btVector3 jointAxis;
jointAxis.deSerialize(mbd->m_links[i].m_jointAxisBottom[0]);
bool disableParentCollision = true;//todo
mb->setupPrismatic(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
break;
}
case btMultibodyLink::eRevolute:
{
btVector3 jointAxis;
jointAxis.deSerialize(mbd->m_links[i].m_jointAxisTop[0]);
bool disableParentCollision = true;//todo
mb->setupRevolute(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
break;
}
case btMultibodyLink::eSpherical:
{
btAssert(0);
bool disableParentCollision = true;//todo
mb->setupSpherical(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
btScalar jointPos[4] = { (btScalar)mbd->m_links[i].m_jointPos[0], (btScalar)mbd->m_links[i].m_jointPos[1], (btScalar)mbd->m_links[i].m_jointPos[2], (btScalar)mbd->m_links[i].m_jointPos[3] };
btScalar jointVel[3] = { (btScalar)mbd->m_links[i].m_jointVel[0], (btScalar)mbd->m_links[i].m_jointVel[1], (btScalar)mbd->m_links[i].m_jointVel[2] };
mb->setJointPosMultiDof(i, jointPos);
mb->setJointVelMultiDof(i, jointVel);
break;
}
case btMultibodyLink::ePlanar:
{
btAssert(0);
break;
}
default:
{
btAssert(0);
}
break;
}
case btMultibodyLink::ePlanar:
{
btAssert(0);
break;
}
default:
{
btAssert(0);
}
}
}
}
bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile2)
bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFile2)
{
bool result = false;
btAlignedObjectArray<btQuaternion> scratchQ;
btAlignedObjectArray<btVector3> scratchM;
if (m_importerFlags&eRESTORE_EXISTING_OBJECTS)
if (m_importerFlags & eRESTORE_EXISTING_OBJECTS)
{
//check if the snapshot is valid for the existing world
//equal number of objects, # links etc
@ -284,7 +279,6 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
//convert all multibodies
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
//for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
for (int i = bulletFile2->m_multiBodies.size() - 1; i >= 0; i--)
{
@ -352,7 +346,6 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
}
}
if (bulletFile2->m_contactManifolds.size())
{
syncContactManifolds((btPersistentManifoldDoubleData**)&bulletFile2->m_contactManifolds[0], bulletFile2->m_contactManifolds.size(), m_data);
@ -389,23 +382,19 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
}
}
if (bulletFile2->m_contactManifolds.size())
{
syncContactManifolds((btPersistentManifoldFloatData**)&bulletFile2->m_contactManifolds[0], bulletFile2->m_contactManifolds.size(), m_data);
}
}
}
else
{
result = btBulletWorldImporter::convertAllObjects(bulletFile2);
//convert all multibodies
for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
{
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
@ -421,7 +410,7 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
//forward kinematics, so that the link world transforms are valid, for collision detection
for (int i = 0; i < m_data->m_mbMap.size(); i++)
{
btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
btMultiBody** ptr = m_data->m_mbMap.getAtIndex(i);
if (ptr)
{
btMultiBody* mb = *ptr;
@ -444,7 +433,6 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
{
btMultiBody* multiBody = *ptr;
btCollisionShape** shapePtr = m_shapeMap.find(mblcd->m_colObjData.m_collisionShape);
if (shapePtr && *shapePtr)
{
@ -491,7 +479,6 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
#endif
m_data->m_mbDynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
}
}
else
{
@ -503,13 +490,12 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
#endif
}
}
}
for (int i = 0; i < m_data->m_mbMap.size(); i++)
{
btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
btMultiBody** ptr = m_data->m_mbMap.getAtIndex(i);
if (ptr)
{
btMultiBody* mb = *ptr;

5
Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.h
View File

@ -8,13 +8,12 @@ class btMultiBodyWorldImporter : public btBulletWorldImporter
struct btMultiBodyWorldImporterInternalData* m_data;
public:
btMultiBodyWorldImporter(class btMultiBodyDynamicsWorld* world);
virtual ~btMultiBodyWorldImporter();
virtual bool convertAllObjects( bParse::btBulletFile* bulletFile2);
virtual bool convertAllObjects(bParse::btBulletFile* bulletFile2);
virtual void deleteAllData();
};
#endif //BT_MULTIBODY_WORLD_IMPORTER_H
#endif //BT_MULTIBODY_WORLD_IMPORTER_H

2457
Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp
View File

File diff suppressed because it is too large Load Diff

143
Extras/Serialize/BulletWorldImporter/btWorldImporter.h
View File

@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_WORLD_IMPORTER_H
#define BT_WORLD_IMPORTER_H
@ -57,76 +56,73 @@ struct btRigidBodyFloatData;
#define btRigidBodyData btRigidBodyDoubleData
#else
#define btRigidBodyData btRigidBodyFloatData
#endif//BT_USE_DOUBLE_PRECISION
#endif //BT_USE_DOUBLE_PRECISION
enum btWorldImporterFlags
{
eRESTORE_EXISTING_OBJECTS=1,//don't create new objects
eRESTORE_EXISTING_OBJECTS = 1, //don't create new objects
};
class btWorldImporter
{
protected:
btDynamicsWorld* m_dynamicsWorld;
int m_verboseMode;
int m_importerFlags;
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
btAlignedObjectArray<btCollisionObject*> m_allocatedRigidBodies;
btAlignedObjectArray<btTypedConstraint*> m_allocatedConstraints;
btAlignedObjectArray<btOptimizedBvh*> m_allocatedBvhs;
btAlignedObjectArray<btOptimizedBvh*> m_allocatedBvhs;
btAlignedObjectArray<btTriangleInfoMap*> m_allocatedTriangleInfoMaps;
btAlignedObjectArray<btTriangleIndexVertexArray*> m_allocatedTriangleIndexArrays;
btAlignedObjectArray<btStridingMeshInterfaceData*> m_allocatedbtStridingMeshInterfaceDatas;
btAlignedObjectArray<char*> m_allocatedNames;
btAlignedObjectArray<char*> m_allocatedNames;
btAlignedObjectArray<int*> m_indexArrays;
btAlignedObjectArray<short int*> m_shortIndexArrays;
btAlignedObjectArray<unsigned char*> m_charIndexArrays;
btAlignedObjectArray<int*> m_indexArrays;
btAlignedObjectArray<short int*> m_shortIndexArrays;
btAlignedObjectArray<unsigned char*> m_charIndexArrays;
btAlignedObjectArray<btVector3FloatData*> m_floatVertexArrays;
btAlignedObjectArray<btVector3DoubleData*> m_doubleVertexArrays;
btAlignedObjectArray<btVector3FloatData*> m_floatVertexArrays;
btAlignedObjectArray<btVector3DoubleData*> m_doubleVertexArrays;
btHashMap<btHashPtr, btOptimizedBvh*> m_bvhMap;
btHashMap<btHashPtr, btTriangleInfoMap*> m_timMap;
btHashMap<btHashPtr,btOptimizedBvh*> m_bvhMap;
btHashMap<btHashPtr,btTriangleInfoMap*> m_timMap;
btHashMap<btHashString,btCollisionShape*> m_nameShapeMap;
btHashMap<btHashString,btRigidBody*> m_nameBodyMap;
btHashMap<btHashString,btTypedConstraint*> m_nameConstraintMap;
btHashMap<btHashPtr,const char*> m_objectNameMap;
btHashMap<btHashPtr,btCollisionShape*> m_shapeMap;
btHashMap<btHashPtr,btCollisionObject*> m_bodyMap;
btHashMap<btHashString, btCollisionShape*> m_nameShapeMap;
btHashMap<btHashString, btRigidBody*> m_nameBodyMap;
btHashMap<btHashString, btTypedConstraint*> m_nameConstraintMap;
btHashMap<btHashPtr, const char*> m_objectNameMap;
btHashMap<btHashPtr, btCollisionShape*> m_shapeMap;
btHashMap<btHashPtr, btCollisionObject*> m_bodyMap;
//methods
static btRigidBody& getFixedBody();
char* duplicateName(const char* name);
char* duplicateName(const char* name);
btCollisionShape* convertCollisionShape( btCollisionShapeData* shapeData );
void convertConstraintBackwardsCompatible281(btTypedConstraintData* constraintData, btRigidBody* rbA, btRigidBody* rbB, int fileVersion);
void convertConstraintFloat(btTypedConstraintFloatData* constraintData, btRigidBody* rbA, btRigidBody* rbB, int fileVersion);
void convertConstraintDouble(btTypedConstraintDoubleData* constraintData, btRigidBody* rbA, btRigidBody* rbB, int fileVersion);
void convertRigidBodyFloat(btRigidBodyFloatData* colObjData);
void convertRigidBodyDouble( btRigidBodyDoubleData* colObjData);
btCollisionShape* convertCollisionShape(btCollisionShapeData* shapeData);
void convertConstraintBackwardsCompatible281(btTypedConstraintData* constraintData, btRigidBody* rbA, btRigidBody* rbB, int fileVersion);
void convertConstraintFloat(btTypedConstraintFloatData* constraintData, btRigidBody* rbA, btRigidBody* rbB, int fileVersion);
void convertConstraintDouble(btTypedConstraintDoubleData* constraintData, btRigidBody* rbA, btRigidBody* rbB, int fileVersion);
void convertRigidBodyFloat(btRigidBodyFloatData* colObjData);
void convertRigidBodyDouble(btRigidBodyDoubleData* colObjData);
public:
btWorldImporter(btDynamicsWorld* world);
virtual ~btWorldImporter();
///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load.
///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load.
///make sure you don't use the dynamics world containing objects after you call this method
virtual void deleteAllData();
void setVerboseMode(int verboseMode)
void setVerboseMode(int verboseMode)
{
m_verboseMode = verboseMode;
}
@ -136,7 +132,7 @@ public:
return m_verboseMode;
}
void setImporterFlags(int importerFlags)
void setImporterFlags(int importerFlags)
{
m_importerFlags = importerFlags;
}
@ -146,87 +142,80 @@ public:
return m_importerFlags;
}
// query for data
int getNumCollisionShapes() const;
// query for data
int getNumCollisionShapes() const;
btCollisionShape* getCollisionShapeByIndex(int index);
int getNumRigidBodies() const;
btCollisionObject* getRigidBodyByIndex(int index) const;
int getNumConstraints() const;
btTypedConstraint* getConstraintByIndex(int index) const;
int getNumBvhs() const;
btOptimizedBvh* getBvhByIndex(int index) const;
btOptimizedBvh* getBvhByIndex(int index) const;
int getNumTriangleInfoMaps() const;
btTriangleInfoMap* getTriangleInfoMapByIndex(int index) const;
// queris involving named objects
btCollisionShape* getCollisionShapeByName(const char* name);
btRigidBody* getRigidBodyByName(const char* name);
btTypedConstraint* getConstraintByName(const char* name);
const char* getNameForPointer(const void* ptr) const;
const char* getNameForPointer(const void* ptr) const;
///those virtuals are called by load and can be overridden by the user
virtual void setDynamicsWorldInfo(const btVector3& gravity, const btContactSolverInfo& solverInfo);
virtual void setDynamicsWorldInfo(const btVector3& gravity, const btContactSolverInfo& solverInfo);
//bodies
virtual btRigidBody* createRigidBody(bool isDynamic, btScalar mass, const btTransform& startTransform, btCollisionShape* shape,const char* bodyName);
virtual btCollisionObject* createCollisionObject( const btTransform& startTransform, btCollisionShape* shape,const char* bodyName);
virtual btRigidBody* createRigidBody(bool isDynamic, btScalar mass, const btTransform& startTransform, btCollisionShape* shape, const char* bodyName);
virtual btCollisionObject* createCollisionObject(const btTransform& startTransform, btCollisionShape* shape, const char* bodyName);
///shapes
virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal,btScalar planeConstant);
virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal, btScalar planeConstant);
virtual btCollisionShape* createBoxShape(const btVector3& halfExtents);
virtual btCollisionShape* createSphereShape(btScalar radius);
virtual btCollisionShape* createCapsuleShapeX(btScalar radius, btScalar height);
virtual btCollisionShape* createCapsuleShapeY(btScalar radius, btScalar height);
virtual btCollisionShape* createCapsuleShapeZ(btScalar radius, btScalar height);
virtual btCollisionShape* createCylinderShapeX(btScalar radius,btScalar height);
virtual btCollisionShape* createCylinderShapeY(btScalar radius,btScalar height);
virtual btCollisionShape* createCylinderShapeZ(btScalar radius,btScalar height);
virtual btCollisionShape* createConeShapeX(btScalar radius,btScalar height);
virtual btCollisionShape* createConeShapeY(btScalar radius,btScalar height);
virtual btCollisionShape* createConeShapeZ(btScalar radius,btScalar height);
virtual class btTriangleIndexVertexArray* createTriangleMeshContainer();
virtual btBvhTriangleMeshShape* createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh);
virtual btCollisionShape* createCylinderShapeX(btScalar radius, btScalar height);
virtual btCollisionShape* createCylinderShapeY(btScalar radius, btScalar height);
virtual btCollisionShape* createCylinderShapeZ(btScalar radius, btScalar height);
virtual btCollisionShape* createConeShapeX(btScalar radius, btScalar height);
virtual btCollisionShape* createConeShapeY(btScalar radius, btScalar height);
virtual btCollisionShape* createConeShapeZ(btScalar radius, btScalar height);
virtual class btTriangleIndexVertexArray* createTriangleMeshContainer();
virtual btBvhTriangleMeshShape* createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh);
virtual btCollisionShape* createConvexTriangleMeshShape(btStridingMeshInterface* trimesh);
virtual btGImpactMeshShape* createGimpactShape(btStridingMeshInterface* trimesh);
virtual btStridingMeshInterfaceData* createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData);
virtual class btConvexHullShape* createConvexHullShape();
virtual class btCompoundShape* createCompoundShape();
virtual class btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScalingbtBvhTriangleMeshShape);
virtual class btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape, const btVector3& localScalingbtBvhTriangleMeshShape);
virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres);
virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres);
virtual btTriangleIndexVertexArray* createMeshInterface(btStridingMeshInterfaceData& meshData);
///acceleration and connectivity structures
virtual btOptimizedBvh* createOptimizedBvh();
virtual btOptimizedBvh* createOptimizedBvh();
virtual btTriangleInfoMap* createTriangleInfoMap();
///constraints
virtual btPoint2PointConstraint* createPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
virtual btPoint2PointConstraint* createPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
virtual btHingeConstraint* createHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA=false);
virtual btHingeConstraint* createHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA=false);
virtual btConeTwistConstraint* createConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
virtual btConeTwistConstraint* createConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
virtual btGeneric6DofConstraint* createGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
virtual btGeneric6DofConstraint* createGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
virtual btGeneric6DofSpringConstraint* createGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
virtual btGeneric6DofSpring2Constraint* createGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, int rotateOrder );
virtual btSliderConstraint* createSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
virtual btSliderConstraint* createSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
virtual btGearConstraint* createGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio);
virtual btPoint2PointConstraint* createPoint2PointConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB);
virtual btPoint2PointConstraint* createPoint2PointConstraint(btRigidBody& rbA, const btVector3& pivotInA);
virtual btHingeConstraint* createHingeConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
virtual btHingeConstraint* createHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA = false);
virtual btConeTwistConstraint* createConeTwistConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
virtual btConeTwistConstraint* createConeTwistConstraint(btRigidBody& rbA, const btTransform& rbAFrame);
virtual btGeneric6DofConstraint* createGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
virtual btGeneric6DofConstraint* createGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
virtual btGeneric6DofSpringConstraint* createGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
virtual btGeneric6DofSpring2Constraint* createGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, int rotateOrder);
virtual btSliderConstraint* createSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
virtual btSliderConstraint* createSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
virtual btGearConstraint* createGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA, const btVector3& axisInB, btScalar ratio);
};
#endif //BT_WORLD_IMPORTER_H
#endif //BT_WORLD_IMPORTER_H

671
Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.cpp
View File

File diff suppressed because it is too large Load Diff

43
Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.h
View File

@ -22,7 +22,7 @@ class btDynamicsWorld;
namespace tinyxml2
{
class XMLNode;
class XMLNode;
};
struct btConvexInternalShapeData;
@ -37,58 +37,53 @@ struct btRigidBodyFloatData;
struct btTypedConstraintFloatData;
#define btTypedConstraintData2 btTypedConstraintFloatData
#define btRigidBodyData btRigidBodyFloatData
#endif//BT_USE_DOUBLE_PRECISION
#endif //BT_USE_DOUBLE_PRECISION
struct btCompoundShapeChildData;
#include "LinearMath/btAlignedObjectArray.h"
#include "btWorldImporter.h"
class btBulletXmlWorldImporter : public btWorldImporter
{
protected:
btAlignedObjectArray<btCollisionShapeData*> m_collisionShapeData;
btAlignedObjectArray<btAlignedObjectArray<btCompoundShapeChildData>* > m_compoundShapeChildDataArrays;
btAlignedObjectArray<btRigidBodyData*> m_rigidBodyData;
btAlignedObjectArray<btTypedConstraintData2*> m_constraintData;
btHashMap<btHashPtr,void*> m_pointerLookup;
int m_fileVersion;
bool m_fileOk;
btAlignedObjectArray<btCollisionShapeData*> m_collisionShapeData;
btAlignedObjectArray<btAlignedObjectArray<btCompoundShapeChildData>*> m_compoundShapeChildDataArrays;
btAlignedObjectArray<btRigidBodyData*> m_rigidBodyData;
btAlignedObjectArray<btTypedConstraintData2*> m_constraintData;
btHashMap<btHashPtr, void*> m_pointerLookup;
int m_fileVersion;
bool m_fileOk;
void auto_serialize_root_level_children(tinyxml2::XMLNode* pParent);
void auto_serialize(tinyxml2::XMLNode* pParent);
void deSerializeVector3FloatData(tinyxml2::XMLNode* pParent,btAlignedObjectArray<btVector3FloatData>& vectors);
void deSerializeVector3FloatData(tinyxml2::XMLNode* pParent, btAlignedObjectArray<btVector3FloatData>& vectors);
void fixupCollisionDataPointers(btCollisionShapeData* shapeData);
void fixupConstraintData(btTypedConstraintData2* tcd);
void fixupCollisionDataPointers(btCollisionShapeData* shapeData);
void fixupConstraintData(btTypedConstraintData2* tcd);
//collision shapes data
void deSerializeCollisionShapeData(tinyxml2::XMLNode* pParent,btCollisionShapeData* colShapeData);
void deSerializeCollisionShapeData(tinyxml2::XMLNode* pParent, btCollisionShapeData* colShapeData);
void deSerializeConvexInternalShapeData(tinyxml2::XMLNode* pParent);
void deSerializeStaticPlaneShapeData(tinyxml2::XMLNode* pParent);
void deSerializeCompoundShapeData(tinyxml2::XMLNode* pParent);
void deSerializeCompoundShapeChildData(tinyxml2::XMLNode* pParent);
void deSerializeConvexHullShapeData(tinyxml2::XMLNode* pParent);
void deSerializeDynamicsWorldData(tinyxml2::XMLNode* parent);
///bodies
void deSerializeRigidBodyFloatData(tinyxml2::XMLNode* pParent);
///constraints
void deSerializeGeneric6DofConstraintData(tinyxml2::XMLNode* pParent);
public:
btBulletXmlWorldImporter(btDynamicsWorld* world);
public:
btBulletXmlWorldImporter(btDynamicsWorld* world);
virtual ~btBulletXmlWorldImporter();
bool loadFile(const char* fileName);
virtual ~btBulletXmlWorldImporter();
bool loadFile(const char* fileName);
};
#endif //BT_BULLET_XML_WORLD_IMPORTER_H
#endif //BT_BULLET_XML_WORLD_IMPORTER_H

375
Extras/Serialize/BulletXmlWorldImporter/string_split.cpp
View File

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

9
Extras/Serialize/BulletXmlWorldImporter/string_split.h
View File

@ -16,7 +16,6 @@ subject to the following restrictions:
///The string split C code is by Lars Wirzenius
///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char
#ifndef STRING_SPLIT_H
#define STRING_SPLIT_H
@ -27,17 +26,16 @@ subject to the following restrictions:
namespace bullet_utils
{
void split( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, const std::string& separator);
void split(btAlignedObjectArray<std::string>& pieces, const std::string& vector_str, const std::string& separator);
};
///The string split C code is by Lars Wirzenius
///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char
/* Split a string into substrings. Return dynamic array of dynamically
allocated substrings, or NULL if there was an error. Caller is
expected to free the memory, for example with str_array_free. */
char** str_split(const char* input, const char* sep);
char** str_split(const char* input, const char* sep);
/* Free a dynamic array of dynamic strings. */
void str_array_free(char** array);
@ -45,5 +43,4 @@ void str_array_free(char** array);
/* Return length of a NULL-delimited array of strings. */
size_t str_array_len(char** array);
#endif //STRING_SPLIT_H
#endif //STRING_SPLIT_H

186
Extras/Serialize/HeaderGenerator/apiGen.cpp
View File

@ -36,13 +36,11 @@ typedef std::map<bString, bString> bStringMap;
typedef std::vector<class bVariable> bVariableList;
typedef std::vector<bString> bStringList;
///////////////////////////////////////////////////////////////////////////////
static FILE *dump = 0;
static bDNA *mDNA =0;
static bDNA *mDNA = 0;
static bStringMap mStructs;
///////////////////////////////////////////////////////////////////////////////
class bVariable
{
@ -50,11 +48,9 @@ public:
bVariable();
~bVariable();
bString dataType;
bString variableName;
bString functionName;
bString classCtor;
@ -62,7 +58,6 @@ public:
bString memberDataType;
bString functionArgs;
void initialize(bString dataType, bString variable, bStringMap refDataTable);
bool isPtr;
@ -103,26 +98,25 @@ bool dataTypeStandard(bString dataType)
void writeTemplate(short *structData)
{
bString type = mDNA->getType(structData[0]);
bString className=type;
bString prefix = isBulletFile? "bullet_" : "blender_";
int thisLen = structData[1];
structData+=2;
bString className = type;
bString prefix = isBulletFile ? "bullet_" : "blender_";
bString fileName = prefix+type;
int thisLen = structData[1];
structData += 2;
bString fileName = prefix + type;
bVariableList dataTypes;
bStringMap includeFiles;
for (int dataVal =0; dataVal<thisLen; dataVal++, structData+=2)
for (int dataVal = 0; dataVal < thisLen; dataVal++, structData += 2)
{
bString dataType = mDNA->getType(structData[0]);
bString dataName = mDNA->getName(structData[1]);
{
bString newDataType = "";
bString newDataName = "";
bStringMap::iterator addB = mStructs.find(dataType);
if (addB != mStructs.end())
{
@ -130,7 +124,7 @@ void writeTemplate(short *structData)
newDataName = dataName;
}
else
else
{
if (dataTypeStandard(dataType))
{
@ -148,8 +142,7 @@ void writeTemplate(short *structData)
if (dataName[0] != '*')
{
}
}
}
}
if (!newDataType.empty() && !newDataName.empty())
@ -160,28 +153,26 @@ void writeTemplate(short *structData)
}
}
bStringMap::iterator include = mStructs.find(dataType);
if (include != mStructs.end())
{
if (dataName[0] != '*')
if (dataName[0] != '*')
{
if (includeFiles.find(dataType)== includeFiles.end())
if (includeFiles.find(dataType) == includeFiles.end())
{
includeFiles[dataType]=prefix+dataType;
includeFiles[dataType] = prefix + dataType;
}
}
}
}
fprintf(dump, "###############################################################\n");
fprintf(dump, "%s = bStructClass()\n", fileName.c_str());
fprintf(dump, "%s.name = '%s'\n", fileName.c_str(), className.c_str());
fprintf(dump, "%s.filename = '%s'\n", fileName.c_str(), fileName.c_str());
bVariableList::iterator vars = dataTypes.begin();
while (vars!= dataTypes.end())
while (vars != dataTypes.end())
{
fprintf(dump, "%s.dataTypes.append('%s %s')\n", fileName.c_str(), vars->dataType.c_str(), vars->variableName.c_str());
vars++;
@ -196,32 +187,27 @@ void writeTemplate(short *structData)
fprintf(dump, "DataTypeList.append(%s)\n", fileName.c_str());
}
///////////////////////////////////////////////////////////////////////////////
char data[] = {
"\n"
"class bStructClass:\n"
" def __init__(self):\n"
" self.name = \"\";\n"
" self.filename = \"\";\n"
" self.includes = []\n"
" self.dataTypes = []\n"
"\n\n"
"DataTypeList = []\n"};
///////////////////////////////////////////////////////////////////////////////
char data[]={
"\n"
"class bStructClass:\n"
" def __init__(self):\n"
" self.name = \"\";\n"
" self.filename = \"\";\n"
" self.includes = []\n"
" self.dataTypes = []\n"
"\n\n"
"DataTypeList = []\n"
};
///////////////////////////////////////////////////////////////////////////////
int main(int argc,char** argv)
int main(int argc, char **argv)
{
using namespace bParse;
dump = fopen("dump.py", "w");
if (!dump) return 0;
fprintf(dump, "%s\n", data);
#if 0
char* filename = "../../../../data/r2d2_multibody.bullet";
@ -275,73 +261,65 @@ int main(int argc,char** argv)
#else
isBulletFile = true;
bool swap = false;
char* memBuf = sBulletDNAstr;
char *memBuf = sBulletDNAstr;
int len = sBulletDNAlen;
#endif
char *blenderData = memBuf;
int sdnaPos=0;
int sdnaPos = 0;
int mDataStart = 12;
char *tempBuffer = blenderData;
for (int i=0; i<len; i++)
for (int i = 0; i < len; i++)
{
// looking for the data's starting position
// and the start of SDNA decls
if (!mDataStart && strncmp(tempBuffer, "REND", 4)==0)
if (!mDataStart && strncmp(tempBuffer, "REND", 4) == 0)
mDataStart = i;
if (!sdnaPos && strncmp(tempBuffer, "SDNA", 4)==0)
if (!sdnaPos && strncmp(tempBuffer, "SDNA", 4) == 0)
sdnaPos = i;
if (mDataStart && sdnaPos) break;
tempBuffer++;
}
FILE* fpdna = fopen("dnaString.txt","w");
FILE *fpdna = fopen("dnaString.txt", "w");
char buf[1024];
for (int i=0;i<len-sdnaPos;i++)
for (int i = 0; i < len - sdnaPos; i++)
{
int dnaval = (memBuf+sdnaPos)[i];
int dnaval = (memBuf + sdnaPos)[i];
if ((i%32)==0)
if ((i % 32) == 0)
{
sprintf(buf,"%d,\n",dnaval);
} else
{
sprintf(buf,"%d,",dnaval);
sprintf(buf, "%d,\n", dnaval);
}
fwrite(buf,strlen(buf),1,fpdna);
else
{
sprintf(buf, "%d,", dnaval);
}
fwrite(buf, strlen(buf), 1, fpdna);
}
fclose(fpdna);
mDNA = new bDNA();
//mDNA->initMemory();
mDNA->init(memBuf+sdnaPos, len-sdnaPos, swap);
for (int i=0; i<mDNA->getNumStructs(); i++)
mDNA->init(memBuf + sdnaPos, len - sdnaPos, swap);
for (int i = 0; i < mDNA->getNumStructs(); i++)
{
short *structData = mDNA->getStruct(i);
bString type = mDNA->getType(structData[0]);
bString className = type;
mStructs[type]=className;
mStructs[type] = className;
}
for (int i=0; i<mDNA->getNumStructs(); i++)
for (int i = 0; i < mDNA->getNumStructs(); i++)
{
short *structData = mDNA->getStruct(i);
writeTemplate(structData);
@ -353,23 +331,22 @@ int main(int argc,char** argv)
}
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
int _getArraySize(char* str)
int _getArraySize(char *str)
{
int a, mul=1;
char stri[100], *cp=0;
int a, mul = 1;
char stri[100], *cp = 0;
int len = (int)strlen(str);
memcpy(stri, str, len+1);
for (a=0; a<len; a++)
memcpy(stri, str, len + 1);
for (a = 0; a < len; a++)
{
if (str[a]== '[')
cp= &(stri[a+1]);
else if ( str[a]==']' && cp)
if (str[a] == '[')
cp = &(stri[a + 1]);
else if (str[a] == ']' && cp)
{
stri[a]= 0;
mul*= atoi(cp);
stri[a] = 0;
mul *= atoi(cp);
}
}
return mul;
@ -377,27 +354,26 @@ int _getArraySize(char* str)
///////////////////////////////////////////////////////////////////////////////
bVariable::bVariable()
: dataType("invalid"),
variableName("invalid"),
functionName(""),
classCtor(""),
memberVariable(""),
memberDataType(""),
functionArgs(""),
isPtr(false),
isFunctionPtr(false),
isPtrToPtr(false),
isArray(false),
isCharArray(false),
isListBase(false),
isPadding(false),
isCommentedOut(false),
isGeneratedType(false),
isbString(false)
: dataType("invalid"),
variableName("invalid"),
functionName(""),
classCtor(""),
memberVariable(""),
memberDataType(""),
functionArgs(""),
isPtr(false),
isFunctionPtr(false),
isPtrToPtr(false),
isArray(false),
isCharArray(false),
isListBase(false),
isPadding(false),
isCommentedOut(false),
isGeneratedType(false),
isbString(false)
{
}
///////////////////////////////////////////////////////////////////////////////
bVariable::~bVariable()
{
@ -405,7 +381,6 @@ bVariable::~bVariable()
variableName.clear();
}
///////////////////////////////////////////////////////////////////////////////
void bVariable::initialize(bString type, bString variable, bStringMap refDataTable)
{
@ -422,7 +397,7 @@ void bVariable::initialize(bString type, bString variable, bStringMap refDataTab
if (variableName[1] == '*')
isFunctionPtr = true;
if (variableName[variableName.size()-1] == ']')
if (variableName[variableName.size() - 1] == ']')
{
isArray = true;
if (type == "char")
@ -434,14 +409,13 @@ void bVariable::initialize(bString type, bString variable, bStringMap refDataTab
if (variableName[0] == 'p')
{
bString sub = variableName.substr(0,3);
bString sub = variableName.substr(0, 3);
if (sub == "pad")
isPadding = true;
}
if (dataType[0] == '/' && dataType[1] == '/')
isCommentedOut = true;
if (refDataTable.find(dataType) != refDataTable.end())
isGeneratedType = true;
@ -450,13 +424,13 @@ void bVariable::initialize(bString type, bString variable, bStringMap refDataTab
// replace valid float arrays
if (dataType == "float" && isArray)
{
int size = _getArraySize((char*)variableName.c_str());
if (size==3)
int size = _getArraySize((char *)variableName.c_str());
if (size == 3)
{
dataType = "vec3f";
variableName = variableName.substr(0, variableName.find_first_of("["));
}
if (size==4)
if (size == 4)
{
dataType = "vec4f";
variableName = variableName.substr(0, variableName.find_first_of("["));

100
Extras/Serialize/ReadBulletSample/BulletDataExtractor.cpp
View File

@ -4,7 +4,7 @@
#include <stdio.h>
///work-in-progress
///work-in-progress
///This ReadBulletSample is kept as simple as possible without dependencies to the Bullet SDK.
///It can be used to load .bullet data for other physics SDKs
///For a more complete example how to load and convert Bullet data using the Bullet SDK check out
@ -22,8 +22,8 @@ enum LocalBroadphaseNativeTypes
CONVEX_HULL_SHAPE_PROXYTYPE,
CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE,
CUSTOM_POLYHEDRAL_SHAPE_TYPE,
//implicit convex shapes
IMPLICIT_CONVEX_SHAPES_START_HERE,
//implicit convex shapes
IMPLICIT_CONVEX_SHAPES_START_HERE,
SPHERE_SHAPE_PROXYTYPE,
MULTI_SPHERE_SHAPE_PROXYTYPE,
CAPSULE_SHAPE_PROXYTYPE,
@ -36,8 +36,8 @@ IMPLICIT_CONVEX_SHAPES_START_HERE,
BOX_2D_SHAPE_PROXYTYPE,
CONVEX_2D_SHAPE_PROXYTYPE,
CUSTOM_CONVEX_SHAPE_TYPE,
//concave shapes
CONCAVE_SHAPES_START_HERE,
//concave shapes
CONCAVE_SHAPES_START_HERE,
//keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
TRIANGLE_MESH_SHAPE_PROXYTYPE,
SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE,
@ -45,15 +45,15 @@ CONCAVE_SHAPES_START_HERE,
FAST_CONCAVE_MESH_PROXYTYPE,
//terrain
TERRAIN_SHAPE_PROXYTYPE,
///Used for GIMPACT Trimesh integration
///Used for GIMPACT Trimesh integration
GIMPACT_SHAPE_PROXYTYPE,
///Multimaterial mesh
MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE,
///Multimaterial mesh
MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE,
EMPTY_SHAPE_PROXYTYPE,
STATIC_PLANE_PROXYTYPE,
CUSTOM_CONCAVE_SHAPE_TYPE,
CONCAVE_SHAPES_END_HERE,
CONCAVE_SHAPES_END_HERE,
COMPOUND_SHAPE_PROXYTYPE,
@ -63,7 +63,7 @@ CONCAVE_SHAPES_END_HERE,
INVALID_SHAPE_PROXYTYPE,
MAX_BROADPHASE_COLLISION_TYPES
};
btBulletDataExtractor::btBulletDataExtractor()
@ -78,28 +78,25 @@ void btBulletDataExtractor::convertAllObjects(bParse::btBulletFile* bulletFile2)
{
int i;
for (i=0;i<bulletFile2->m_collisionShapes.size();i++)
for (i = 0; i < bulletFile2->m_collisionShapes.size(); i++)
{
btCollisionShapeData* shapeData = (btCollisionShapeData*)bulletFile2->m_collisionShapes[i];
if (shapeData->m_name)
printf("converting shape %s\n", shapeData->m_name);
void* shape = convertCollisionShape(shapeData);
}
}
void* btBulletDataExtractor::convertCollisionShape( btCollisionShapeData* shapeData )
void* btBulletDataExtractor::convertCollisionShape(btCollisionShapeData* shapeData)
{
void* shape = 0;
switch (shapeData->m_shapeType)
{
case STATIC_PLANE_PROXYTYPE:
{
case STATIC_PLANE_PROXYTYPE:
{
btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)shapeData;
void* shape = createPlaneShape(planeData->m_planeNormal,planeData->m_planeConstant, planeData->m_localScaling);
void* shape = createPlaneShape(planeData->m_planeNormal, planeData->m_planeConstant, planeData->m_localScaling);
break;
}
@ -109,21 +106,21 @@ void* btBulletDataExtractor::convertCollisionShape( btCollisionShapeData* shape
case SPHERE_SHAPE_PROXYTYPE:
case MULTI_SPHERE_SHAPE_PROXYTYPE:
case CONVEX_HULL_SHAPE_PROXYTYPE:
{
btConvexInternalShapeData* bsd = (btConvexInternalShapeData*)shapeData;
switch (shapeData->m_shapeType)
{
btConvexInternalShapeData* bsd = (btConvexInternalShapeData*)shapeData;
switch (shapeData->m_shapeType)
case BOX_SHAPE_PROXYTYPE:
{
case BOX_SHAPE_PROXYTYPE:
{
shape = createBoxShape(bsd->m_implicitShapeDimensions, bsd->m_localScaling,bsd->m_collisionMargin);
break;
}
case SPHERE_SHAPE_PROXYTYPE:
{
shape = createSphereShape(bsd->m_implicitShapeDimensions.m_floats[0],bsd->m_localScaling, bsd->m_collisionMargin);
break;
}
shape = createBoxShape(bsd->m_implicitShapeDimensions, bsd->m_localScaling, bsd->m_collisionMargin);
break;
}
case SPHERE_SHAPE_PROXYTYPE:
{
shape = createSphereShape(bsd->m_implicitShapeDimensions.m_floats[0], bsd->m_localScaling, bsd->m_collisionMargin);
break;
}
#if 0
case CAPSULE_SHAPE_PROXYTYPE:
{
@ -221,14 +218,14 @@ void* btBulletDataExtractor::convertCollisionShape( btCollisionShapeData* shape
}
#endif
default:
{
printf("error: cannot create shape type (%d)\n",shapeData->m_shapeType);
}
default:
{
printf("error: cannot create shape type (%d)\n", shapeData->m_shapeType);
}
break;
}
break;
}
#if 0
case TRIANGLE_MESH_SHAPE_PROXYTYPE:
{
@ -257,7 +254,7 @@ void* btBulletDataExtractor::convertCollisionShape( btCollisionShapeData* shape
#ifdef USE_INTERNAL_EDGE_UTILITY
gContactAddedCallback = btAdjustInternalEdgeContactsCallback;
#endif //USE_INTERNAL_EDGE_UTILITY
#endif //USE_INTERNAL_EDGE_UTILITY
}
@ -313,33 +310,30 @@ void* btBulletDataExtractor::convertCollisionShape( btCollisionShapeData* shape
{
return 0;
}
#endif
#endif
default:
{
printf("unsupported shape type (%d)\n",shapeData->m_shapeType);
}
{
printf("unsupported shape type (%d)\n", shapeData->m_shapeType);
}
}
return shape;
return shape;
}
void* btBulletDataExtractor::createBoxShape( const Bullet::btVector3FloatData& halfDimensions, const Bullet::btVector3FloatData& localScaling, float collisionMargin)
void* btBulletDataExtractor::createBoxShape(const Bullet::btVector3FloatData& halfDimensions, const Bullet::btVector3FloatData& localScaling, float collisionMargin)
{
printf("createBoxShape with halfDimensions %f,%f,%f\n",halfDimensions.m_floats[0], halfDimensions.m_floats[1],halfDimensions.m_floats[2]);
printf("createBoxShape with halfDimensions %f,%f,%f\n", halfDimensions.m_floats[0], halfDimensions.m_floats[1], halfDimensions.m_floats[2]);
return 0;
}
void* btBulletDataExtractor::createSphereShape( float radius, const Bullet::btVector3FloatData& localScaling, float collisionMargin)
void* btBulletDataExtractor::createSphereShape(float radius, const Bullet::btVector3FloatData& localScaling, float collisionMargin)
{
printf("createSphereShape with radius %f\n",radius);
printf("createSphereShape with radius %f\n", radius);
return 0;
}
void* btBulletDataExtractor::createPlaneShape( const btVector3FloatData& planeNormal, float planeConstant, const Bullet::btVector3FloatData& localScaling)
void* btBulletDataExtractor::createPlaneShape(const btVector3FloatData& planeNormal, float planeConstant, const Bullet::btVector3FloatData& localScaling)
{
printf("createPlaneShape with normal %f,%f,%f and planeConstant\n",planeNormal.m_floats[0], planeNormal.m_floats[1],planeNormal.m_floats[2],planeConstant);
printf("createPlaneShape with normal %f,%f,%f and planeConstant\n", planeNormal.m_floats[0], planeNormal.m_floats[1], planeNormal.m_floats[2], planeConstant);
return 0;
}

25
Extras/Serialize/ReadBulletSample/BulletDataExtractor.h
View File

@ -1,32 +1,29 @@
#ifndef BULLET_DATA_EXTRACTOR_H
#define BULLET_DATA_EXTRACTOR_H
#include "../BulletFileLoader/autogenerated/bullet.h"
namespace bParse
{
class btBulletFile;
class btBulletFile;
};
class btBulletDataExtractor
{
public:
public:
btBulletDataExtractor();
virtual ~btBulletDataExtractor();
virtual void convertAllObjects(bParse::btBulletFile* bulletFile);
virtual void* convertCollisionShape( Bullet::btCollisionShapeData* shapeData );
virtual void* createPlaneShape( const Bullet::btVector3FloatData& planeNormal, float planeConstant, const Bullet::btVector3FloatData& localScaling);
virtual void* createBoxShape( const Bullet::btVector3FloatData& halfDimensions, const Bullet::btVector3FloatData& localScaling, float collisionMargin);
virtual void* convertCollisionShape(Bullet::btCollisionShapeData* shapeData);
virtual void* createSphereShape( float radius, const Bullet::btVector3FloatData& localScaling, float collisionMargin);
virtual void* createPlaneShape(const Bullet::btVector3FloatData& planeNormal, float planeConstant, const Bullet::btVector3FloatData& localScaling);
virtual void* createBoxShape(const Bullet::btVector3FloatData& halfDimensions, const Bullet::btVector3FloatData& localScaling, float collisionMargin);
virtual void* createSphereShape(float radius, const Bullet::btVector3FloatData& localScaling, float collisionMargin);
};
#endif //BULLET_DATA_EXTRACTOR_H
#endif //BULLET_DATA_EXTRACTOR_H

19
Extras/Serialize/ReadBulletSample/main.cpp
View File

@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include <stdio.h>
#include "../BulletFileLoader/btBulletFile.h"
#include "BulletDataExtractor.h"
@ -25,39 +24,37 @@ subject to the following restrictions:
int main(int argc, char** argv)
{
const char* fileName="testFile.bullet";
const char* fileName = "testFile.bullet";
bool verboseDumpAllTypes = false;
bParse::btBulletFile* bulletFile2 = new bParse::btBulletFile(fileName);
bool ok = (bulletFile2->getFlags()& bParse::FD_OK)!=0;
bool ok = (bulletFile2->getFlags() & bParse::FD_OK) != 0;
if (ok)
bulletFile2->parse(verboseDumpAllTypes);
else
{
printf("Error loading file %s.\n",fileName);
printf("Error loading file %s.\n", fileName);
exit(0);
}
ok = (bulletFile2->getFlags()& bParse::FD_OK)!=0;
ok = (bulletFile2->getFlags() & bParse::FD_OK) != 0;
if (!ok)
{
printf("Error parsing file %s.\n",fileName);
printf("Error parsing file %s.\n", fileName);
exit(0);
}
if (verboseDumpAllTypes)
{
bulletFile2->dumpChunks(bulletFile2->getFileDNA());
}
btBulletDataExtractor extractor;
extractor.convertAllObjects(bulletFile2);
delete bulletFile2;
return 0;
}

17
Extras/Serialize/makesdna/DNA_rigidbody.h
View File

@ -2,20 +2,18 @@
#ifndef DNA_RIGIDBODY_H
#define DNA_RIGIDBODY_H
struct PointerArray
struct PointerArray
{
int m_size;
int m_capacity;
void *m_data;
int m_size;
int m_capacity;
void *m_data;
};
struct btPhysicsSystem
{
PointerArray m_collisionShapes;
PointerArray m_collisionObjects;
PointerArray m_constraints;
PointerArray m_collisionShapes;
PointerArray m_collisionObjects;
PointerArray m_constraints;
};
///we need this to compute the pointer sizes
@ -25,5 +23,4 @@ struct ListBase
void *last;
};
#endif

Some files were not shown because too many files have changed in this diff Show More