bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

migration part 1

This commit is contained in:
kwc 2009-09-04 23:10:08 +00:00
commit 6285603c53
109 changed files with 50909 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
CMakeLists.txt Normal file
View File

@ -0,0 +1,19 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
set(ROSPACK_MAKEDIST true)
# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
# directories (or patterns, but directories should suffice) that should
# be excluded from the distro. This is not the place to put things that
# should be ignored everywhere, like "build" directories; that happens in
# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
# ready for inclusion in a distro.
#
# This list is combined with the list in rosbuild/rosbuild.cmake. Note
# that CMake 2.6 may be required to ensure that the two lists are combined
# properly. CMake 2.4 seems to have unpredictable scoping rules for such
# variables.
#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
rosbuild_make_distribution(0.1.0)

1
Makefile Normal file
View File

@ -0,0 +1 @@
include $(shell rospack find mk)/cmake_stack.mk

1065
convex_decomposition/ConvexDecomposition/ConvexDecomposition/ConvexDecomposition.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

249
convex_decomposition/ConvexDecomposition/ConvexDecomposition/ConvexDecomposition.h Normal file
View File

@ -0,0 +1,249 @@
#ifndef CONVEX_DECOMPOSITION_H
#define CONVEX_DECOMPOSITION_H
namespace ConvexDecomposition
{
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
class ConvexResult
{
public:
ConvexResult(void)
{
mHullVcount = 0;
mHullVertices = 0;
mHullTcount = 0;
mHullIndices = 0;
}
ConvexResult(unsigned int hvcount,const double *hvertices,unsigned int htcount,const unsigned int *hindices)
{
mHullVcount = hvcount;
if ( mHullVcount )
{
mHullVertices = new double[mHullVcount*sizeof(double)*3];
memcpy(mHullVertices, hvertices, sizeof(double)*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) // copy constructor, perform a deep copy of the data.
{
mHullVcount = r.mHullVcount;
if ( mHullVcount )
{
mHullVertices = new double[mHullVcount*sizeof(double)*3];
memcpy(mHullVertices, r.mHullVertices, sizeof(double)*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;
double * mHullVertices;
unsigned int mHullTcount;
unsigned int *mHullIndices;
double mHullVolume; // the volume of the convex hull.
};
// convert from doubles back down to floats.
class FConvexResult
{
public:
FConvexResult(const ConvexResult &r)
{
mHullVcount = r.mHullVcount;
mHullVertices = 0;
if ( mHullVcount )
{
mHullVertices = new float[mHullVcount*3];
const double *src = r.mHullVertices;
float * dest = mHullVertices;
for (unsigned int i=0; i<mHullVcount; i++)
{
dest[0] = (float) src[0];
dest[1] = (float) src[1];
dest[2] = (float) src[2];
src+=3;
dest+=3;
}
}
mHullTcount = r.mHullTcount;
if ( mHullTcount )
{
mHullIndices = new unsigned int[mHullTcount*3];
memcpy(mHullIndices,r.mHullIndices,sizeof(unsigned int)*mHullTcount*3);
}
else
{
mHullIndices = 0;
}
mHullVolume = (float)r.mHullVolume;
}
~FConvexResult(void)
{
delete mHullVertices;
delete mHullIndices;
}
unsigned int mHullVcount;
float * mHullVertices;
unsigned int mHullTcount;
unsigned int *mHullIndices;
float mHullVolume; // the volume of the convex hull.
};
class ConvexDecompInterface
{
public:
virtual void ConvexDebugTri(const double *p1,const double *p2,const double *p3,unsigned int color) { };
virtual void ConvexDebugPoint(const double *p,double dist,unsigned int color) { };
virtual void ConvexDebugBound(const double *bmin,const double *bmax,unsigned int color) { };
virtual void ConvexDebugOBB(const double *sides, const double *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)
{
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 double *mVertices; // start of the vertex position array. Assumes a stride of 3 doubles.
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.
double mCpercent; // the concavity threshold percentage. 0=20 is reasonable.
double 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.
double 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.
};
#endif

483
convex_decomposition/ConvexDecomposition/ConvexDecomposition/bestfit.cpp Normal file
View File

@ -0,0 +1,483 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
// Geometric Tools, Inc.
// http://www.geometrictools.com
// Copyright (c) 1998-2006. All Rights Reserved
//
// The Wild Magic Library (WM3) source code is supplied under the terms of
// the license agreement
// http://www.geometrictools.com/License/WildMagic3License.pdf
// and may not be copied or disclosed except in accordance with the terms
// of that agreement.
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "bestfit.h"
namespace ConvexDecomposition
{
class Vec3
{
public:
Vec3(void) { };
Vec3(double _x,double _y,double _z) { x = _x; y = _y; z = _z; };
double dot(const Vec3 &v)
{
return x*v.x + y*v.y + z*v.z; // the dot product
}
double x;
double y;
double z;
};
class Eigen
{
public:
void DecrSortEigenStuff(void)
{
Tridiagonal(); //diagonalize the matrix.
QLAlgorithm(); //
DecreasingSort();
GuaranteeRotation();
}
void Tridiagonal(void)
{
double fM00 = mElement[0][0];
double fM01 = mElement[0][1];
double fM02 = mElement[0][2];
double fM11 = mElement[1][1];
double fM12 = mElement[1][2];
double fM22 = mElement[2][2];
m_afDiag[0] = fM00;
m_afSubd[2] = 0;
if (fM02 != (double)0.0)
{
double fLength = sqrt(fM01*fM01+fM02*fM02);
double fInvLength = ((double)1.0)/fLength;
fM01 *= fInvLength;
fM02 *= fInvLength;
double fQ = ((double)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] = (double)1.0;
mElement[0][1] = (double)0.0;
mElement[0][2] = (double)0.0;
mElement[1][0] = (double)0.0;
mElement[1][1] = fM01;
mElement[1][2] = fM02;
mElement[2][0] = (double)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] = (double)1.0;
mElement[0][1] = (double)0.0;
mElement[0][2] = (double)0.0;
mElement[1][0] = (double)0.0;
mElement[1][1] = (double)1.0;
mElement[1][2] = (double)0.0;
mElement[2][0] = (double)0.0;
mElement[2][1] = (double)0.0;
mElement[2][2] = (double)1.0;
m_bIsRotation = true;
}
}
bool QLAlgorithm(void)
{
const int iMaxIter = 32;
for (int i0 = 0; i0 <3; i0++)
{
int i1;
for (i1 = 0; i1 < iMaxIter; i1++)
{
int i2;
for (i2 = i0; i2 <= (3-2); i2++)
{
double fTmp = fabs(m_afDiag[i2]) + fabs(m_afDiag[i2+1]);
if ( fabs(m_afSubd[i2]) + fTmp == fTmp )
break;
}
if (i2 == i0)
{
break;
}
double fG = (m_afDiag[i0+1] - m_afDiag[i0])/(((double)2.0) * m_afSubd[i0]);
double fR = sqrt(fG*fG+(double)1.0);
if (fG < (double)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);
}
double fSin = (double)1.0, fCos = (double)1.0, fP = (double)0.0;
for (int i3 = i2-1; i3 >= i0; i3--)
{
double fF = fSin*m_afSubd[i3];
double fB = fCos*m_afSubd[i3];
if (fabs(fF) >= fabs(fG))
{
fCos = fG/fF;
fR = sqrt(fCos*fCos+(double)1.0);
m_afSubd[i3+1] = fF*fR;
fSin = ((double)1.0)/fR;
fCos *= fSin;
}
else
{
fSin = fF/fG;
fR = sqrt(fSin*fSin+(double)1.0);
m_afSubd[i3+1] = fG*fR;
fCos = ((double)1.0)/fR;
fSin *= fCos;
}
fG = m_afDiag[i3+1]-fP;
fR = (m_afDiag[i3]-fG)*fSin+((double)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] = (double)0.0;
}
if (i1 == iMaxIter)
{
return false;
}
}
return true;
}
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;
double fMax = m_afDiag[i1];
int i2;
for (i2 = i0+1; i2 < 3; i2++)
{
if (m_afDiag[i2] > fMax)
{
i1 = i2;
fMax = m_afDiag[i1];
}
}
if (i1 != i0)
{
// swap eigenvalues
m_afDiag[i1] = m_afDiag[i0];
m_afDiag[i0] = fMax;
// swap eigenvectors
for (i2 = 0; i2 < 3; i2++)
{
double 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];
}
}
}
double mElement[3][3];
double m_afDiag[3];
double m_afSubd[3];
bool m_bIsRotation;
};
bool getBestFitPlane(unsigned int vcount,
const double *points,
unsigned int vstride,
const double *weights,
unsigned int wstride,
double *plane)
{
bool ret = false;
Vec3 kOrigin(0,0,0);
double wtotal = 0;
if ( 1 )
{
const char *source = (const char *) points;
const char *wsource = (const char *) weights;
for (unsigned int i=0; i<vcount; i++)
{
const double *p = (const double *) source;
double w = 1;
if ( wsource )
{
const double *ws = (const double *) wsource;
w = *ws; //
wsource+=wstride;
}
kOrigin.x+=p[0]*w;
kOrigin.y+=p[1]*w;
kOrigin.z+=p[2]*w;
wtotal+=w;
source+=vstride;
}
}
double recip = 1.0f / wtotal; // reciprocol of total weighting
kOrigin.x*=recip;
kOrigin.y*=recip;
kOrigin.z*=recip;
double fSumXX=0;
double fSumXY=0;
double fSumXZ=0;
double fSumYY=0;
double fSumYZ=0;
double fSumZZ=0;
if ( 1 )
{
const char *source = (const char *) points;
const char *wsource = (const char *) weights;
for (unsigned int i=0; i<vcount; i++)
{
const double *p = (const double *) source;
double w = 1;
if ( wsource )
{
const double *ws = (const double *) wsource;
w = *ws; //
wsource+=wstride;
}
Vec3 kDiff;
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+= 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.
fSumYY+= kDiff.y * kDiff.y;
fSumYZ+= kDiff.y * kDiff.z;
fSumZZ+= kDiff.z * kDiff.z;
source+=vstride;
}
}
fSumXX *= recip;
fSumXY *= recip;
fSumXZ *= recip;
fSumYY *= recip;
fSumYZ *= recip;
fSumZZ *= recip;
// setup the eigensolver
Eigen kES;
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;
kES.mElement[2][0] = fSumXZ;
kES.mElement[2][1] = fSumYZ;
kES.mElement[2][2] = fSumZZ;
// 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;
}
double getBoundingRegion(unsigned int vcount,const double *points,unsigned int pstride,double *bmin,double *bmax) // returns the diagonal distance
{
const unsigned char *source = (const unsigned char *) points;
bmin[0] = points[0];
bmin[1] = points[1];
bmin[2] = points[2];
bmax[0] = points[0];
bmax[1] = points[1];
bmax[2] = points[2];
for (unsigned int i=1; i<vcount; i++)
{
source+=pstride;
const double *p = (const double *) 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] > bmax[0] ) bmax[0] = p[0];
if ( p[1] > bmax[1] ) bmax[1] = p[1];
if ( p[2] > bmax[2] ) bmax[2] = p[2];
}
double dx = bmax[0] - bmin[0];
double dy = bmax[1] - bmin[1];
double dz = bmax[2] - bmin[2];
return sqrt( dx*dx + dy*dy + dz*dz );
}
bool overlapAABB(const double *bmin1,const double *bmax1,const double *bmin2,const double *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 ( 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
}
}; // end of namespace

90
convex_decomposition/ConvexDecomposition/ConvexDecomposition/bestfit.h Normal file
View File

@ -0,0 +1,90 @@
#ifndef BEST_FIT_H
#define BEST_FIT_H
// This routine was released in 'snippet' form
// by John W. Ratcliff mailto:jratcliff@infiniplex.net
// on March 22, 2006.
//
// This routine computes the 'best fit' plane equation to
// a set of input data points with an optional per vertex
// weighting component.
//
// The implementation for this was lifted directly from
// David Eberly's Magic Software implementation.
// 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)
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
bool getBestFitPlane(unsigned int vcount, // number of input data points
const double *points, // starting address of points array.
unsigned int vstride, // stride between input points.
const double *weights, // *optional point weighting values.
unsigned int wstride, // weight stride for each vertex.
double *plane);
double getBoundingRegion(unsigned int vcount,const double *points,unsigned int pstride,double *bmin,double *bmax); // returns the diagonal distance
bool overlapAABB(const double *bmin1,const double *bmax1,const double *bmin2,const double *bmax2); // return true if the two AABB's overlap.
};
#endif

368
convex_decomposition/ConvexDecomposition/ConvexDecomposition/bestfitobb.cpp Normal file
View File

@ -0,0 +1,368 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <assert.h>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// compute the 'best fit' oriented bounding box of an input point cloud by doing an exhaustive search.
// it spins the point cloud around searching for the minimal volume. It keeps narrowing down until
// it fails to find a better fit. The only dependency is on 'double_math'
//
// The inputs are:
//
// vcount : number of input vertices in the point cloud.
// points : a pointer to the first vertex.
// pstride : The stride between each point measured in bytes.
//
// The outputs are:
//
// sides : The length of the sides of the OBB as X, Y, Z distance.
// matrix : A pointer to a 4x4 matrix. This will contain the 3x3 rotation and the translation component.
// pos : The center of the OBB
// quat : The orientation of the OBB expressed as quaternion in the form of X,Y,Z,W
//
//
// Please email bug fixes or improvements to John W. Ratcliff at mailto:jratcliff@infiniplex.net
//
// If you find this source code useful donate a couple of bucks to my kid's fund raising website at
// www.amillionpixels.us
//
// More snippets at: www.codesuppository.com
//
#include "bestfitobb.h"
#include "float_math.h"
namespace ConvexDecomposition
{
// computes the OBB for this set of points relative to this transform matrix.
void computeOBB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *matrix)
{
const char *src = (const char *) points;
double bmin[3] = { 1e9, 1e9, 1e9 };
double bmax[3] = { -1e9, -1e9, -1e9 };
for (unsigned int i=0; i<vcount; i++)
{
const double *p = (const double *) src;
double t[3];
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] > 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;
}
double center[3];
sides[0] = bmax[0]-bmin[0];
sides[1] = bmax[1]-bmin[1];
sides[2] = bmax[2]-bmin[2];
center[0] = sides[0]*0.5f+bmin[0];
center[1] = sides[1]*0.5f+bmin[1];
center[2] = sides[2]*0.5f+bmin[2];
double ocenter[3];
fm_rotate(matrix,center,ocenter);
matrix[12]+=ocenter[0];
matrix[13]+=ocenter[1];
matrix[14]+=ocenter[2];
}
void computeBestFitOBB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *matrix)
{
double bmin[3];
double bmax[3];
fm_getAABB(vcount,points,pstride,bmin,bmax);
double 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];
double ax = 0;
double ay = 0;
double az = 0;
double sweep = 45.0f; // 180 degree sweep on all three axes.
double steps = 7.0f; // 7 steps on each axis)
double bestVolume = 1e9;
double angle[3];
while ( sweep >= 1 )
{
bool found = false;
double stepsize = sweep / steps;
for (double x=ax-sweep; x<=ax+sweep; x+=stepsize)
{
for (double y=ay-sweep; y<=ay+sweep; y+=stepsize)
{
for (double z=az-sweep; z<=az+sweep; z+=stepsize)
{
double pmatrix[16];
fm_eulerMatrix( x*FM_DEG_TO_RAD, y*FM_DEG_TO_RAD, z*FM_DEG_TO_RAD, pmatrix );
pmatrix[3*4+0] = center[0];
pmatrix[3*4+1] = center[1];
pmatrix[3*4+2] = center[2];
double psides[3];
computeOBB( vcount, points, pstride, psides, pmatrix );
double volume = psides[0]*psides[1]*psides[2]; // the volume of the cube
if ( volume < bestVolume )
{
bestVolume = volume;
sides[0] = psides[0];
sides[1] = psides[1];
sides[2] = psides[2];
angle[0] = ax;
angle[1] = ay;
angle[2] = az;
memcpy(matrix,pmatrix,sizeof(double)*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
}
}
}
void computeBestFitOBB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *pos,double *quat)
{
double matrix[16];
computeBestFitOBB(vcount,points,pstride,sides,matrix);
fm_getTranslation(matrix,pos);
fm_matrixToQuat(matrix,quat);
}
void computeBestFitABB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *pos)
{
double bmin[3];
double bmax[3];
bmin[0] = points[0];
bmin[1] = points[1];
bmin[2] = points[2];
bmax[0] = points[0];
bmax[1] = points[1];
bmax[2] = points[2];
const char *cp = (const char *) points;
for (unsigned int i=0; i<vcount; i++)
{
const double *p = (const double *) cp;
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];
cp+=pstride;
}
sides[0] = bmax[0] - bmin[0];
sides[1] = bmax[1] - bmin[1];
sides[2] = bmax[2] - bmin[2];
pos[0] = bmin[0]+sides[0]*0.5f;
pos[1] = bmin[1]+sides[1]*0.5f;
pos[2] = bmin[2]+sides[2]*0.5f;
}
void computeBestFitOBB(unsigned int vcount,const float *points,unsigned int pstride,float *sides,float *pos,float *quat) // the float version of the routine.
{
double *temp = new double[vcount*3];
const char *src = (const char *)points;
double *dest = temp;
for (unsigned int i=0; i<vcount; i++)
{
const float *s = (const float *) src;
temp[0] = s[0];
temp[1] = s[1];
temp[2] = s[2];
temp+=3;
s+=pstride;
}
double dsides[3];
double dpos[3];
double dquat[3];
computeBestFitOBB(vcount,temp,sizeof(double)*3,dsides,dpos,dquat);
if ( sides )
{
sides[0] = (float) dsides[0];
sides[1] = (float) dsides[1];
sides[2] = (float) dsides[2];
}
if ( pos )
{
pos[0] = (float) dpos[0];
pos[1] = (float) dpos[1];
pos[2] = (float) dpos[2];
}
if ( quat )
{
quat[0] = (float) dquat[0];
quat[1] = (float) dquat[1];
quat[2] = (float) dquat[2];
quat[3] = (float) dquat[3];
}
delete temp;
}
void computeBestFitABB(unsigned int vcount,const float *points,unsigned int pstride,float *sides,float *pos)
{
float bmin[3];
float bmax[3];
bmin[0] = points[0];
bmin[1] = points[1];
bmin[2] = points[2];
bmax[0] = points[0];
bmax[1] = points[1];
bmax[2] = points[2];
const char *cp = (const char *) points;
for (unsigned int i=0; i<vcount; i++)
{
const float *p = (const float *) cp;
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];
cp+=pstride;
}
sides[0] = bmax[0] - bmin[0];
sides[1] = bmax[1] - bmin[1];
sides[2] = bmax[2] - bmin[2];
pos[0] = bmin[0]+sides[0]*0.5f;
pos[1] = bmin[1]+sides[1]*0.5f;
pos[2] = bmin[2]+sides[2]*0.5f;
}
};

100
convex_decomposition/ConvexDecomposition/ConvexDecomposition/bestfitobb.h Normal file
View File

@ -0,0 +1,100 @@
#ifndef BEST_FIT_OBB_H
#define BEST_FIT_OBB_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// compute the 'best fit' oriented bounding box of an input point cloud by doing an exhaustive search.
// it spins the point cloud around searching for the minimal volume. It keeps narrowing down until
// it fails to find a better fit. The only dependency is on 'double_math'
//
// The inputs are:
//
// vcount : number of input vertices in the point cloud.
// points : a pointer to the first vertex.
// pstride : The stride between each point measured in bytes.
//
// The outputs are:
//
// sides : The length of the sides of the OBB as X, Y, Z distance.
// matrix : A pointer to a 4x4 matrix. This will contain the 3x3 rotation and the translation component.
// pos : The center of the OBB
// quat : The orientation of the OBB expressed as quaternion in the form of X,Y,Z,W
//
//
// Please email bug fixes or improvements to John W. Ratcliff at mailto:jratcliff@infiniplex.net
//
// If you find this source code useful donate a couple of bucks to my kid's fund raising website at
// www.amillionpixels.us
//
// More snippets at: www.codesuppository.com
//
namespace ConvexDecomposition
{
void computeBestFitOBB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *matrix);
void computeBestFitOBB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *pos,double *quat);
void computeBestFitABB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *pos);
void computeBestFitOBB(unsigned int vcount,const float *points,unsigned int pstride,float *sides,float *pos,float *quat); // the float version of the routine.
void computeBestFitABB(unsigned int vcount,const float *points,unsigned int pstride,float *sides,float *pos);
};
#endif

3650
convex_decomposition/ConvexDecomposition/ConvexDecomposition/cd_hull.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

250
convex_decomposition/ConvexDecomposition/ConvexDecomposition/cd_hull.h Normal file
View File

@ -0,0 +1,250 @@
#ifndef HULL_LIB_H
#define HULL_LIB_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
class HullResult
{
public:
HullResult(void)
{
mPolygons = true;
mNumOutputVertices = 0;
mOutputVertices = 0;
mNumFaces = 0;
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
double *mOutputVertices; // array of vertices, 3 doubles 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..
};
class FHullResult
{
public:
FHullResult(const HullResult &r)
{
mPolygons = r.mPolygons;
mNumOutputVertices = r.mNumOutputVertices;
mNumFaces = r.mNumFaces;
mNumIndices = r.mNumIndices;
mIndices = 0;
mOutputVertices = 0;
if ( mNumIndices )
{
mIndices = new unsigned int[mNumIndices];
memcpy(mIndices,r.mIndices,sizeof(unsigned int)*mNumIndices);
}
if ( mNumOutputVertices )
{
mOutputVertices = new float[mNumOutputVertices*3];
const double *src = r.mOutputVertices;
float *dst = mOutputVertices;
for (unsigned int i=0; i<mNumOutputVertices; i++)
{
dst[0] = (float) src[0];
dst[1] = (float) src[1];
dst[2] = (float) src[2];
dst+=3;
src+=3;
}
}
}
~FHullResult(void)
{
delete mIndices;
delete mOutputVertices;
}
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 doubles 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..
};
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
};
class HullDesc
{
public:
HullDesc(void)
{
mFlags = QF_DEFAULT;
mVcount = 0;
mVertices = 0;
mVertexStride = 0;
mNormalEpsilon = 0.001f;
mMaxVertices = 4096; // maximum number of points to be considered for a convex hull.
mSkinWidth = 0.01f; // default is one centimeter
};
HullDesc(HullFlag flag,
unsigned int vcount,
const double *vertices,
unsigned int stride)
{
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;
return false;
}
void SetHullFlag(HullFlag flag)
{
mFlags|=flag;
}
void ClearHullFlag(HullFlag 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 double *mVertices; // the array of vertices.
unsigned int mVertexStride; // the stride of each vertex, in bytes.
double mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on.
double mSkinWidth;
unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull!
};
enum HullError
{
QE_OK, // success!
QE_FAIL // failed.
};
// This class is used when converting a convex hull into a triangle mesh.
class ConvexHullVertex
{
public:
double mPos[3];
double mNormal[3];
double mTexel[2];
};
// A virtual interface to receive the triangles from the convex hull.
class ConvexHullTriangleInterface
{
public:
virtual void ConvexHullTriangle(const ConvexHullVertex &v1,const ConvexHullVertex &v2,const ConvexHullVertex &v3) = 0;
};
class HullLibrary
{
public:
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.
// Utility function to convert the output convex hull as a renderable set of triangles. Unfolds the polygons into
// individual triangles, compute the vertex normals, and projects some texture co-ordinates.
HullError CreateTriangleMesh(HullResult &answer,ConvexHullTriangleInterface *iface);
private:
double ComputeNormal(double *n,const double *A,const double *B,const double *C);
void AddConvexTriangle(ConvexHullTriangleInterface *callback,const double *p1,const double *p2,const double *p3);
void BringOutYourDead(const double *verts,unsigned int vcount, double *overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount);
bool CleanupVertices(unsigned int svcount,
const double *svertices,
unsigned int stride,
unsigned int &vcount, // output number of vertices
double *vertices, // location to store the results.
double normalepsilon,
double *scale);
};
};
#endif

1209
convex_decomposition/ConvexDecomposition/ConvexDecomposition/cd_vector.h Normal file
View File

File diff suppressed because it is too large Load Diff

858
convex_decomposition/ConvexDecomposition/ConvexDecomposition/cd_wavefront.cpp Normal file
View File

@ -0,0 +1,858 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <ctype.h>
#pragma warning(disable:4996)
#include "cd_wavefront.h"
using namespace ConvexDecomposition;
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <vector>
namespace ConvexDecomposition
{
typedef std::vector< int > IntVector;
typedef std::vector< double > FloatVector;
#if defined(__APPLE__) || defined(__CELLOS_LV2__)
#define stricmp(a, b) strcasecmp((a), (b))
#endif
/*******************************************************************/
/******************** InParser.h ********************************/
/*******************************************************************/
class InPlaceParserInterface
{
public:
virtual int ParseLine(int lineno,int argc,const char **argv) =0; // return TRUE to continue parsing, return FALSE to abort parsing process
};
enum SeparatorType
{
ST_DATA, // is data
ST_HARD, // is a hard separator
ST_SOFT, // is a soft separator
ST_EOS // is a comment symbol, and everything past this character should be ignored
};
class InPlaceParser
{
public:
InPlaceParser(void)
{
Init();
}
InPlaceParser(char *data,int len)
{
Init();
SetSourceData(data,len);
}
InPlaceParser(const char *fname)
{
Init();
SetFile(fname);
}
~InPlaceParser(void);
void Init(void)
{
mQuoteChar = 34;
mData = 0;
mLen = 0;
mMyAlloc = false;
for (int i=0; i<256; i++)
{
mHard[i] = ST_DATA;
mHardString[i*2] = i;
mHardString[i*2+1] = 0;
}
mHard[0] = ST_EOS;
mHard[32] = ST_SOFT;
mHard[9] = ST_SOFT;
mHard[13] = ST_SOFT;
mHard[10] = ST_SOFT;
}
void SetFile(const char *fname); // use this file as source data to parse.
void SetSourceData(char *data,int len)
{
mData = data;
mLen = len;
mMyAlloc = false;
};
int Parse(InPlaceParserInterface *callback); // returns true if entire file was parsed, false if it aborted for some reason
int ProcessLine(int lineno,char *line,InPlaceParserInterface *callback);
const char ** GetArglist(char *source,int &count); // convert source string into an arg list, this is a destructive parse.
void SetHardSeparator(char c) // add a hard separator
{
mHard[c] = ST_HARD;
}
void SetHard(char c) // add a hard separator
{
mHard[c] = ST_HARD;
}
void SetCommentSymbol(char c) // comment character, treated as 'end of string'
{
mHard[c] = ST_EOS;
}
void ClearHardSeparator(char c)
{
mHard[c] = ST_DATA;
}
void DefaultSymbols(void); // set up default symbols for hard seperator and comment symbol of the '#' character.
bool EOS(char c)
{
if ( mHard[c] == ST_EOS )
{
return true;
}
return false;
}
void SetQuoteChar(char c)
{
mQuoteChar = c;
}
private:
inline char * AddHard(int &argc,const char **argv,char *foo);
inline bool IsHard(char c);
inline char * SkipSpaces(char *foo);
inline bool IsWhiteSpace(char c);
inline bool IsNonSeparator(char c); // non seperator,neither hard nor soft
bool mMyAlloc; // whether or not *I* allocated the buffer and am responsible for deleting it.
char *mData; // ascii data to parse.
int mLen; // length of data
SeparatorType mHard[256];
char mHardString[256*2];
char mQuoteChar;
};
/*******************************************************************/
/******************** InParser.cpp ********************************/
/*******************************************************************/
void InPlaceParser::SetFile(const char *fname)
{
if ( mMyAlloc )
{
free(mData);
}
mData = 0;
mLen = 0;
mMyAlloc = false;
FILE *fph = fopen(fname,"rb");
if ( fph )
{
fseek(fph,0L,SEEK_END);
mLen = ftell(fph);
fseek(fph,0L,SEEK_SET);
if ( mLen )
{
mData = (char *) malloc(sizeof(char)*(mLen+1));
int ok = fread(mData, mLen, 1, fph);
if ( !ok )
{
free(mData);
mData = 0;
}
else
{
mData[mLen] = 0; // zero byte terminate end of file marker.
mMyAlloc = true;
}
}
fclose(fph);
}
}
InPlaceParser::~InPlaceParser(void)
{
if ( mMyAlloc )
{
free(mData);
}
}
#define MAXARGS 512
bool InPlaceParser::IsHard(char c)
{
return mHard[c] == ST_HARD;
}
char * InPlaceParser::AddHard(int &argc,const char **argv,char *foo)
{
while ( IsHard(*foo) )
{
const char *hard = &mHardString[*foo*2];
if ( argc < MAXARGS )
{
argv[argc++] = hard;
}
foo++;
}
return foo;
}
bool InPlaceParser::IsWhiteSpace(char c)
{
return mHard[c] == ST_SOFT;
}
char * InPlaceParser::SkipSpaces(char *foo)
{
while ( !EOS(*foo) && IsWhiteSpace(*foo) ) foo++;
return foo;
}
bool InPlaceParser::IsNonSeparator(char c)
{
if ( !IsHard(c) && !IsWhiteSpace(c) && c != 0 ) return true;
return false;
}
int InPlaceParser::ProcessLine(int lineno,char *line,InPlaceParserInterface *callback)
{
int ret = 0;
const char *argv[MAXARGS];
int argc = 0;
char *foo = line;
while ( !EOS(*foo) && argc < MAXARGS )
{
foo = SkipSpaces(foo); // skip any leading spaces
if ( EOS(*foo) ) break;
if ( *foo == mQuoteChar ) // if it is an open quote
{
foo++;
if ( argc < MAXARGS )
{
argv[argc++] = foo;
}
while ( !EOS(*foo) && *foo != mQuoteChar ) foo++;
if ( !EOS(*foo) )
{
*foo = 0; // replace close quote with zero byte EOS
foo++;
}
}
else
{
foo = AddHard(argc,argv,foo); // add any hard separators, skip any spaces
if ( IsNonSeparator(*foo) ) // add non-hard argument.
{
bool quote = false;
if ( *foo == mQuoteChar )
{
foo++;
quote = true;
}
if ( argc < MAXARGS )
{
argv[argc++] = foo;
}
if ( quote )
{
while (*foo && *foo != mQuoteChar ) foo++;
if ( *foo ) *foo = 32;
}
// continue..until we hit an eos ..
while ( !EOS(*foo) ) // until we hit EOS
{
if ( IsWhiteSpace(*foo) ) // if we hit a space, stomp a zero byte, and exit
{
*foo = 0;
foo++;
break;
}
else if ( IsHard(*foo) ) // if we hit a hard separator, stomp a zero byte and store the hard separator argument
{
const char *hard = &mHardString[*foo*2];
*foo = 0;
if ( argc < MAXARGS )
{
argv[argc++] = hard;
}
foo++;
break;
}
foo++;
} // end of while loop...
}
}
}
if ( argc )
{
ret = callback->ParseLine(lineno, argc, argv );
}
return ret;
}
int InPlaceParser::Parse(InPlaceParserInterface *callback) // returns true if entire file was parsed, false if it aborted for some reason
{
assert( callback );
if ( !mData ) return 0;
int ret = 0;
int lineno = 0;
char *foo = mData;
char *begin = foo;
while ( *foo )
{
if ( *foo == 10 || *foo == 13 )
{
lineno++;
*foo = 0;
if ( *begin ) // if there is any data to parse at all...
{
int v = ProcessLine(lineno,begin,callback);
if ( v ) ret = v;
}
foo++;
if ( *foo == 10 ) foo++; // skip line feed, if it is in the carraige-return line-feed format...
begin = foo;
}
else
{
foo++;
}
}
lineno++; // lasst line.
int v = ProcessLine(lineno,begin,callback);
if ( v ) ret = v;
return ret;
}
void InPlaceParser::DefaultSymbols(void)
{
SetHardSeparator(',');
SetHardSeparator('(');
SetHardSeparator(')');
SetHardSeparator('=');
SetHardSeparator('[');
SetHardSeparator(']');
SetHardSeparator('{');
SetHardSeparator('}');
SetCommentSymbol('#');
}
const char ** InPlaceParser::GetArglist(char *line,int &count) // convert source string into an arg list, this is a destructive parse.
{
const char **ret = 0;
static const char *argv[MAXARGS];
int argc = 0;
char *foo = line;
while ( !EOS(*foo) && argc < MAXARGS )
{
foo = SkipSpaces(foo); // skip any leading spaces
if ( EOS(*foo) ) break;
if ( *foo == mQuoteChar ) // if it is an open quote
{
foo++;
if ( argc < MAXARGS )
{
argv[argc++] = foo;
}
while ( !EOS(*foo) && *foo != mQuoteChar ) foo++;
if ( !EOS(*foo) )
{
*foo = 0; // replace close quote with zero byte EOS
foo++;
}
}
else
{
foo = AddHard(argc,argv,foo); // add any hard separators, skip any spaces
if ( IsNonSeparator(*foo) ) // add non-hard argument.
{
bool quote = false;
if ( *foo == mQuoteChar )
{
foo++;
quote = true;
}
if ( argc < MAXARGS )
{
argv[argc++] = foo;
}
if ( quote )
{
while (*foo && *foo != mQuoteChar ) foo++;
if ( *foo ) *foo = 32;
}
// continue..until we hit an eos ..
while ( !EOS(*foo) ) // until we hit EOS
{
if ( IsWhiteSpace(*foo) ) // if we hit a space, stomp a zero byte, and exit
{
*foo = 0;
foo++;
break;
}
else if ( IsHard(*foo) ) // if we hit a hard separator, stomp a zero byte and store the hard separator argument
{
const char *hard = &mHardString[*foo*2];
*foo = 0;
if ( argc < MAXARGS )
{
argv[argc++] = hard;
}
foo++;
break;
}
foo++;
} // end of while loop...
}
}
}
count = argc;
if ( argc )
{
ret = argv;
}
return ret;
}
/*******************************************************************/
/******************** Geometry.h ********************************/
/*******************************************************************/
class GeometryVertex
{
public:
double mPos[3];
double mNormal[3];
double mTexel[2];
};
class GeometryInterface
{
public:
virtual void NodeTriangle(const GeometryVertex *v1,const GeometryVertex *v2,const GeometryVertex *v3)
{
}
};
/*******************************************************************/
/******************** Obj.h ********************************/
/*******************************************************************/
class OBJ : public InPlaceParserInterface
{
public:
int LoadMesh(const char *fname,GeometryInterface *callback);
int ParseLine(int lineno,int argc,const char **argv); // return TRUE to continue parsing, return FALSE to abort parsing process
private:
void GetVertex(GeometryVertex &v,const char *face) const;
FloatVector mVerts;
FloatVector mTexels;
FloatVector mNormals;
GeometryInterface *mCallback;
friend class WavefrontObj;
};
/*******************************************************************/
/******************** Obj.cpp ********************************/
/*******************************************************************/
int OBJ::LoadMesh(const char *fname,GeometryInterface *iface)
{
int ret = 0;
mVerts.clear();
mTexels.clear();
mNormals.clear();
mCallback = iface;
InPlaceParser ipp(fname);
ipp.Parse(this);
return ret;
}
static const char * GetArg(const char **argv,int i,int argc)
{
const char * ret = 0;
if ( i < argc ) ret = argv[i];
return ret;
}
void OBJ::GetVertex(GeometryVertex &v,const char *face) const
{
v.mPos[0] = 0;
v.mPos[1] = 0;
v.mPos[2] = 0;
v.mTexel[0] = 0;
v.mTexel[1] = 0;
v.mNormal[0] = 0;
v.mNormal[1] = 1;
v.mNormal[2] = 0;
int index = atoi( face )-1;
const char *texel = strstr(face,"/");
if ( texel )
{
int tindex = atoi( texel+1) - 1;
if ( tindex >=0 && tindex < (int)(mTexels.size()/2) )
{
const double *t = &mTexels[tindex*2];
v.mTexel[0] = t[0];
v.mTexel[1] = t[1];
}
const char *normal = strstr(texel+1,"/");
if ( normal )
{
int nindex = atoi( normal+1 ) - 1;
if (nindex >= 0 && nindex < (int)(mNormals.size()/3) )
{
const double *n = &mNormals[nindex*3];
v.mNormal[0] = n[0];
v.mNormal[1] = n[1];
v.mNormal[2] = n[2];
}
}
}
if ( index >= 0 && index < (int)(mVerts.size()/3) )
{
const double *p = &mVerts[index*3];
v.mPos[0] = p[0];
v.mPos[1] = p[1];
v.mPos[2] = p[2];
}
}
int OBJ::ParseLine(int lineno,int argc,const char **argv) // return TRUE to continue parsing, return FALSE to abort parsing process
{
int ret = 0;
if ( argc >= 1 )
{
const char *foo = argv[0];
if ( *foo != '#' )
{
if ( strcmp(argv[0],"v") == 0 && argc == 4 )
{
double vx = (double) atof( argv[1] );
double vy = (double) atof( argv[2] );
double vz = (double) atof( argv[3] );
mVerts.push_back(vx);
mVerts.push_back(vy);
mVerts.push_back(vz);
}
else if ( strcmp(argv[0],"vt") == 0 && argc == 3 )
{
double tx = (double) atof( argv[1] );
double ty = (double) atof( argv[2] );
mTexels.push_back(tx);
mTexels.push_back(ty);
}
else if ( strcmp(argv[0],"vn") == 0 && argc == 4 )
{
double normalx = (double) atof(argv[1]);
double normaly = (double) atof(argv[2]);
double normalz = (double) atof(argv[3]);
mNormals.push_back(normalx);
mNormals.push_back(normaly);
mNormals.push_back(normalz);
}
else if ( strcmp(argv[0],"f") == 0 && argc >= 4 )
{
GeometryVertex v[32];
int vcount = argc-1;
for (int i=1; i<argc; i++)
{
GetVertex(v[i-1],argv[i] );
}
// need to generate a normal!
#if 0 // not currently implemented
if ( mNormals.empty() )
{
Vector3d<double> p1( v[0].mPos );
Vector3d<double> p2( v[1].mPos );
Vector3d<double> p3( v[2].mPos );
Vector3d<double> n;
n.ComputeNormal(p3,p2,p1);
for (int i=0; i<vcount; i++)
{
v[i].mNormal[0] = n.x;
v[i].mNormal[1] = n.y;
v[i].mNormal[2] = n.z;
}
}
#endif
mCallback->NodeTriangle(&v[0],&v[1],&v[2]);
if ( vcount >=3 ) // do the fan
{
for (int i=2; i<(vcount-1); i++)
{
mCallback->NodeTriangle(&v[0],&v[i],&v[i+1]);
}
}
}
}
}
return ret;
}
class BuildMesh : public GeometryInterface
{
public:
int GetIndex(const double *p)
{
int vcount = mVertices.size()/3;
if(vcount>0)
{
//New MS STL library checks indices in debug build, so zero causes an assert if it is empty.
const double *v = &mVertices[0];
for (int i=0; i<vcount; i++)
{
if ( v[0] == p[0] && v[1] == p[1] && v[2] == p[2] ) return i;
v+=3;
}
}
mVertices.push_back( p[0] );
mVertices.push_back( p[1] );
mVertices.push_back( p[2] );
return vcount;
}
virtual void NodeTriangle(const GeometryVertex *v1,const GeometryVertex *v2,const GeometryVertex *v3)
{
mIndices.push_back( GetIndex(v1->mPos) );
mIndices.push_back( GetIndex(v2->mPos) );
mIndices.push_back( GetIndex(v3->mPos) );
}
const FloatVector& GetVertices(void) const { return mVertices; };
const IntVector& GetIndices(void) const { return mIndices; };
private:
FloatVector mVertices;
IntVector mIndices;
};
WavefrontObj::WavefrontObj(void)
{
mVertexCount = 0;
mTriCount = 0;
mIndices = 0;
mVertices = 0;
}
WavefrontObj::~WavefrontObj(void)
{
delete mIndices;
delete mVertices;
}
unsigned int WavefrontObj::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 ret = 0;
delete mVertices;
mVertices = 0;
delete mIndices;
mIndices = 0;
mVertexCount = 0;
mTriCount = 0;
BuildMesh bm;
OBJ obj;
obj.LoadMesh(fname,&bm);
const FloatVector &vlist = bm.GetVertices();
const IntVector &indices = bm.GetIndices();
if ( vlist.size() )
{
mVertexCount = vlist.size()/3;
mVertices = new double[mVertexCount*3];
memcpy( mVertices, &vlist[0], sizeof(double)*mVertexCount*3 );
mTriCount = indices.size()/3;
mIndices = new int[mTriCount*3*sizeof(int)];
memcpy(mIndices, &indices[0], sizeof(int)*mTriCount*3);
ret = mTriCount;
}
else if( obj.mVerts.size() > 0 ) {
// take consecutive vertices
mVertexCount = obj.mVerts.size()/3;
mVertices = new double[mVertexCount*3];
memcpy( mVertices, &obj.mVerts[0], sizeof(double)*mVertexCount*3 );
mTriCount = mVertexCount/3;
mIndices = new int[mTriCount*3*sizeof(int)];
for(int i = 0; i < mVertexCount; ++i)
mIndices[i] = i;
ret = mTriCount;
}
return ret;
}
};

82
convex_decomposition/ConvexDecomposition/ConvexDecomposition/cd_wavefront.h Normal file
View File

@ -0,0 +1,82 @@
#ifndef CD_WAVEFRONT_OBJ_H
#define CD_WAVEFRONT_OBJ_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
class WavefrontObj
{
public:
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.
int mVertexCount;
int mTriCount;
int *mIndices;
double *mVertices;
};
};
#endif

821
convex_decomposition/ConvexDecomposition/ConvexDecomposition/concavity.cpp Normal file
View File

@ -0,0 +1,821 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <vector>
#include "concavity.h"
#include "raytri.h"
#include "bestfit.h"
#include "cd_hull.h"
#include "meshvolume.h"
#include "cd_vector.h"
#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
};
static int count = 0;
count++;
if ( count == 8 ) count = 0;
assert( count >= 0 && count < 8 );
unsigned int color = colors[count];
return color;
}
class Wpoint
{
public:
Wpoint(const Vector3d<double> &p,double w)
{
mPoint = p;
mWeight = w;
}
Vector3d<double> mPoint;
double mWeight;
};
typedef std::vector< Wpoint > WpointVector;
static inline double DistToPt(const double *p,const double *plane)
{
double x = p[0];
double y = p[1];
double z = p[2];
double d = x*plane[0] + y*plane[1] + z*plane[2] + plane[3];
return d;
}
static void intersect(const double *p1,const double *p2,double *split,const double *plane)
{
double dp1 = DistToPt(p1,plane);
double dp2 = DistToPt(p2,plane);
double dir[3];
dir[0] = p2[0] - p1[0];
dir[1] = p2[1] - p1[1];
dir[2] = p2[2] - p1[2];
double dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2];
double dot2 = dp1 - plane[3];
double 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];
}
class CTri
{
public:
CTri(void) { };
CTri(const double *p1,const double *p2,const double *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);
mPlaneD = mNormal.ComputePlane(mP1,mP2,mP3);
}
double Facing(const CTri &t)
{
double d = mNormal.Dot(t.mNormal);
return d;
}
// clip this line segment against this triangle.
bool clip(const Vector3d<double> &start,Vector3d<double> &end) const
{
Vector3d<double> 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<double> &p,double &distance,Vector3d<double> &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
{
indices[tcount*3+0] = i1;
indices[tcount*3+1] = i2;
indices[tcount*3+2] = i3;
tcount++;
}
double getVolume(ConvexDecompInterface *callback) const
{
unsigned int indices[8*3];
unsigned int tcount = 0;
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,1,4,5,tcount);
addTri(indices,1,5,2,tcount);
addTri(indices,0,3,5,tcount);
addTri(indices,0,5,2,tcount);
const double *vertices = mP1.Ptr();
if ( callback )
{
unsigned int color = getDebugColor();
#if 0
Vector3d<double> d1 = mNear1;
Vector3d<double> d2 = mNear2;
Vector3d<double> d3 = mNear3;
callback->ConvexDebugPoint(mP1.Ptr(),0.01f,0x00FF00);
callback->ConvexDebugPoint(mP2.Ptr(),0.01f,0x00FF00);
callback->ConvexDebugPoint(mP3.Ptr(),0.01f,0x00FF00);
callback->ConvexDebugPoint(d1.Ptr(),0.01f,0xFF0000);
callback->ConvexDebugPoint(d2.Ptr(),0.01f,0xFF0000);
callback->ConvexDebugPoint(d3.Ptr(),0.01f,0xFF0000);
callback->ConvexDebugTri(mP1.Ptr(), d1.Ptr(), d1.Ptr(),0x00FF00);
callback->ConvexDebugTri(mP2.Ptr(), d2.Ptr(), d2.Ptr(),0x00FF00);
callback->ConvexDebugTri(mP3.Ptr(), d3.Ptr(), d3.Ptr(),0x00FF00);
#else
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];
const double *p1 = &vertices[ i1*3 ];
const double *p2 = &vertices[ i2*3 ];
const double *p3 = &vertices[ i3*3 ];
callback->ConvexDebugTri(p1,p2,p3,color);
}
#endif
}
double v = computeMeshVolume(mP1.Ptr(), tcount, indices );
return v;
}
double raySect(const Vector3d<double> &p,const Vector3d<double> &dir,Vector3d<double> &sect) const
{
double plane[4];
plane[0] = mNormal.x;
plane[1] = mNormal.y;
plane[2] = mNormal.z;
plane[3] = mPlaneD;
Vector3d<double> dest = p+dir*100000;
intersect( p.Ptr(), dest.Ptr(), sect.Ptr(), plane );
return sect.Distance(p); // return the intersection distance.
}
double planeDistance(const Vector3d<double> &p) const
{
double plane[4];
plane[0] = mNormal.x;
plane[1] = mNormal.y;
plane[2] = mNormal.z;
plane[3] = mPlaneD;
return DistToPt( p.Ptr(), plane );
}
bool samePlane(const CTri &t) const
{
const double THRESH = 0.001f;
double dd = fabs( t.mPlaneD - mPlaneD );
if ( dd > THRESH ) return false;
dd = fabs( t.mNormal.x - mNormal.x );
if ( dd > THRESH ) return false;
dd = fabs( t.mNormal.y - mNormal.y );
if ( dd > THRESH ) return false;
dd = fabs( 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;
return false;
}
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 ( count >= 2 ) ret = true;
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 );
}
double area(void)
{
double 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);
Vector3d<double> d1 = mNear1 - mP1;
Vector3d<double> d2 = mNear2 - mP2;
Vector3d<double> d3 = mNear3 - mP3;
d1*=WSCALE;
d2*=WSCALE;
d3*=WSCALE;
d1 = d1 + mP1;
d2 = d2 + mP2;
d3 = d3 + mP3;
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(p4);
list.push_back(p5);
list.push_back(p6);
#if 0
callback->ConvexDebugPoint(mP1.Ptr(),0.01f,0x00FF00);
callback->ConvexDebugPoint(mP2.Ptr(),0.01f,0x00FF00);
callback->ConvexDebugPoint(mP3.Ptr(),0.01f,0x00FF00);
callback->ConvexDebugPoint(d1.Ptr(),0.01f,0xFF0000);
callback->ConvexDebugPoint(d2.Ptr(),0.01f,0xFF0000);
callback->ConvexDebugPoint(d3.Ptr(),0.01f,0xFF0000);
callback->ConvexDebugTri(mP1.Ptr(), d1.Ptr(), d1.Ptr(),0x00FF00);
callback->ConvexDebugTri(mP2.Ptr(), d2.Ptr(), d2.Ptr(),0x00FF00);
callback->ConvexDebugTri(mP3.Ptr(), d3.Ptr(), d3.Ptr(),0x00FF00);
Vector3d<double> np1 = mP1 + mNormal*0.05f;
Vector3d<double> np2 = mP2 + mNormal*0.05f;
Vector3d<double> np3 = mP3 + mNormal*0.05f;
callback->ConvexDebugTri(mP1.Ptr(), np1.Ptr(), np1.Ptr(), 0xFF00FF );
callback->ConvexDebugTri(mP2.Ptr(), np2.Ptr(), np2.Ptr(), 0xFF00FF );
callback->ConvexDebugTri(mP3.Ptr(), np3.Ptr(), np3.Ptr(), 0xFF00FF );
callback->ConvexDebugPoint( np1.Ptr(), 0.01F, 0XFF00FF );
callback->ConvexDebugPoint( np2.Ptr(), 0.01F, 0XFF00FF );
callback->ConvexDebugPoint( np3.Ptr(), 0.01F, 0XFF00FF );
#endif
}
Vector3d<double> mP1;
Vector3d<double> mP2;
Vector3d<double> mP3;
Vector3d<double> mNear1;
Vector3d<double> mNear2;
Vector3d<double> mNear3;
Vector3d<double> mNormal;
double mPlaneD;
double mConcavity;
double mC1;
double mC2;
double mC3;
unsigned int mI1;
unsigned int mI2;
unsigned int mI3;
int mProcessed; // already been added...
};
typedef std::vector< CTri > CTriVector;
bool featureMatch(CTri &m,const CTriVector &tris,ConvexDecompInterface *callback,const CTriVector &input_mesh)
{
bool ret = false;
double neardot = 0.707f;
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 );
//gLog->Display("*********************************************\r\n");
CTriVector::const_iterator i;
CTri nearest;
double near[3] = { 1e9, 1e9, 1e9 };
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 );
if ( t.samePlane(m) )
{
//gLog->Display("*** PLANE MATCH!!!\r\n");
ret = false;
break;
}
double dot = t.mNormal.Dot(m.mNormal);
if ( dot > neardot )
{
double d1 = t.planeDistance( m.mP1 );
double d2 = t.planeDistance( m.mP2 );
double d3 = t.planeDistance( m.mP3 );
if ( d1 > 0.001f || d2 > 0.001f || d3 > 0.001f ) // can't be near coplaner!
{
neardot = dot;
Vector3d<double> 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 );
nearest = t;
ret = true;
}
}
}
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 );
}
}
}
//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.mConcavity = m.mC1;
if ( m.mC2 > m.mConcavity ) m.mConcavity = m.mC2;
if ( m.mC3 > m.mConcavity ) m.mConcavity = m.mC3;
#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
}
else
{
//gLog->Display("No match\r\n");
}
//gLog->Display("*********************************************\r\n");
return ret;
}
bool isFeatureTri(CTri &t,CTriVector &flist,double fc,ConvexDecompInterface *callback,unsigned int color)
{
bool ret = false;
if ( t.mProcessed == 0 ) // if not already processed
{
double 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() )
{
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 );
ret = true;
}
}
else
{
t.mProcessed = 1; // eliminated for this feature, but might be valid for the next one..
}
}
return ret;
}
double computeConcavity(unsigned int vcount,
const double *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
double *plane, // plane equation to split on
double &volume)
{
double cret = 0;
volume = 1;
HullResult result;
HullLibrary hl;
HullDesc desc;
desc.mMaxVertices = 256;
desc.SetHullFlag(QF_TRIANGLES);
desc.mVcount = vcount;
desc.mVertices = vertices;
desc.mVertexStride = sizeof(double)*3;
HullError ret = hl.CreateConvexHull(desc,result);
if ( ret == QE_OK )
{
double bmin[3];
double bmax[3];
double diagonal = getBoundingRegion( result.mNumOutputVertices, result.mOutputVertices, sizeof(double)*3, bmin, bmax );
double dx = bmax[0] - bmin[0];
double dy = bmax[1] - bmin[1];
double dz = bmax[2] - bmin[2];
Vector3d<double> center;
center.x = bmin[0] + dx*0.5f;
center.y = bmin[1] + dy*0.5f;
center.z = bmin[2] + dz*0.5f;
double boundVolume = dx*dy*dz;
volume = computeMeshVolume2( result.mOutputVertices, result.mNumFaces, result.mIndices );
#if 1
// ok..now..for each triangle on the original mesh..
// we extrude the points to the nearest point on the hull.
const unsigned int *source = result.mIndices;
CTriVector tris;
for (unsigned int i=0; i<result.mNumFaces; i++)
{
unsigned int i1 = *source++;
unsigned int i2 = *source++;
unsigned int i3 = *source++;
const double *p1 = &result.mOutputVertices[i1*3];
const double *p2 = &result.mOutputVertices[i2*3];
const double *p3 = &result.mOutputVertices[i3*3];
// callback->ConvexDebugTri(p1,p2,p3,0xFFFFFF);
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..
double totalVolume = 0;
CTriVector ftris; // 'feature' triangles.
const unsigned int *src = indices;
double maxc=0;
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 double *p1 = &vertices[i1*3];
const double *p2 = &vertices[i2*3];
const double *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++)
{
unsigned int i1 = *src++;
unsigned int i2 = *src++;
unsigned int i3 = *src++;
const double *p1 = &vertices[i1*3];
const double *p2 = &vertices[i2*3];
const double *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;
}
double v = t.getVolume(0);
totalVolume+=v;
ftris.push_back(t);
}
}
}
if ( ftris.size() && 0 )
{
// ok..now we extract the triangles which form the maximum concavity.
CTriVector major_feature;
double maxarea = 0;
while ( maxc > CONCAVE_THRESH )
{
unsigned int color = getDebugColor(); //
CTriVector flist;
bool found;
double totalarea = 0;
do
{
found = false;
CTriVector::iterator i;
for (i=ftris.begin(); i!=ftris.end(); ++i)
{
CTri &t = (*i);
if ( isFeatureTri(t,flist,maxc,callback,color) )
{
found = true;
totalarea+=t.area();
}
}
} while ( found );
if ( totalarea > maxarea )
{
major_feature = flist;
maxarea = totalarea;
}
maxc = 0;
for (unsigned int i=0; i<ftris.size(); i++)
{
CTri &t = ftris[i];
if ( t.mProcessed != 2 )
{
t.mProcessed = 0;
if ( t.mConcavity > maxc )
{
maxc = t.mConcavity;
}
}
}
}
unsigned int color = getDebugColor();
WpointVector list;
for (unsigned int i=0; i<major_feature.size(); ++i)
{
major_feature[i].addWeighted(list,callback);
major_feature[i].debug(color,callback);
}
getBestFitPlane( list.size(), &list[0].mPoint.x, sizeof(Wpoint), &list[0].mWeight, sizeof(Wpoint), plane );
computeSplitPlane( vcount, vertices, tcount, indices, callback, plane );
}
else
{
computeSplitPlane( vcount, vertices, tcount, indices, callback, plane );
}
#endif
cret = totalVolume;
hl.ReleaseResult(result);
}
return cret;
}
};

81
convex_decomposition/ConvexDecomposition/ConvexDecomposition/concavity.h Normal file
View File

@ -0,0 +1,81 @@
#ifndef COMPUTE_CONCAVITY_H
#define COMPUTE_CONCAVITY_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
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.
double computeConcavity(unsigned int vcount,
const double *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
double *plane,
double &volume);
};
#endif

378
convex_decomposition/ConvexDecomposition/ConvexDecomposition/fitsphere.cpp Normal file
View File

@ -0,0 +1,378 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include "fitsphere.h"
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/*
An Efficient Bounding Sphere
by Jack Ritter
from "Graphics Gems", Academic Press, 1990
*/
/* Routine to calculate tight bounding sphere over */
/* a set of points in 3D */
/* This contains the routine find_bounding_sphere(), */
/* the struct definition, and the globals used for parameters. */
/* The abs() of all coordinates must be < BIGNUMBER */
/* Code written by Jack Ritter and Lyle Rains. */
namespace ConvexDecomposition
{
#define BIGNUMBER 100000000.0 /* hundred million */
static inline void Set(double *n,double x,double y,double z)
{
n[0] = x;
n[1] = y;
n[2] = z;
};
static inline void Copy(double *dest,const double *source)
{
dest[0] = source[0];
dest[1] = source[1];
dest[2] = source[2];
}
double computeBoundingSphere(unsigned int vcount,const double *points,double *center)
{
double mRadius;
double mRadius2;
double xmin[3];
double xmax[3];
double ymin[3];
double ymax[3];
double zmin[3];
double zmax[3];
double dia1[3];
double 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);
for (unsigned i=0; i<vcount; i++)
{
const double *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);
}
/* Set xspan = distance between the 2 points xmin & xmax (squared) */
double dx = xmax[0] - xmin[0];
double dy = xmax[1] - xmin[1];
double dz = xmax[2] - xmin[2];
double 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];
double yspan = dx*dx + dy*dy + dz*dz;
dx = zmax[0] - zmin[0];
dy = zmax[1] - zmin[1];
dz = zmax[2] - zmin[2];
double 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 */
double maxspan = xspan;
if (yspan>maxspan)
{
maxspan = yspan;
Copy(dia1,ymin);
Copy(dia2,ymax);
}
if (zspan>maxspan)
{
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;
/* 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 */
mRadius2 = dx*dx + dy*dy + dz*dz;
mRadius = double(sqrt(mRadius2));
/* SECOND PASS: increment current sphere */
if ( 1 )
{
for (unsigned i=0; i<vcount; i++)
{
const double *caller_p = &points[i*3];
dx = caller_p[0]-center[0];
dy = caller_p[1]-center[1];
dz = caller_p[2]-center[2];
double 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 */
double old_to_p = double(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 */
double old_to_new = old_to_p - mRadius;
/* calc center of new sphere */
double recip = 1.0f /old_to_p;
double cx = (mRadius*center[0] + old_to_new*caller_p[0]) * recip;
double cy = (mRadius*center[1] + old_to_new*caller_p[1]) * recip;
double cz = (mRadius*center[2] + old_to_new*caller_p[2]) * recip;
Set(center,cx,cy,cz);
}
}
}
return mRadius;
}
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)
{
dest[0] = source[0];
dest[1] = source[1];
dest[2] = source[2];
}
float computeBoundingSphere(unsigned int vcount,const float *points,float *center)
{
float mRadius;
float mRadius2;
float xmin[3];
float xmax[3];
float ymin[3];
float ymax[3];
float zmin[3];
float zmax[3];
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);
for (unsigned i=0; i<vcount; i++)
{
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);
}
/* 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;
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;
if (yspan>maxspan)
{
maxspan = yspan;
Copy(dia1,ymin);
Copy(dia2,ymax);
}
if (zspan>maxspan)
{
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;
/* 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 */
mRadius2 = dx*dx + dy*dy + dz*dz;
mRadius = float(sqrt(mRadius2));
/* SECOND PASS: increment current sphere */
if ( 1 )
{
for (unsigned i=0; i<vcount; i++)
{
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];
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));
/* 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;
/* calc center of new sphere */
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;
Set(center,cx,cy,cz);
}
}
}
return mRadius;
}
double computeSphereVolume(double r)
{
return (4.0f*3.141592654f*r*r*r)/3.0f; // 4/3 PI R cubed
}
};

70
convex_decomposition/ConvexDecomposition/ConvexDecomposition/fitsphere.h Normal file
View File

@ -0,0 +1,70 @@
#ifndef FIT_SPHERE_H
#define FIT_SPHERE_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
double computeBoundingSphere(unsigned int vcount,const double *points,double *center);
float computeBoundingSphere(unsigned int vcount,const float *points,float *center);
double computeSphereVolume(double r);
};
#endif

463
convex_decomposition/ConvexDecomposition/ConvexDecomposition/float_math.cpp Normal file
View File

@ -0,0 +1,463 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include "float_math.h"
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// a set of routines that let you do common 3d math
// operations without any vector, matrix, or quaternion
// classes or templates.
//
// a vector (or point) is a 'double *' to 3 doubleing point numbers.
// a matrix is a 'double *' to an array of 16 doubleing point numbers representing a 4x4 transformation matrix compatible with D3D or OGL
// a quaternion is a 'double *' to 4 doubles representing a quaternion x,y,z,w
//
//
//
// Please email bug fixes or improvements to John W. Ratcliff at mailto:jratcliff@infiniplex.net
//
// If you find this source code useful donate a couple of bucks to my kid's fund raising website at
// www.amillionpixels.us
//
// More snippets at: www.codesuppository.com
//
namespace ConvexDecomposition
{
void fm_inverseRT(const double *matrix,const double *pos,double *t) // inverse rotate translate the point.
{
double _x = pos[0] - matrix[3*4+0];
double _y = pos[1] - matrix[3*4+1];
double _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);
}
void fm_identity(double *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[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+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(double ax,double ay,double az,double *matrix) // convert euler (in radians) to a dest 4x4 matrix (translation set to zero)
{
double quat[4];
fm_eulerToQuat(ax,ay,az,quat);
fm_quatToMatrix(quat,matrix);
}
void fm_getAABB(unsigned int vcount,const double *points,unsigned int pstride,double *bmin,double *bmax)
{
const unsigned char *source = (const unsigned char *) points;
bmin[0] = points[0];
bmin[1] = points[1];
bmin[2] = points[2];
bmax[0] = points[0];
bmax[1] = points[1];
bmax[2] = points[2];
for (unsigned int i=1; i<vcount; i++)
{
source+=pstride;
const double *p = (const double *) 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] > 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(double roll,double pitch,double yaw,double *quat) // convert euler angles to quaternion.
{
roll *= 0.5f;
pitch *= 0.5f;
yaw *= 0.5f;
double cr = cos(roll);
double cp = cos(pitch);
double cy = cos(yaw);
double sr = sin(roll);
double sp = sin(pitch);
double sy = sin(yaw);
double cpcy = cp * cy;
double spsy = sp * sy;
double spcy = sp * cy;
double 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;
}
void fm_quatToMatrix(const double *quat,double *matrix) // convert quaterinion rotation to matrix, zeros out the translation component.
{
double xx = quat[0]*quat[0];
double yy = quat[1]*quat[1];
double zz = quat[2]*quat[2];
double xy = quat[0]*quat[1];
double xz = quat[0]*quat[2];
double yz = quat[1]*quat[2];
double wx = quat[3]*quat[0];
double wy = quat[3]*quat[1];
double 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+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] = 0.0f;
matrix[3*4+1] = 0.0f;
matrix[3*4+2] = 0.0f;
matrix[0*4+3] = 0.0f;
matrix[1*4+3] = 0.0f;
matrix[2*4+3] = 0.0f;
matrix[3*4+3] =(double) 1.0f;
}
void fm_quatRotate(const double *quat,const double *v,double *r) // rotate a vector directly by a quaternion.
{
double 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]);
}
void fm_getTranslation(const double *matrix,double *t)
{
t[0] = matrix[3*4+0];
t[1] = matrix[3*4+1];
t[2] = matrix[3*4+2];
}
void fm_matrixToQuat(const double *matrix,double *quat) // convert the 3x3 portion of a 4x4 matrix into a quaterion as x,y,z,w
{
double tr = matrix[0*4+0] + matrix[1*4+1] + matrix[2*4+2];
// check the diagonal
if (tr > 0.0f )
{
double s = (double) 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;
}
else
{
// diagonal is negative
int nxt[3] = {1, 2, 0};
double 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;
int j = nxt[i];
int k = nxt[j];
double s = sqrt ( ((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;
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];
}
}
double fm_sphereVolume(double 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;
}
double fm_cylinderVolume(double radius,double h)
{
return FM_PI * radius * radius *h;
}
double fm_capsuleVolume(double radius,double h)
{
double volume = fm_sphereVolume(radius); // volume of the sphere portion.
double ch = h-radius*2; // this is the cylinder length
if ( ch > 0 )
{
volume+=fm_cylinderVolume(radius,ch);
}
return volume;
}
void fm_transform(const double *matrix,const double *v,double *t) // rotate and translate this point
{
t[0] = (matrix[0*4+0] * v[0]) + (matrix[1*4+0] * v[1]) + (matrix[2*4+0] * v[2]) + matrix[3*4+0];
t[1] = (matrix[0*4+1] * v[0]) + (matrix[1*4+1] * v[1]) + (matrix[2*4+1] * v[2]) + matrix[3*4+1];
t[2] = (matrix[0*4+2] * v[0]) + (matrix[1*4+2] * v[1]) + (matrix[2*4+2] * v[2]) + matrix[3*4+2];
}
void fm_rotate(const double *matrix,const double *v,double *t) // rotate and translate this point
{
t[0] = (matrix[0*4+0] * v[0]) + (matrix[1*4+0] * v[1]) + (matrix[2*4+0] * v[2]);
t[1] = (matrix[0*4+1] * v[0]) + (matrix[1*4+1] * v[1]) + (matrix[2*4+1] * v[2]);
t[2] = (matrix[0*4+2] * v[0]) + (matrix[1*4+2] * v[1]) + (matrix[2*4+2] * v[2]);
}
double fm_distance(const double *p1,const double *p2)
{
double dx = p1[0] - p2[0];
double dy = p1[1] - p2[1];
double dz = p1[2] - p2[2];
return sqrt( dx*dx + dy*dy + dz *dz );
}
double fm_distanceSquared(const double *p1,const double *p2)
{
double dx = p1[0] - p2[0];
double dy = p1[1] - p2[1];
double dz = p1[2] - p2[2];
return dx*dx + dy*dy + dz *dz;
}
double fm_computePlane(const double *A,const double *B,const double *C,double *n) // returns D
{
double vx = (B[0] - C[0]);
double vy = (B[1] - C[1]);
double vz = (B[2] - C[2]);
double wx = (A[0] - B[0]);
double wy = (A[1] - B[1]);
double wz = (A[2] - B[2]);
double vw_x = vy * wz - vz * wy;
double vw_y = vz * wx - vx * wz;
double vw_z = vx * wy - vy * wx;
double mag = sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
if ( mag < 0.000001f )
{
mag = 0;
}
else
{
mag = 1.0f/mag;
}
double x = vw_x * mag;
double y = vw_y * mag;
double z = vw_z * mag;
double D = 0.0f - ((x*A[0])+(y*A[1])+(z*A[2]));
n[0] = x;
n[1] = y;
n[2] = z;
return D;
}
double fm_distToPlane(const double *plane,const double *p) // computes the distance of this point from the plane.
{
return p[0]*plane[0]+p[1]*plane[1]+p[2]*plane[2]+plane[3];
}
double fm_dot(const double *p1,const double *p2)
{
return p1[0]*p2[0]+p1[1]*p2[2]+p1[2]*p2[2];
}
void fm_cross(double *cross,const double *a,const double *b)
{
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];
}
void fm_computeNormalVector(double *n,const double *p1,const double *p2)
{
n[0] = p2[0] - p1[0];
n[1] = p2[1] - p1[1];
n[2] = p2[2] - p1[2];
fm_normalize(n);
}
bool fm_computeWindingOrder(const double *p1,const double *p2,const double *p3) // returns true if the triangle is clockwise.
{
bool ret = false;
double v1[3];
double v2[3];
fm_computeNormalVector(v1,p1,p2); // p2-p1 (as vector) and then normalized
fm_computeNormalVector(v2,p1,p3); // p3-p1 (as vector) and then normalized
double cross[3];
fm_cross(cross, v1, v2 );
double ref[3] = { 1, 0, 0 };
double d = fm_dot( cross, ref );
if ( d <= 0 )
ret = false;
else
ret = true;
return ret;
}
void fm_normalize(double *n) // normalize this vector
{
double dist = n[0]*n[0] + n[1]*n[1] + n[2]*n[2];
double mag = 0;
if ( dist > 0.0000001f )
mag = 1.0f / sqrt(dist);
n[0]*=mag;
n[1]*=mag;
n[2]*=mag;
}
}; // end of namespace

112
convex_decomposition/ConvexDecomposition/ConvexDecomposition/float_math.h Normal file
View File

@ -0,0 +1,112 @@
#ifndef FLOAT_MATH_H
#define FLOAT_MATH_H
namespace ConvexDecomposition
{
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// a set of routines that let you do common 3d math
// operations without any vector, matrix, or quaternion
// classes or templates.
//
// a vector (or point) is a 'double *' to 3 doubleing point numbers.
// a matrix is a 'double *' to an array of 16 doubleing point numbers representing a 4x4 transformation matrix compatible with D3D or OGL
// a quaternion is a 'double *' to 4 doubles representing a quaternion x,y,z,w
//
//
//
// Please email bug fixes or improvements to John W. Ratcliff at mailto:jratcliff@infiniplex.net
//
// If you find this source code useful donate a couple of bucks to my kid's fund raising website at
// www.amillionpixels.us
//
// More snippets at: www.codesuppository.com
//
const double FM_PI = 3.141592654f;
const double FM_DEG_TO_RAD = ((2.0f * FM_PI) / 360.0f);
const double FM_RAD_TO_DEG = (360.0f / (2.0f * FM_PI));
void fm_identity(double *matrix); // set 4x4 matrix to identity.
void fm_inverseRT(const double *matrix,const double *pos,double *t); // inverse rotate translate the point.
void fm_transform(const double *matrix,const double *pos,double *t); // rotate and translate this point.
void fm_rotate(const double *matrix,const double *pos,double *t); // only rotate the point by a 4x4 matrix, don't translate.
void fm_eulerMatrix(double ax,double ay,double az,double *matrix); // convert euler (in radians) to a dest 4x4 matrix (translation set to zero)
void fm_getAABB(unsigned int vcount,const double *points,unsigned int pstride,double *bmin,double *bmax);
void fm_eulerToQuat(double roll,double pitch,double yaw,double *quat); // convert euler angles to quaternion.
void fm_quatToMatrix(const double *quat,double *matrix); // convert quaterinion rotation to matrix, translation set to zero.
void fm_quatRotate(const double *quat,const double *v,double *r); // rotate a vector directly by a quaternion.
void fm_getTranslation(const double *matrix,double *t);
void fm_matrixToQuat(const double *matrix,double *quat); // convert the 3x3 portion of a 4x4 matrix into a quaterion as x,y,z,w
double fm_sphereVolume(double radius); // return's the volume of a sphere of this radius (4/3 PI * R cubed )
double fm_cylinderVolume(double radius,double h);
double fm_capsuleVolume(double radius,double h);
double fm_distance(const double *p1,const double *p2);
double fm_distanceSquared(const double *p1,const double *p2);
double fm_computePlane(const double *p1,const double *p2,const double *p3,double *n); // return D
double fm_distToPlane(const double *plane,const double *pos); // computes the distance of this point from the plane.
double fm_dot(const double *p1,const double *p2);
void fm_cross(double *cross,const double *a,const double *b);
void fm_computeNormalVector(double *n,const double *p1,const double *p2); // as P2-P1 normalized.
bool fm_computeWindingOrder(const double *p1,const double *p2,const double *p3); // returns true if the triangle is clockwise.
void fm_normalize(double *n); // normalize this vector
}; // end of nsamepace
#endif

251
convex_decomposition/ConvexDecomposition/ConvexDecomposition/meshvolume.cpp Normal file
View File

@ -0,0 +1,251 @@
#include "meshvolume.h"
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
inline double det(const double *p1,const double *p2,const double *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];
}
double computeMeshVolume(const double *vertices,unsigned int tcount,const unsigned int *indices)
{
double volume = 0;
const double *p0 = vertices;
for (unsigned int i=0; i<tcount; i++,indices+=3)
{
const double *p1 = &vertices[ indices[0]*3 ];
const double *p2 = &vertices[ indices[1]*3 ];
const double *p3 = &vertices[ indices[2]*3 ];
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;
return volume;
}
inline void CrossProduct(const double *a,const double *b,double *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];
}
inline double DotProduct(const double *a,const double *b)
{
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}
inline double tetVolume(const double *p0,const double *p1,const double *p2,const double *p3)
{
double a[3];
double b[3];
double c[3];
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];
double cross[3];
CrossProduct( b, c, cross );
double volume = DotProduct( a, cross );
if ( volume < 0 )
return -volume;
return volume;
}
inline double det(const double *p0,const double *p1,const double *p2,const double *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];
}
double computeMeshVolume2(const double *vertices,unsigned int tcount,const unsigned int *indices)
{
double volume = 0;
const double *p0 = vertices;
for (unsigned int i=0; i<tcount; i++,indices+=3)
{
const double *p1 = &vertices[ indices[0]*3 ];
const double *p2 = &vertices[ indices[1]*3 ];
const double *p3 = &vertices[ indices[2]*3 ];
volume+=tetVolume(p0,p1,p2,p3); // compute the volume of the tetrahdren relative to the root vertice
}
return volume * (1.0f / 6.0f );
}
//** Float versions
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];
}
float computeMeshVolume(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)
{
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*=(1.0f/6.0f);
if ( volume < 0 )
volume*=-1;
return volume;
}
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];
}
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)
{
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];
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];
float cross[3];
CrossProduct( b, c, cross );
float volume = DotProduct( a, cross );
if ( volume < 0 )
return -volume;
return volume;
}
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];
}
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)
{
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
}
return volume * (1.0f / 6.0f );
}
};

70
convex_decomposition/ConvexDecomposition/ConvexDecomposition/meshvolume.h Normal file
View File

@ -0,0 +1,70 @@
#ifndef MESH_VOLUME_H
#define MESH_VOLUME_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
double computeMeshVolume(const double *vertices,unsigned int tcount,const unsigned int *indices);
double computeMeshVolume2(const double *vertices,unsigned int tcount,const unsigned int *indices);
float computeMeshVolume(const float *vertices,unsigned int tcount,const unsigned int *indices);
};
#endif

314
convex_decomposition/ConvexDecomposition/ConvexDecomposition/planetri.cpp Normal file
View File

@ -0,0 +1,314 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "planetri.h"
namespace ConvexDecomposition
{
static inline double DistToPt(const double *p,const double *plane)
{
double x = p[0];
double y = p[1];
double z = p[2];
double d = x*plane[0] + y*plane[1] + z*plane[2] + plane[3];
return d;
}
static PlaneTriResult getSidePlane(const double *p,const double *plane,double epsilon)
{
double d = DistToPt(p,plane);
if ( (d+epsilon) > 0 )
return PTR_FRONT; // it is 'in front' within the provided epsilon value.
return PTR_BACK;
}
static void add(const double *p,double *dest,unsigned int tstride,unsigned int &pcount)
{
char *d = (char *) dest;
d = d + pcount*tstride;
dest = (double *) 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 double *p1,const double *p2,double *split,const double *plane)
{
double dp1 = DistToPt(p1,plane);
double dp2 = DistToPt(p2,plane);
double dir[3];
dir[0] = p2[0] - p1[0];
dir[1] = p2[1] - p1[1];
dir[2] = p2[2] - p1[2];
double dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2];
double dot2 = dp1 - plane[3];
double 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];
}
#define MAXPTS 256
class point
{
public:
void set(const double *p)
{
x = p[0];
y = p[1];
z = p[2];
}
double x;
double y;
double z;
};
class polygon
{
public:
polygon(void)
{
mVcount = 0;
}
polygon(const double *p1,const double *p2,const double *p3)
{
mVcount = 3;
mVertices[0].set(p1);
mVertices[1].set(p2);
mVertices[2].set(p3);
}
int NumVertices(void) const { return mVcount; };
const point& Vertex(int index)
{
if ( index < 0 ) index+=mVcount;
return mVertices[index];
};
void set(const point *pts,int count)
{
for (int i=0; i<count; i++)
{
mVertices[i] = pts[i];
}
mVcount = count;
}
int mVcount;
point mVertices[MAXPTS];
};
class plane
{
public:
plane(const double *p)
{
normal.x = p[0];
normal.y = p[1];
normal.z = p[2];
D = p[3];
}
double Classify_Point(const point &p)
{
return p.x*normal.x + p.y*normal.y + p.z*normal.z + D;
}
point normal;
double D;
};
void Split_Polygon(polygon *poly, plane *part, polygon &front, polygon &back)
{
int count = poly->NumVertices ();
int out_c = 0, in_c = 0;
point ptA, ptB,outpts[MAXPTS],inpts[MAXPTS];
double sideA, sideB;
ptA = poly->Vertex (count - 1);
sideA = part->Classify_Point (ptA);
for (int i = -1; ++i < count;)
{
ptB = poly->Vertex(i);
sideB = part->Classify_Point(ptB);
if (sideB > 0)
{
if (sideA < 0)
{
point v;
intersect(&ptB.x, &ptA.x, &v.x, &part->normal.x );
outpts[out_c++] = inpts[in_c++] = v;
}
outpts[out_c++] = ptB;
}
else if (sideB < 0)
{
if (sideA > 0)
{
point v;
intersect(&ptB.x, &ptA.x, &v.x, &part->normal.x );
outpts[out_c++] = inpts[in_c++] = v;
}
inpts[in_c++] = ptB;
}
else
outpts[out_c++] = inpts[in_c++] = ptB;
ptA = ptB;
sideA = sideB;
}
front.set(&outpts[0], out_c);
back.set(&inpts[0], in_c);
}
PlaneTriResult planeTriIntersection(const double *_plane, // the plane equation in Ax+By+Cz+D format
const double *triangle, // the source triangle.
unsigned int tstride, // stride in bytes of the input and output *vertices*
double epsilon, // the co-planer epsilon value.
double *front, // the triangle in front of the
unsigned int &fcount, // number of vertices in the 'front' triangle
double *back, // the triangle in back of the plane
unsigned int &bcount) // the number of vertices in the 'back' triangle.
{
fcount = 0;
bcount = 0;
const char *tsource = (const char *) triangle;
// get the three vertices of the triangle.
const double *p1 = (const double *) (tsource);
const double *p2 = (const double *) (tsource+tstride);
const double *p3 = (const double *) (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);
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 'back' 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
}
polygon pi(p1,p2,p3);
polygon pfront,pback;
plane part(_plane);
Split_Polygon(&pi,&part,pfront,pback);
for (int i=0; i<pfront.mVcount; i++)
{
add( &pfront.mVertices[i].x, front, tstride, fcount );
}
for (int i=0; i<pback.mVcount; i++)
{
add( &pback.mVertices[i].x, back, tstride, bcount );
}
PlaneTriResult ret = PTR_SPLIT;
if ( fcount == 0 && bcount )
ret = PTR_BACK;
if ( bcount == 0 && fcount )
ret = PTR_FRONT;
return ret;
}
};

82
convex_decomposition/ConvexDecomposition/ConvexDecomposition/planetri.h Normal file
View File

@ -0,0 +1,82 @@
#ifndef PLANE_TRI_H
#define PLANE_TRI_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
enum PlaneTriResult
{
PTR_FRONT,
PTR_BACK,
PTR_SPLIT,
};
PlaneTriResult planeTriIntersection(const double *plane, // the plane equation in Ax+By+Cz+D format
const double *triangle, // the source position triangle.
unsigned int tstride, // stride in bytes between vertices of the triangle.
double epsilon, // the co-planer epsilon value.
double *front, // the triangle in front of the
unsigned int &fcount, // number of vertices in the 'front' triangle.
double *back, // the triangle in back of the plane
unsigned int &bcount); // the number of vertices in the 'back' triangle.
};
#endif

160
convex_decomposition/ConvexDecomposition/ConvexDecomposition/raytri.cpp Normal file
View File

@ -0,0 +1,160 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "raytri.h"
namespace ConvexDecomposition
{
/* a = b - c */
#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 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 double *p,const double *d,const double *v0,const double *v1,const double *v2,double &t)
{
double e1[3],e2[3],h[3],s[3],q[3];
double a,f,u,v;
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);
f = 1/a;
vector(s,p,v0);
u = f * (innerProduct(s,h));
if (u < 0.0 || u > 1.0)
return(false);
crossProduct(q,s,e1);
v = f * innerProduct(d,q);
if (v < 0.0 || u + v > 1.0)
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);
}
bool lineIntersectsTriangle(const double *rayStart,const double *rayEnd,const double *p1,const double *p2,const double *p3,double *sect)
{
double dir[3];
dir[0] = rayEnd[0] - rayStart[0];
dir[1] = rayEnd[1] - rayStart[1];
dir[2] = rayEnd[2] - rayStart[2];
double d = sqrt(dir[0]*dir[0] + dir[1]*dir[1] + dir[2]*dir[2]);
double r = 1.0f / d;
dir[0]*=r;
dir[1]*=r;
dir[2]*=r;
double t;
bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t );
if ( ret )
{
if ( t > d )
{
sect[0] = rayStart[0] + dir[0]*t;
sect[1] = rayStart[1] + dir[1]*t;
sect[2] = rayStart[2] + dir[2]*t;
}
else
{
ret = false;
}
}
return ret;
}
}; // end of namespace

69
convex_decomposition/ConvexDecomposition/ConvexDecomposition/raytri.h Normal file
View File

@ -0,0 +1,69 @@
#ifndef RAY_TRI_H
#define RAY_TRI_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
// returns true if the ray intersects the triangle.
bool lineIntersectsTriangle(const double *rayStart,const double *rayEnd,const double *p1,const double *p2,const double *p3,double *sect);
bool rayIntersectsTriangle(const double *p,const double *d,const double *v0,const double *v1,const double *v2,double &t);
};
#endif

339
convex_decomposition/ConvexDecomposition/ConvexDecomposition/splitplane.cpp Normal file
View File

@ -0,0 +1,339 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <float.h>
#include <math.h>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "splitplane.h"
#include "ConvexDecomposition.h"
#include "cd_vector.h"
#include "cd_hull.h"
#include "cd_wavefront.h"
#include "bestfit.h"
#include "planetri.h"
#include "vlookup.h"
#include "meshvolume.h"
#include "bestfitobb.h"
#include "float_math.h"
namespace ConvexDecomposition
{
static void computePlane(const double *A,const double *B,const double *C,double *plane)
{
double vx = (B[0] - C[0]);
double vy = (B[1] - C[1]);
double vz = (B[2] - C[2]);
double wx = (A[0] - B[0]);
double wy = (A[1] - B[1]);
double wz = (A[2] - B[2]);
double vw_x = vy * wz - vz * wy;
double vw_y = vz * wx - vx * wz;
double vw_z = vx * wy - vy * wx;
double mag = sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
if ( mag < 0.000001f )
{
mag = 0;
}
else
{
mag = 1.0f/mag;
}
double x = vw_x * mag;
double y = vw_y * mag;
double z = vw_z * mag;
double D = 0.0f - ((x*A[0])+(y*A[1])+(z*A[2]));
plane[0] = x;
plane[1] = y;
plane[2] = z;
plane[3] = D;
}
class Rect3d
{
public:
Rect3d(void) { };
Rect3d(const double *bmin,const double *bmax)
{
mMin[0] = bmin[0];
mMin[1] = bmin[1];
mMin[2] = bmin[2];
mMax[0] = bmax[0];
mMax[1] = bmax[1];
mMax[2] = bmax[2];
}
void SetMin(const double *bmin)
{
mMin[0] = bmin[0];
mMin[1] = bmin[1];
mMin[2] = bmin[2];
}
void SetMax(const double *bmax)
{
mMax[0] = bmax[0];
mMax[1] = bmax[1];
mMax[2] = bmax[2];
}
void SetMin(double x,double y,double z)
{
mMin[0] = x;
mMin[1] = y;
mMin[2] = z;
}
void SetMax(double x,double y,double z)
{
mMax[0] = x;
mMax[1] = y;
mMax[2] = z;
}
double mMin[3];
double mMax[3];
};
void splitRect(unsigned int axis,
const Rect3d &source,
Rect3d &b1,
Rect3d &b2,
const double *midpoint)
{
switch ( axis )
{
case 0:
b1.SetMin(source.mMin);
b1.SetMax( midpoint[0], source.mMax[1], source.mMax[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] );
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] );
b2.SetMin( source.mMin[0], source.mMin[1], midpoint[2] );
b2.SetMax(source.mMax);
break;
}
}
bool computeSplitPlane(unsigned int vcount,
const double *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
double *plane)
{
bool cret = false;
double sides[3];
double matrix[16];
computeBestFitOBB( vcount, vertices, sizeof(double)*3, sides, matrix );
double bmax[3];
double bmin[3];
bmax[0] = sides[0]*0.5f;
bmax[1] = sides[1]*0.5f;
bmax[2] = sides[2]*0.5f;
bmin[0] = -bmax[0];
bmin[1] = -bmax[1];
bmin[2] = -bmax[2];
double dx = sides[0];
double dy = sides[1];
double dz = sides[2];
double laxis = dx;
unsigned int axis = 0;
if ( dy > dx )
{
axis = 1;
laxis = dy;
}
if ( dz > dx && dz > dy )
{
axis = 2;
laxis = dz;
}
double p1[3];
double p2[3];
double 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;
Rect3d b(bmin,bmax);
Rect3d b1,b2;
splitRect(axis,b,b1,b2,p1);
// 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];
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];
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];
if ( dx > dy )
{
p3[0] = bmax[0];
p3[1] = bmin[1];
}
else
{
p3[0] = bmin[0];
p3[1] = bmax[1];
}
break;
}
double tp1[3];
double tp2[3];
double tp3[3];
fm_transform(matrix,p1,tp1);
fm_transform(matrix,p2,tp2);
fm_transform(matrix,p3,tp3);
// callback->ConvexDebugTri(p1,p2,p3,0xFF0000);
computePlane(tp1,tp2,tp3,plane);
return true;
}
};

76
convex_decomposition/ConvexDecomposition/ConvexDecomposition/splitplane.h Normal file
View File

@ -0,0 +1,76 @@
#ifndef SPLIT_PLANE_H
#define SPLIT_PLANE_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
class ConvexDecompInterface;
bool computeSplitPlane(unsigned int vcount,
const double *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
double *plane);
};
#endif

410
convex_decomposition/ConvexDecomposition/ConvexDecomposition/triangulate.cpp Normal file
View File

@ -0,0 +1,410 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <vector> // Include STL vector class.
#include "triangulate.h"
namespace ConvexDecomposition
{
class Vec2d
{
public:
Vec2d(const double *v)
{
mX = v[0];
mY = v[1];
}
Vec2d(double x,double y)
{
Set(x,y);
};
double GetX(void) const { return mX; };
double GetY(void) const { return mY; };
void Set(double x,double y)
{
mX = x;
mY = y;
};
private:
double mX;
double mY;
};// Typedef an STL vector of vertices which are used to represent
// a polygon/contour and a series of triangles.
typedef std::vector< Vec2d > Vec2dVector;
static bool Process(const Vec2dVector &contour,Vec2dVector &result); // compute area of a contour/polygon
static double Area(const Vec2dVector &contour); // decide if point Px/Py is inside triangle defined by (Ax,Ay) (Bx,By) (Cx,Cy)
static bool InsideTriangle(double Ax, double Ay,double Bx, double By,double Cx, double Cy,double Px, double Py);
static bool Snip(const Vec2dVector &contour,int u,int v,int w,int n,int *V);
static const double EPSILON=0.0000000001f;
double Area(const Vec2dVector &contour)
{
int n = contour.size();
double A=0.0f;
for(int p=n-1,q=0; q<n; p=q++)
{
A+= contour[p].GetX()*contour[q].GetY() - contour[q].GetX()*contour[p].GetY();
}
return A*0.5f;
}
/*
InsideTriangle decides if a point P is Inside of the triangle
defined by A, B, C.
*/
bool InsideTriangle(double Ax, double Ay,double Bx, double By,double Cx, double Cy,double Px, double Py)
{
double ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
double cCROSSap, bCROSScp, aCROSSbp; ax = Cx - Bx; ay = Cy - By;
bx = Ax - Cx; by = Ay - Cy;
cx = Bx - Ax; cy = By - Ay;
apx= Px - Ax; apy= Py - Ay;
bpx= Px - Bx; bpy= Py - By;
cpx= Px - Cx; cpy= Py - Cy; aCROSSbp = ax*bpy - ay*bpx;
cCROSSap = cx*apy - cy*apx;
bCROSScp = bx*cpy - by*cpx; return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
};
bool Snip(const Vec2dVector &contour,int u,int v,int w,int n,int *V)
{
int p;
double Ax, Ay, Bx, By, Cx, Cy, Px, Py;
Ax = contour[V[u]].GetX();
Ay = contour[V[u]].GetY();
Bx = contour[V[v]].GetX();
By = contour[V[v]].GetY();
Cx = contour[V[w]].GetX();
Cy = contour[V[w]].GetY();
if ( EPSILON > (((Bx-Ax)*(Cy-Ay)) - ((By-Ay)*(Cx-Ax))) ) return false; for (p=0;p<n;p++)
{
if( (p == u) || (p == v) || (p == w) ) continue;
Px = contour[V[p]].GetX();
Py = contour[V[p]].GetY();
if (InsideTriangle(Ax,Ay,Bx,By,Cx,Cy,Px,Py)) return false;
} return true;
}
bool Process(const Vec2dVector &contour,Vec2dVector &result)
{
/* allocate and initialize list of Vertices in polygon */
int n = contour.size();
if ( n < 3 ) return false; int *V = new int[n]; /* we want a counter-clockwise polygon in V */ if ( 0.0f < Area(contour) )
for (int v=0; v<n; v++) V[v] = v;
else
for(int v=0; v<n; v++) V[v] = (n-1)-v; int nv = n; /* remove nv-2 Vertices, creating 1 triangle every time */
int count = 2*nv; /* error detection */
for(int m=0, v=nv-1; nv>2; )
{
/* if we loop, it is probably a non-simple polygon */
if (0 >= (count--))
{
//** Triangulate: ERROR - probable bad polygon!
return false;
} /* three consecutive vertices in current polygon, <u,v,w> */
int u = v ;
if (nv <= u) u = 0; /* previous */
v = u+1; if (nv <= v) v = 0; /* new v */
int w = v+1;
if (nv <= w) w = 0; /* next */
if ( Snip(contour,u,v,w,nv,V) )
{
int a,b,c,s,t; /* true names of the vertices */
a = V[u];
b = V[v];
c = V[w]; /* output Triangle */
result.push_back( contour[a] );
result.push_back( contour[b] );
result.push_back( contour[c] );
m++; /* remove v from remaining polygon */
for(s=v,t=v+1;t<nv;s++,t++) V[s] = V[t]; nv--; /* resest error detection counter */
count = 2*nv;
}
}
delete V;
return true;
}
unsigned int triangulate3d(unsigned int pcount, // number of points in the polygon
const double *vertices, // array of 3d vertices.
double *triangles, // memory to store output triangles
unsigned int maxTri, // maximum triangles we are allowed to output.
const double *plane)
{
unsigned int ret = 0;
FILE *fph = fopen("debug.obj", "wb");
if ( fph )
{
fprintf(fph,"v 10 10 0\r\n");
for (unsigned int i=0; i<pcount; i++)
{
fprintf(fph,"v %f %f %f\r\n", vertices[i*3+0], vertices[i*3+1], vertices[i*3+2]);
}
for (unsigned int i=0; i<pcount; i++)
{
unsigned int next = i+1;
if ( next == pcount ) next = 0;
fprintf(fph,"f %d %d %d\r\n", i+2, 1, next+2 );
}
fclose(fph);
}
if ( pcount >= 3 )
{
double normal[3];
normal[0] = plane[0];
normal[1] = plane[1];
normal[2] = plane[2];
double D = plane[3];
unsigned int i0 = 0;
unsigned int i1 = 1;
unsigned int i2 = 2;
unsigned int axis = 0;
// find the dominant axis.
double dx = fabs(normal[0]);
double dy = fabs(normal[1]);
double dz = fabs(normal[2]);
if ( dx > dy && dx > dz )
{
axis = 0;
i0 = 1;
i1 = 2;
i2 = 0;
}
else if ( dy > dx && dy > dz )
{
i0 = 0;
i1 = 2;
i2 = 1;
axis = 1;
}
else if ( dz > dx && dz > dy )
{
i0 = 0;
i1 = 1;
i2 = 2;
axis = 2;
}
double *ptemp = new double[pcount*2];
double *ptri = new double[maxTri*2*3];
const double *source = vertices;
double *dest = ptemp;
for (unsigned int i=0; i<pcount; i++)
{
dest[0] = source[i0];
dest[1] = source[i1];
dest+=2;
source+=3;
}
ret = triangulate2d(pcount, ptemp, ptri, maxTri );
// ok..now we have to copy it back and project the 3d component.
if ( ret )
{
const double *source = ptri;
double *dest = triangles;
double inverseZ = -1.0f / normal[i2];
for (unsigned int i=0; i<ret*3; i++)
{
dest[i0] = source[0];
dest[i1] = source[1];
dest[i2] = (normal[i0]*source[0] + normal[i1]*source[1] + D ) * inverseZ; // solve for projected component
dest+=3;
source+=2;
}
if ( 1 )
{
FILE *fph = fopen("test.obj","wb");
const double *source = triangles;
for (unsigned int i=0; i<ret*3; i++)
{
fprintf(fph,"v %f %f %f\r\n", source[0], source[1], source[2] );
source+=3;
}
int index = 1;
for (unsigned int i=0; i<ret; i++)
{
fprintf(fph,"f %d %d %d\r\n", index, index+1, index+2 );
index+=3;
}
fclose(fph);
}
}
delete ptri;
delete ptemp;
}
return ret;
}
unsigned int triangulate3d(unsigned int pcount, // number of points in the polygon
const unsigned int *indices, // polygon points using indices
const double *vertices, // base address for array indexing
double *triangles, // buffer to store output 3d triangles.
unsigned int maxTri, // maximum triangles we can output.
const double *plane)
{
unsigned int ret = 0;
if ( pcount )
{
// copy the indexed polygon out as a flat array of vertices.
double *ptemp = new double[pcount*3];
double *dest = ptemp;
for (unsigned int i=0; i<pcount; i++)
{
unsigned int index = indices[i];
const double *source = &vertices[index*3];
*dest++ = *source++;
*dest++ = *source++;
*dest++ = *source++;
}
ret = triangulate3d(pcount,ptemp,triangles,maxTri,plane);
delete ptemp;
}
return ret;
}
unsigned int triangulate2d(unsigned int pcount, // number of points in the polygon
const double *vertices, // address of input points (2d)
double *triangles, // destination buffer for output triangles.
unsigned int maxTri) // maximum number of triangles we can store.
{
unsigned int ret = 0;
const double *source = vertices;
Vec2dVector vlist;
for (unsigned int i=0; i<pcount; i++)
{
Vec2d v(source);
vlist.push_back(v);
source+=2;
}
Vec2dVector result;
bool ok = Process(vlist,result);
if ( ok )
{
ret = result.size()/3;
if ( ret < maxTri )
{
double *dest = triangles;
for (unsigned int i=0; i<ret*3; i++)
{
dest[0] = result[i].GetX();
dest[1] = result[i].GetY();
dest+=2;
}
}
else
{
ret = 0;
}
}
return ret;
}
}; // end of namespace

85
convex_decomposition/ConvexDecomposition/ConvexDecomposition/triangulate.h Normal file
View File

@ -0,0 +1,85 @@
#ifndef TRIANGULATE_H
#define TRIANGULATE_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
// all 3d triangles should be co-planer. Doesn't bother to check, you should have done that yourself to begin with!
unsigned int triangulate3d(unsigned int pcount, // number of points in the polygon
const double *vertices, // array of 3d vertices.
double *triangles, // memory to store output triangles
unsigned int maxTri,
const double *plane); // maximum triangles we are allowed to output.
unsigned int triangulate3d(unsigned int pcount, // number of points in the polygon
const unsigned int *indices, // polygon points using indices
const double *vertices, // base address for array indexing
double *triangles, // buffer to store output 3d triangles.
unsigned int maxTri,
const double *plane); // maximum triangles we can output.
unsigned int triangulate2d(unsigned int pcount, // number of points in the polygon
const double *vertices, // address of input points (2d)
double *triangles, // destination buffer for output triangles.
unsigned int maxTri); // maximum number of triangles we can store.
};
#endif

340
convex_decomposition/ConvexDecomposition/ConvexDecomposition/vlookup.cpp Normal file
View File

@ -0,0 +1,340 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#pragma warning(disable:4786)
#include <vector>
#include <map>
#include <set>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// CodeSnippet provided by John W. Ratcliff
// on March 23, 2006.
//
// mailto: jratcliff@infiniplex.net
//
// Personal website: http://jratcliffscarab.blogspot.com
// Coding Website: http://codesuppository.blogspot.com
// FundRaising Blog: http://amillionpixels.blogspot.com
// Fundraising site: http://www.amillionpixels.us
// New Temple Site: http://newtemple.blogspot.com
//
// This snippet shows how to 'hide' the complexity of
// the STL by wrapping some useful piece of functionality
// around a handful of discrete API calls.
//
// This API allows you to create an indexed triangle list
// from a collection of raw input triangles. Internally
// it uses an STL set to build the lookup table very rapidly.
//
// Here is how you would use it to build an indexed triangle
// list from a raw list of triangles.
//
// (1) create a 'VertexLookup' interface by calling
//
// VertexLook vl = Vl_createVertexLookup();
//
// (2) For each vertice in each triangle call:
//
// unsigned int i1 = Vl_getIndex(vl,p1);
// unsigned int i2 = Vl_getIndex(vl,p2);
// unsigned int i3 = Vl_getIndex(vl,p3);
//
// save the 3 indices into your triangle list array.
//
// (3) Get the vertex array by calling:
//
// const double *vertices = Vl_getVertices(vl);
//
// (4) Get the number of vertices so you can copy them into
// your own buffer.
// unsigned int vcount = Vl_getVcount(vl);
//
// (5) Release the VertexLookup interface when you are done with it.
// Vl_releaseVertexLookup(vl);
//
// Teaches the following lessons:
//
// How to wrap the complexity of STL and C++ classes around a
// simple API interface.
//
// How to use an STL set and custom comparator operator for
// a complex data type.
//
// How to create a template class.
//
// How to achieve significant performance improvements by
// taking advantage of built in STL containers in just
// a few lines of code.
//
// You could easily modify this code to support other vertex
// formats with any number of interpolants.
#include "vlookup.h"
namespace ConvexDecomposition
{
class VertexPosition
{
public:
VertexPosition(void) { };
VertexPosition(const double *p)
{
mPos[0] = p[0];
mPos[1] = p[1];
mPos[2] = p[2];
};
void Set(int index,const double *pos)
{
const double * p = &pos[index*3];
mPos[0] = p[0];
mPos[1] = p[1];
mPos[2] = p[2];
};
double GetX(void) const { return mPos[0]; };
double GetY(void) const { return mPos[1]; };
double GetZ(void) const { return mPos[2]; };
double mPos[3];
};
template <typename Type> class VertexLess
{
public:
typedef std::vector< Type > VertexVector;
bool operator()(int v1,int v2) const;
static void SetSearch(const Type& match,VertexVector *list)
{
mFind = match;
mList = list;
};
private:
const Type& Get(int index) const
{
if ( index == -1 ) return mFind;
VertexVector &vlist = *mList;
return vlist[index];
}
static Type mFind; // vertice to locate.
static VertexVector *mList;
};
template <typename Type> class VertexPool
{
public:
typedef std::set<int, VertexLess<Type> > VertexSet;
typedef std::vector< Type > VertexVector;
int GetVertex(const Type& vtx)
{
VertexLess<Type>::SetSearch(vtx,&mVtxs);
typename VertexSet::iterator found;
found = mVertSet.find( -1 );
if ( found != mVertSet.end() )
{
return *found;
}
int idx = (int)mVtxs.size();
mVtxs.push_back( vtx );
mVertSet.insert( idx );
return idx;
};
const double * GetPos(int idx) const
{
return mVtxs[idx].mPos;
}
const Type& Get(int idx) const
{
return mVtxs[idx];
};
unsigned int GetSize(void) const
{
return mVtxs.size();
};
void Clear(int reservesize) // clear the vertice pool.
{
mVertSet.clear();
mVtxs.clear();
mVtxs.reserve(reservesize);
};
const VertexVector& GetVertexList(void) const { return mVtxs; };
void Set(const Type& vtx)
{
mVtxs.push_back(vtx);
}
unsigned int GetVertexCount(void) const
{
return mVtxs.size();
};
Type * GetBuffer(void)
{
return &mVtxs[0];
};
private:
VertexSet mVertSet; // ordered list.
VertexVector mVtxs; // set of vertices.
};
double tmpp[3] = {0,0,0};
template<> VertexPosition VertexLess<VertexPosition>::mFind = tmpp;
template<> std::vector<VertexPosition > *VertexLess<VertexPosition>::mList =0;
enum RDIFF
{
RD_EQUAL,
RD_LESS,
RD_GREATER
};
static RDIFF relativeDiff(const double *a,const double *b,double magnitude)
{
RDIFF ret = RD_EQUAL;
double m2 = magnitude*magnitude;
double dx = a[0]-b[0];
double dy = a[1]-b[1];
double dz = a[2]-b[2];
double d2 = (dx*dx)+(dy*dy)+(dz*dz);
if ( d2 > m2 )
{
if ( a[0] < b[0] ) ret = RD_LESS;
else if ( a[0] > b[0] ) ret = RD_GREATER;
else if ( a[1] < b[1] ) ret = RD_LESS;
else if ( a[1] > b[1] ) ret = RD_GREATER;
else if ( a[2] < b[2] ) ret = RD_LESS;
else if ( a[2] > b[2] ) ret = RD_GREATER;
}
return ret;
}
template<>
bool VertexLess<VertexPosition>::operator()(int v1,int v2) const
{
bool ret = false;
const VertexPosition& a = Get(v1);
const VertexPosition& b = Get(v2);
RDIFF d = relativeDiff(a.mPos,b.mPos,0.0001f);
if ( d == RD_LESS ) ret = true;
return ret;
};
VertexLookup Vl_createVertexLookup(void)
{
VertexLookup ret = new VertexPool< VertexPosition >;
return ret;
}
void Vl_releaseVertexLookup(VertexLookup vlook)
{
VertexPool< VertexPosition > *vp = (VertexPool< VertexPosition > *) vlook;
delete vp;
}
unsigned int Vl_getIndex(VertexLookup vlook,const double *pos) // get index.
{
VertexPool< VertexPosition > *vp = (VertexPool< VertexPosition > *) vlook;
VertexPosition p(pos);
return vp->GetVertex(p);
}
const double * Vl_getVertices(VertexLookup vlook)
{
VertexPool< VertexPosition > *vp = (VertexPool< VertexPosition > *) vlook;
return vp->GetPos(0);
}
unsigned int Vl_getVcount(VertexLookup vlook)
{
VertexPool< VertexPosition > *vp = (VertexPool< VertexPosition > *) vlook;
return vp->GetVertexCount();
}
}; // end of namespace

143
convex_decomposition/ConvexDecomposition/ConvexDecomposition/vlookup.h Normal file
View File

@ -0,0 +1,143 @@
#ifndef VLOOKUP_H
#define VLOOKUP_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// CodeSnippet provided by John W. Ratcliff
// on March 23, 2006.
//
// mailto: jratcliff@infiniplex.net
//
// Personal website: http://jratcliffscarab.blogspot.com
// Coding Website: http://codesuppository.blogspot.com
// FundRaising Blog: http://amillionpixels.blogspot.com
// Fundraising site: http://www.amillionpixels.us
// New Temple Site: http://newtemple.blogspot.com
//
// This snippet shows how to 'hide' the complexity of
// the STL by wrapping some useful piece of functionality
// around a handful of discrete API calls.
//
// This API allows you to create an indexed triangle list
// from a collection of raw input triangles. Internally
// it uses an STL set to build the lookup table very rapidly.
//
// Here is how you would use it to build an indexed triangle
// list from a raw list of triangles.
//
// (1) create a 'VertexLookup' interface by calling
//
// VertexLook vl = Vl_createVertexLookup();
//
// (2) For each vertice in each triangle call:
//
// unsigned int i1 = Vl_getIndex(vl,p1);
// unsigned int i2 = Vl_getIndex(vl,p2);
// unsigned int i3 = Vl_getIndex(vl,p3);
//
// save the 3 indices into your triangle list array.
//
// (3) Get the vertex array by calling:
//
// const double *vertices = Vl_getVertices(vl);
//
// (4) Get the number of vertices so you can copy them into
// your own buffer.
// unsigned int vcount = Vl_getVcount(vl);
//
// (5) Release the VertexLookup interface when you are done with it.
// Vl_releaseVertexLookup(vl);
//
// Teaches the following lessons:
//
// How to wrap the complexity of STL and C++ classes around a
// simple API interface.
//
// How to use an STL set and custom comparator operator for
// a complex data type.
//
// How to create a template class.
//
// How to achieve significant performance improvements by
// taking advantage of built in STL containers in just
// a few lines of code.
//
// You could easily modify this code to support other vertex
// formats with any number of interpolants.
//
// Hide C++ classes from the rest of your application by
// keeping them in the CPP and wrapping them in a namespace
// 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.
namespace ConvexDecomposition
{
typedef void * VertexLookup;
VertexLookup Vl_createVertexLookup(void);
void Vl_releaseVertexLookup(VertexLookup vlook);
unsigned int Vl_getIndex(VertexLookup vlook,const double *pos); // get index.
const double * Vl_getVertices(VertexLookup vlook);
unsigned int Vl_getVcount(VertexLookup vlook);
};
#endif

668
convex_decomposition/ConvexDecomposition/DecomposeSample.cpp Normal file
View File

@ -0,0 +1,668 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <float.h>
#include <vector>
#include "./ConvexDecomposition/ConvexDecomposition.h"
#include "./ConvexDecomposition/cd_wavefront.h"
using namespace ConvexDecomposition;
// Test application to demonstrate how to use the ConvexDecomposition system.
typedef std::vector< ConvexResult * > ConvexResultVector;
static const char * fstring(float v)
{
static char data[64*16];
static int index=0;
char *ret = &data[index*64];
index++;
if (index == 16 ) index = 0;
if ( v == FLT_MIN ) return "-INF"; // collada notation for FLT_MIN and FLT_MAX
if ( v == FLT_MAX ) return "INF";
if ( v == 1 )
{
strcpy(ret,"1");
}
else if ( v == 0 )
{
strcpy(ret,"0");
}
else if ( v == - 1 )
{
strcpy(ret,"-1");
}
else
{
sprintf(ret,"%.9f", v );
const char *dot = strstr(ret,".");
if ( dot )
{
int len = (int)strlen(ret);
char *foo = &ret[len-1];
while ( *foo == '0' ) foo--;
if ( *foo == '.' )
*foo = 0;
else
foo[1] = 0;
}
}
return ret;
}
class CBuilder : public ConvexDecompInterface
{
public:
CBuilder(const char *fname,const DecompDesc &d)
{
strcpy(mBaseName,fname);
char *dot = strstr(mBaseName,".");
if ( dot ) *dot = 0;
sprintf(mObjName,"%s_convex.obj", mBaseName );
mBaseCount = 0;
mHullCount = 0;
mSkinWidth = (float)d.mSkinWidth;
mFph = fopen(mObjName,"wb");
printf("########################################################################\r\n");
printf("# Perfomring approximate convex decomposition for triangle mesh %s\r\n", fname );
printf("# Input Mesh has %d vertices and %d triangles.\r\n", d.mVcount, d.mTcount );
printf("# Recursion depth: %d\r\n", d.mDepth );
printf("# Concavity Threshold Percentage: %0.2f\r\n", d.mCpercent );
printf("# Hull Merge Volume Percentage: %0.2f\r\n", d.mPpercent );
printf("# Maximum output vertices per hull: %d\r\n", d.mMaxVertices );
printf("# Hull Skin Width: %0.2f\r\n", d.mSkinWidth );
printf("########################################################################\r\n");
if ( mFph )
{
fprintf(mFph,"########################################################################\r\n");
fprintf(mFph,"# Approximate convex decomposition for triangle mesh %s\r\n", fname );
fprintf(mFph,"# Input Mesh has %d vertices and %d triangles.\r\n", d.mVcount, d.mTcount );
fprintf(mFph,"# Recursion depth: %d\r\n", d.mDepth );
fprintf(mFph,"# Concavity Threshold Percentage: %0.2f\r\n", d.mCpercent );
fprintf(mFph,"# Hull Merge Volume Percentage: %0.2f\r\n", d.mPpercent );
fprintf(mFph,"# Maximum output vertices per hull: %d\r\n", d.mMaxVertices );
fprintf(mFph,"# Hull Skin Width: %0.2f\r\n", d.mSkinWidth );
fprintf(mFph,"########################################################################\r\n");
printf("Opened Wavefront file %s for output.\r\n", mObjName );
}
else
{
printf("Failed to open output file %s\r\n", mObjName );
}
}
~CBuilder(void)
{
if ( mFph )
{
fclose(mFph);
}
for (unsigned int i=0; i<mHulls.size(); i++)
{
ConvexResult *cr = mHulls[i];
delete cr;
}
}
virtual void ConvexDecompResult(ConvexResult &result)
{
// we have a hull...
mHullCount++;
printf("Received hull %d HullVolume(%0.3f)\r\n", mHullCount, result.mHullVolume );
if ( mFph )
{
ConvexResult *cr = new ConvexResult(result);
mHulls.push_back(cr);
fprintf(mFph,"########################################################################\r\n");
fprintf(mFph,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount );
fprintf(mFph,"########################################################################\r\n");
for (unsigned int i=0; i<result.mHullVcount; i++)
{
const double *p = &result.mHullVertices[i*3];
fprintf(mFph,"v %0.9f %0.9f %0.9f\r\n", (float)p[0], (float)p[1], (float)p[2] );
}
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
unsigned int i1 = *src++;
unsigned int i2 = *src++;
unsigned int i3 = *src++;
i1+=mBaseCount;
i2+=mBaseCount;
i3+=mBaseCount;
fprintf(mFph,"f %d %d %d\r\n", i1+1, i2+1, i3+1 );
}
}
mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
}
}
void saveCOLLADA(FILE *fph,unsigned int index,ConvexResult *cr)
{
fprintf(fph," <geometry id=\"%s_%d-Mesh\" name=\"%s_%d-Mesh\">\r\n", mBaseName, index, mBaseName, index);
fprintf(fph," <convex_mesh>\r\n");
fprintf(fph," <source id=\"%s_%d-Position\">\r\n", mBaseName, index);
fprintf(fph," <float_array count=\"%d\" id=\"%s_%d-Position-array\">\r\n", cr->mHullVcount*3, mBaseName, index);
fprintf(fph," ");
for (unsigned int i=0; i<cr->mHullVcount; i++)
{
const double *p = &cr->mHullVertices[i*3];
fprintf(fph,"%s %s %s ", fstring((float)p[0]), fstring((float)p[1]), fstring((float)p[2]) );
if ( ((i+1)&3) == 0 && (i+1) < cr->mHullVcount )
{
fprintf(fph,"\r\n");
fprintf(fph," ");
}
}
fprintf(fph,"\r\n");
fprintf(fph," </float_array>\r\n");
fprintf(fph," <technique_common>\r\n");
fprintf(fph," <accessor count=\"%d\" source=\"#%s_%d-Position-array\" stride=\"3\">\r\n", cr->mHullVcount, mBaseName, index );
fprintf(fph," <param name=\"X\" type=\"float\"/>\r\n");
fprintf(fph," <param name=\"Y\" type=\"float\"/>\r\n");
fprintf(fph," <param name=\"Z\" type=\"float\"/>\r\n");
fprintf(fph," </accessor>\r\n");
fprintf(fph," </technique_common>\r\n");
fprintf(fph," </source>\r\n");
fprintf(fph," <vertices id=\"%s_%d-Vertex\">\r\n", mBaseName, index);
fprintf(fph," <input semantic=\"POSITION\" source=\"#%s_%d-Position\"/>\r\n", mBaseName, index);
fprintf(fph," </vertices>\r\n");
fprintf(fph," <triangles material=\"Material\" count=\"%d\">\r\n", cr->mHullTcount );
fprintf(fph," <input offset=\"0\" semantic=\"VERTEX\" source=\"#%s_%d-Vertex\"/>\r\n", mBaseName, index);
fprintf(fph," <p>\r\n");
fprintf(fph," ");
for (unsigned int i=0; i<cr->mHullTcount; i++)
{
unsigned int *tri = &cr->mHullIndices[i*3];
fprintf(fph,"%d %d %d ", tri[0], tri[1], tri[2] );
if ( ((i+1)&3) == 0 && (i+1) < cr->mHullTcount )
{
fprintf(fph,"\r\n");
fprintf(fph," ");
}
}
fprintf(fph,"\r\n");
fprintf(fph," </p>\r\n");
fprintf(fph," </triangles>\r\n");
fprintf(fph," </convex_mesh>\r\n");
fprintf(fph," </geometry>\r\n");
}
void saveCOLLADA(void)
{
char scratch[512];
sprintf(scratch,"%s.dae", mBaseName );
FILE *fph = fopen(scratch,"wb");
if ( fph )
{
printf("Saving convex decomposition of %d hulls to COLLADA file '%s'\r\n", (int)mHulls.size(), scratch );
fprintf(fph,"<?xml version=\"1.0\" encoding=\"utf-8\"?>\r\n");
fprintf(fph,"<COLLADA version=\"1.4.0\" xmlns=\"http://www.collada.org/2005/11/COLLADASchema\">\r\n");
fprintf(fph," <asset>\r\n");
fprintf(fph," <contributor>\r\n");
fprintf(fph," <author>NxuStream2 converter - http://www.ageia.com</author>\r\n");
fprintf(fph," <authoring_tool>PhysX Rocket, PhysX Viewer, or CreateDynamics</authoring_tool>\r\n");
fprintf(fph," <comments>questions to: jratcliff@ageia.com</comments>\r\n");
fprintf(fph," <copyright></copyright>\r\n");
fprintf(fph," <source_data>chair_gothic_tilted</source_data>\r\n");
fprintf(fph," </contributor>\r\n");
fprintf(fph," <unit meter=\"1\" name=\"meter\"/>\r\n");
fprintf(fph," <up_axis>Y_UP</up_axis>\r\n");
fprintf(fph," </asset>\r\n");
fprintf(fph," <library_materials>\r\n");
fprintf(fph," <material id=\"Material\" name=\"Material\">\r\n");
fprintf(fph," <instance_effect url=\"#Material-fx\"></instance_effect>\r\n");
fprintf(fph," </material>\r\n");
fprintf(fph," </library_materials>\r\n");
fprintf(fph," <library_effects>\r\n");
fprintf(fph," <effect id=\"Material-fx\" name=\"Material\">\r\n");
fprintf(fph," <profile_COMMON>\r\n");
fprintf(fph," <technique id=\"Material-fx-COMMON\" sid=\"COMMON\">\r\n");
fprintf(fph," <phong>\r\n");
fprintf(fph," <ambient>\r\n");
fprintf(fph," <color>0.803922 0.588235 0.92549 1</color>\r\n");
fprintf(fph," </ambient>\r\n");
fprintf(fph," <diffuse>\r\n");
fprintf(fph," <color>0.803922 0.588235 0.92549 1</color>\r\n");
fprintf(fph," </diffuse>\r\n");
fprintf(fph," <specular>\r\n");
fprintf(fph," <color>0.631373 0.631373 0.631373 1</color>\r\n");
fprintf(fph," </specular>\r\n");
fprintf(fph," <shininess>\r\n");
fprintf(fph," <float>1</float>\r\n");
fprintf(fph," </shininess>\r\n");
fprintf(fph," <reflective>\r\n");
fprintf(fph," <color>0 0 0 1</color>\r\n");
fprintf(fph," </reflective>\r\n");
fprintf(fph," <transparent>\r\n");
fprintf(fph," <color>1 1 1 1</color>\r\n");
fprintf(fph," </transparent>\r\n");
fprintf(fph," <transparency>\r\n");
fprintf(fph," <float>0</float>\r\n");
fprintf(fph," </transparency>\r\n");
fprintf(fph," </phong>\r\n");
fprintf(fph," </technique>\r\n");
fprintf(fph," </profile_COMMON>\r\n");
fprintf(fph," </effect>\r\n");
fprintf(fph," </library_effects>\r\n");
fprintf(fph," <library_geometries>\r\n");
for (unsigned int i=0; i<mHulls.size(); i++)
{
ConvexResult *cr = mHulls[i];
saveCOLLADA(fph,i,cr);
}
fprintf(fph," </library_geometries>\r\n");
fprintf(fph," <library_visual_scenes>\r\n");
fprintf(fph," <visual_scene id=\"Scene0-Visual\" name=\"Scene0-Visual\">\r\n");
fprintf(fph," <node id=\"%s-Node\" name=\"%s\" type=\"NODE\">\r\n", mBaseName, mBaseName );
fprintf(fph," <translate>0 0 0</translate>\r\n");
fprintf(fph," <rotate>1 0 0 0</rotate>\r\n");
fprintf(fph," </node>\r\n");
fprintf(fph," </visual_scene>\r\n");
fprintf(fph," </library_visual_scenes>\r\n");
fprintf(fph," <library_physics_materials>\r\n");
fprintf(fph," <physics_material id=\"pmat0_0-PhysicsMaterial\" name=\"pmat0_0-PhysicsMaterial\">\r\n");
fprintf(fph," <technique_common>\r\n");
fprintf(fph," <dynamic_friction>0.5</dynamic_friction>\r\n");
fprintf(fph," <restitution>0</restitution>\r\n");
fprintf(fph," <static_friction>0.5</static_friction>\r\n");
fprintf(fph," </technique_common>\r\n");
fprintf(fph," </physics_material>\r\n");
fprintf(fph," </library_physics_materials>\r\n");
fprintf(fph," <library_physics_models>\r\n");
fprintf(fph," <physics_model id=\"Scene0-PhysicsModel\">\r\n");
fprintf(fph," <rigid_body sid=\"%s-RigidBody\" name=\"%s\">\r\n", mBaseName, mBaseName);
fprintf(fph," <technique_common>\r\n");
fprintf(fph," <instance_physics_material url=\"#pmat0_0-PhysicsMaterial\"/>\r\n");
for (unsigned int i=0; i<mHulls.size(); i++)
{
ConvexResult *ch = mHulls[i];
fprintf(fph," <shape>\r\n");
fprintf(fph," <translate>0 0 0</translate>\r\n");
fprintf(fph," <rotate>1 0 0 0</rotate>\r\n");
fprintf(fph," <instance_physics_material url=\"#pmat0_0-PhysicsMaterial\"/>\r\n");
fprintf(fph," <density>1</density>\r\n");
fprintf(fph," <extra>\r\n");
fprintf(fph," <technique profile=\"PhysX\">\r\n");
fprintf(fph," <skinWidth>%s</skinWidth>\r\n", fstring( mSkinWidth ) );
fprintf(fph," </technique>\r\n");
fprintf(fph," </extra>\r\n");
fprintf(fph," <instance_geometry url=\"%s_%d-Mesh\"/>\r\n", mBaseName, i);
fprintf(fph," </shape>\r\n");
}
fprintf(fph," <dynamic>true</dynamic>\r\n");
fprintf(fph," <mass>1</mass>\r\n");
fprintf(fph," </technique_common>\r\n");
fprintf(fph," <extra>\r\n");
fprintf(fph," <technique profile=\"PhysX\">\r\n");
fprintf(fph," <extra>\r\n");
fprintf(fph," <technique profile=\"PhysX\">\r\n");
fprintf(fph," <linearDamping>0</linearDamping>\r\n");
fprintf(fph," <angularDamping>0</angularDamping>\r\n");
fprintf(fph," <solverIterationCount>32</solverIterationCount>\r\n");
fprintf(fph," </technique>\r\n");
fprintf(fph," </extra>\r\n");
fprintf(fph," <disable_collision>false</disable_collision>\r\n");
fprintf(fph," </technique>\r\n");
fprintf(fph," </extra>\r\n");
fprintf(fph," </rigid_body>\r\n");
fprintf(fph," </physics_model>\r\n");
fprintf(fph," </library_physics_models>\r\n");
fprintf(fph," <library_physics_scenes>\r\n");
fprintf(fph," <physics_scene id=\"Scene0-Physics\">\r\n");
fprintf(fph," <instance_physics_model url=\"#Scene0-PhysicsModel\">\r\n");
fprintf(fph," <instance_rigid_body target=\"#%s-Node\" body=\"%s-RigidBody\"/>\r\n", mBaseName, mBaseName );
fprintf(fph," <extra>\r\n");
fprintf(fph," <technique profile=\"PhysX\">\r\n");
fprintf(fph," </technique>\r\n");
fprintf(fph," </extra>\r\n");
fprintf(fph," </instance_physics_model>\r\n");
fprintf(fph," <technique_common>\r\n");
fprintf(fph," <gravity>0 -9.800000191 0</gravity>\r\n");
fprintf(fph," </technique_common>\r\n");
fprintf(fph," </physics_scene>\r\n");
fprintf(fph," </library_physics_scenes>\r\n");
fprintf(fph," <scene>\r\n");
fprintf(fph," <instance_visual_scene url=\"#Scene0-Visual\"/>\r\n");
fprintf(fph," <instance_physics_scene url=\"#Scene0-Physics\"/>\r\n");
fprintf(fph," </scene>\r\n");
fprintf(fph,"</COLLADA>\r\n");
fclose(fph);
}
else
{
printf("Failed to open file '%s' for write access.\r\n", scratch );
}
}
void saveXML(FILE *fph,unsigned int index,ConvexResult *cr)
{
fprintf(fph," <NxConvexMeshDesc id=\"%s_%d\">\r\n", mBaseName, index);
fprintf(fph," <points>\r\n");
fprintf(fph," ");
for (unsigned int i=0; i<cr->mHullVcount; i++)
{
const double *p = &cr->mHullVertices[i*3];
fprintf(fph,"%s %s %s ", fstring((float)p[0]), fstring((float)p[1]), fstring((float)p[2]) );
if ( ((i+1)&3) == 0 && (i+1) < cr->mHullVcount )
{
fprintf(fph,"\r\n");
fprintf(fph," ");
}
}
fprintf(fph,"\r\n");
fprintf(fph," </points>\r\n");
fprintf(fph," <triangles>\r\n");
fprintf(fph," ");
for (unsigned int i=0; i<cr->mHullTcount; i++)
{
unsigned int *tri = &cr->mHullIndices[i*3];
fprintf(fph,"%d %d %d ", tri[0], tri[1], tri[2] );
if ( ((i+1)&3) == 0 && (i+1) < cr->mHullTcount )
{
fprintf(fph,"\r\n");
fprintf(fph," ");
}
}
fprintf(fph,"\r\n");
fprintf(fph," </triangles>\r\n");
fprintf(fph," </NxConvexMeshDesc>\r\n");
}
void saveNxuStream(void)
{
char scratch[512];
sprintf(scratch,"%s.xml", mBaseName );
FILE *fph = fopen(scratch,"wb");
if ( fph )
{
printf("Saving convex decomposition of %d hulls to NxuStream file '%s'\r\n", mHulls.size(), scratch );
fprintf(fph,"<?xml version=\"1.0\" encoding=\"utf-8\"?>\r\n");
fprintf(fph," <NXUSTREAM2>\r\n");
fprintf(fph," <NxuPhysicsCollection id=\"beshon\" sdkVersion=\"244\" nxuVersion=\"100\">\r\n");
fprintf(fph," <NxPhysicsSDKDesc id=\"SDK\">\r\n");
fprintf(fph," <hwPageSize>65536</hwPageSize>\r\n");
fprintf(fph," <hwPageMax>256</hwPageMax>\r\n");
fprintf(fph," <hwConvexMax>2048</hwConvexMax>\r\n");
fprintf(fph," </NxPhysicsSDKDesc>\r\n");
for (unsigned int i=0; i<mHulls.size(); i++)
{
saveXML(fph, i, mHulls[i] );
}
fprintf(fph," <NxSceneDesc id=\"%s\">\r\n", mBaseName);
fprintf(fph," <filterBool>false</filterBool>\r\n");
fprintf(fph," <filterOp0>NX_FILTEROP_AND</filterOp0>\r\n");
fprintf(fph," <filterOp1>NX_FILTEROP_AND</filterOp1>\r\n");
fprintf(fph," <filterOp2>NX_FILTEROP_AND</filterOp2>\r\n");
fprintf(fph," <NxGroupsMask id=\"groupMask0\" bits0=\"0\" bits1=\"0\" bits2=\"0\" bits3=\"0\"/>\r\n");
fprintf(fph," <NxGroupsMask id=\"groupMask1\" bits0=\"0\" bits1=\"0\" bits2=\"0\" bits3=\"0\"/>\r\n");
fprintf(fph," <gravity>0 -9.800000191 0</gravity>\r\n");
fprintf(fph," <maxTimestep>0.016666668</maxTimestep>\r\n");
fprintf(fph," <maxIter>8</maxIter>\r\n");
fprintf(fph," <timeStepMethod>NX_TIMESTEP_FIXED</timeStepMethod>\r\n");
fprintf(fph," <maxBounds>FLT_MIN FLT_MIN FLT_MIN FLT_MAX FLT_MAX FLT_MAX</maxBounds>\r\n");
fprintf(fph," <NxSceneLimits id=\"limits\">\r\n");
fprintf(fph," <maxNbActors>0</maxNbActors>\r\n");
fprintf(fph," <maxNbBodies>0</maxNbBodies>\r\n");
fprintf(fph," <maxNbStaticShapes>0</maxNbStaticShapes>\r\n");
fprintf(fph," <maxNbDynamicShapes>0</maxNbDynamicShapes>\r\n");
fprintf(fph," <maxNbJoints>0</maxNbJoints>\r\n");
fprintf(fph," </NxSceneLimits>\r\n");
fprintf(fph," <simType>NX_SIMULATION_SW</simType>\r\n");
fprintf(fph," <groundPlane>true</groundPlane>\r\n");
fprintf(fph," <boundsPlanes>false</boundsPlanes>\r\n");
fprintf(fph," <NxSceneFlags id=\"flags\">\r\n");
fprintf(fph," <NX_SF_DISABLE_SSE>false</NX_SF_DISABLE_SSE>\r\n");
fprintf(fph," <NX_SF_DISABLE_COLLISIONS>false</NX_SF_DISABLE_COLLISIONS>\r\n");
fprintf(fph," <NX_SF_SIMULATE_SEPARATE_THREAD>true</NX_SF_SIMULATE_SEPARATE_THREAD>\r\n");
fprintf(fph," <NX_SF_ENABLE_MULTITHREAD>false</NX_SF_ENABLE_MULTITHREAD>\r\n");
fprintf(fph," </NxSceneFlags>\r\n");
fprintf(fph," <internalThreadCount>0</internalThreadCount>\r\n");
fprintf(fph," <backgroundThreadCount>0</backgroundThreadCount>\r\n");
fprintf(fph," <threadMask>1431655764</threadMask>\r\n");
fprintf(fph," <backgroundThreadMask>1431655764</backgroundThreadMask>\r\n");
fprintf(fph," <NxMaterialDesc id=\"Material_0\" userProperties=\"\" hasSpring=\"false\" materialIndex=\"0\">\r\n");
fprintf(fph," <dynamicFriction>0.5</dynamicFriction>\r\n");
fprintf(fph," <staticFriction>0.5</staticFriction>\r\n");
fprintf(fph," <restitution>0</restitution>\r\n");
fprintf(fph," <dynamicFrictionV>0</dynamicFrictionV>\r\n");
fprintf(fph," <staticFrictionV>0</staticFrictionV>\r\n");
fprintf(fph," <frictionCombineMode>NX_CM_AVERAGE</frictionCombineMode>\r\n");
fprintf(fph," <restitutionCombineMode>NX_CM_AVERAGE</restitutionCombineMode>\r\n");
fprintf(fph," <dirOfAnisotropy>1 0 0</dirOfAnisotropy>\r\n");
fprintf(fph," <NxMaterialFlag id=\"flags\">\r\n");
fprintf(fph," <NX_MF_ANISOTROPIC>false</NX_MF_ANISOTROPIC>\r\n");
fprintf(fph," <NX_MF_DISABLE_FRICTION>false</NX_MF_DISABLE_FRICTION>\r\n");
fprintf(fph," <NX_MF_DISABLE_STRONG_FRICTION>false</NX_MF_DISABLE_STRONG_FRICTION>\r\n");
fprintf(fph," </NxMaterialFlag>\r\n");
fprintf(fph," </NxMaterialDesc>\r\n");
fprintf(fph," <NxActorDesc id=\"%s\" userProperties=\"\" hasBody=\"true\" name=\"%s\">\r\n", mBaseName, mBaseName);
fprintf(fph," <globalPose>1 0 0 0 1 0 0 0 1 0 0 0 </globalPose>\r\n");
fprintf(fph," <NxBodyDesc>\r\n");
fprintf(fph," <mass>1</mass>\r\n");
fprintf(fph," <linearDamping>0</linearDamping>\r\n");
fprintf(fph," <angularDamping>0</angularDamping>\r\n");
fprintf(fph," <solverIterationCount>32</solverIterationCount>\r\n");
fprintf(fph," <NxBodyFlag id=\"flags\">\r\n");
fprintf(fph," <NX_BF_POSE_SLEEP_TEST>true</NX_BF_POSE_SLEEP_TEST>\r\n");
fprintf(fph," <NX_AF_DISABLE_COLLISION>false</NX_AF_DISABLE_COLLISION>\r\n");
fprintf(fph," </NxBodyFlag>\r\n");
fprintf(fph," </NxBodyDesc>\r\n");
fprintf(fph," <name>Bip01 Pelvis</name>\r\n");
for (unsigned int i=0; i<mHulls.size(); i++)
{
fprintf(fph," <NxConvexShapeDesc id=\"Shape_%d\" meshData=\"%s_%d\">\r\n", i, mBaseName, i);
fprintf(fph," <NxShapeDesc>\r\n");
fprintf(fph," <localPose>1 0 0 0 1 0 0 0 1 0 0 0 </localPose>\r\n");
fprintf(fph," <skinWidth>0</skinWidth>\r\n");
fprintf(fph," </NxShapeDesc>\r\n");
fprintf(fph," </NxConvexShapeDesc>\r\n");
}
fprintf(fph," </NxActorDesc>\r\n");
fprintf(fph," </NxSceneDesc>\r\n");
fprintf(fph," <NxSceneInstance id=\"%s\">\r\n", mBaseName);
fprintf(fph," <sceneName>beshon</sceneName>\r\n");
fprintf(fph," <NxuUserProperties></NxuUserProperties>\r\n");
fprintf(fph," <rootNode>1 0 0 0 1 0 0 0 1 0 0 0</rootNode>\r\n");
fprintf(fph," <newScene>false</newScene>\r\n");
fprintf(fph," <ignorePlane>true</ignorePlane>\r\n");
fprintf(fph," <numSceneInstances>0</numSceneInstances>\r\n");
fprintf(fph," </NxSceneInstance>\r\n");
fprintf(fph," </NxuPhysicsCollection>\r\n");
fprintf(fph,"</NXUSTREAM2>\r\n");
}
else
{
printf("Failed to open file '%s' for write access.\r\n", scratch );
}
}
float mSkinWidth;
unsigned int mHullCount;
FILE *mFph;
unsigned int mBaseCount;
char mObjName[512];
char mBaseName[512];
ConvexResultVector mHulls;
};
int main(int argc,const char **argv)
{
if ( argc < 2 )
{
printf("Usage: Test <meshanme.obj> (options)\r\n");
printf("\r\n");
printf("Options:\r\n");
printf("\r\n");
printf(" -d<depth> : How deep to recursively split. Values of 3-7 are reasonable.\r\n");
printf(" -c<percent> : Percentage of concavity to keep splitting. 0-20% is reasonable.\r\n");
printf(" -p<percent> : Percentage of volume delta to collapse hulls. 0-30% is reasonable.\r\n");
printf(" -v<maxverts> : Maximum number of vertices in the output hull. Default is 32.\r\n");
printf(" -s<skinwidth> : Skin Width inflation. Default is 0.\r\n");
}
else
{
unsigned int depth = 5;
float cpercent = 5;
float ppercent = 5;
unsigned int maxv = 32;
float skinWidth = 0;
// process command line switches.
for (int i=2; i<argc; i++)
{
const char *o = argv[i];
if ( strncmp(o,"-d",2) == 0 )
{
depth = (unsigned int) atoi( &o[2] );
if ( depth < 0 || depth > 10 )
{
depth = 5;
printf("Invalid depth value in switch, defaulting to 5.\r\n");
}
}
if ( strncmp(o,"-c",2) == 0 )
{
cpercent = (float) atof( &o[2] );
if ( cpercent < 0 || cpercent > 100 )
{
cpercent = 5;
printf("Invalid concavity percentage in switch, defaulting to 5.\r\n");
}
}
if ( strncmp(o,"-p",2) == 0 )
{
ppercent = (float) atof( &o[2] );
if ( ppercent < 0 || ppercent > 100 )
{
ppercent = 5;
printf("Invalid hull merge percentage in switch, defaulting to 5.\r\n");
}
}
if ( strncmp(o,"-v",2) == 0 )
{
maxv = (unsigned int) atoi( &o[2] );
if ( maxv < 8 || maxv > 256 )
{
maxv = 32;
printf("Invalid max vertices in switch, defaulting to 32.\r\n");
}
}
if ( strncmp(o,"-s",2) == 0 )
{
skinWidth = (float) atof( &o[2] );
if ( skinWidth < 0 || skinWidth > 0.1f )
{
skinWidth = 0;
printf("Invalid skinWidth in switch, defaulting to 0.\r\n");
}
}
}
WavefrontObj wo;
unsigned int tcount = wo.loadObj(argv[1]);
if ( tcount )
{
DecompDesc d;
d.mVcount = wo.mVertexCount;
d.mVertices = wo.mVertices;
d.mTcount = wo.mTriCount;
d.mIndices = (unsigned int *)wo.mIndices;
d.mDepth = depth;
d.mCpercent = cpercent;
d.mPpercent = ppercent;
d.mMaxVertices = maxv;
d.mSkinWidth = skinWidth;
CBuilder cb(argv[1],d); // receives the answers and writes out a wavefront OBJ file.
d.mCallback = &cb;
unsigned int count = performConvexDecomposition(d);
if ( count )
{
printf("Input triangle mesh with %d triangles produced %d output hulls.\r\n", d.mTcount, count );
cb.saveNxuStream(); // save results in NxuStream format.
cb.saveCOLLADA(); // save results in COLLADA physics 1.4.1 format.
}
else
{
printf("Failed to produce any convex hulls.\r\n");
}
}
else
{
// sorry..no
printf("Sorry unable to load file '%s'\r\n", argv[1] );
}
}
}

BIN
convex_decomposition/ConvexDecomposition/DecomposeSample.exe Normal file
View File

Binary file not shown.

21
convex_decomposition/ConvexDecomposition/DecomposeSample.sln Normal file
View File

@ -0,0 +1,21 @@
Microsoft Visual Studio Solution File, Format Version 8.00
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "DecomposeSample", "DecomposeSample.vcproj", "{0282D4E2-DA97-437D-9BDE-1C9F5E56F6F8}"
ProjectSection(ProjectDependencies) = postProject
EndProjectSection
EndProject
Global
GlobalSection(SolutionConfiguration) = preSolution
Debug = Debug
Release = Release
EndGlobalSection
GlobalSection(ProjectConfiguration) = postSolution
{0282D4E2-DA97-437D-9BDE-1C9F5E56F6F8}.Debug.ActiveCfg = Debug|Win32
{0282D4E2-DA97-437D-9BDE-1C9F5E56F6F8}.Debug.Build.0 = Debug|Win32
{0282D4E2-DA97-437D-9BDE-1C9F5E56F6F8}.Release.ActiveCfg = Release|Win32
{0282D4E2-DA97-437D-9BDE-1C9F5E56F6F8}.Release.Build.0 = Release|Win32
EndGlobalSection
GlobalSection(ExtensibilityGlobals) = postSolution
EndGlobalSection
GlobalSection(ExtensibilityAddIns) = postSolution
EndGlobalSection
EndGlobal

BIN
convex_decomposition/ConvexDecomposition/DecomposeSample.suo Normal file
View File

Binary file not shown.

240
convex_decomposition/ConvexDecomposition/DecomposeSample.vcproj Normal file
View File

@ -0,0 +1,240 @@
<?xml version="1.0" encoding="Windows-1252"?>
<VisualStudioProject
ProjectType="Visual C++"
Version="7.10"
Name="DecomposeSample"
SccProjectName=""
SccLocalPath="">
<Platforms>
<Platform
Name="Win32"/>
</Platforms>
<Configurations>
<Configuration
Name="Release|Win32"
OutputDirectory=".\Release"
IntermediateDirectory=".\Release"
ConfigurationType="1"
UseOfMFC="0"
ATLMinimizesCRunTimeLibraryUsage="FALSE"
CharacterSet="2">
<Tool
Name="VCCLCompilerTool"
Optimization="2"
InlineFunctionExpansion="1"
PreprocessorDefinitions="WIN32;NDEBUG;_CONSOLE"
StringPooling="TRUE"
RuntimeLibrary="0"
EnableFunctionLevelLinking="TRUE"
PrecompiledHeaderFile=".\Release/DecomposeSample.pch"
AssemblerListingLocation=".\Release/"
ObjectFile=".\Release/"
ProgramDataBaseFileName=".\Release/"
WarningLevel="3"
SuppressStartupBanner="TRUE"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLinkerTool"
OutputFile=".\Release/DecomposeSample.exe"
LinkIncremental="1"
SuppressStartupBanner="TRUE"
ProgramDatabaseFile=".\Release/DecomposeSample.pdb"
SubSystem="1"
TargetMachine="1"/>
<Tool
Name="VCMIDLTool"
TypeLibraryName=".\Release/DecomposeSample.tlb"
HeaderFileName=""/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
<Tool
Name="VCResourceCompilerTool"
PreprocessorDefinitions="NDEBUG"
Culture="1033"/>
<Tool
Name="VCWebServiceProxyGeneratorTool"/>
<Tool
Name="VCXMLDataGeneratorTool"/>
<Tool
Name="VCWebDeploymentTool"/>
<Tool
Name="VCManagedWrapperGeneratorTool"/>
<Tool
Name="VCAuxiliaryManagedWrapperGeneratorTool"/>
</Configuration>
<Configuration
Name="Debug|Win32"
OutputDirectory=".\Debug"
IntermediateDirectory=".\Debug"
ConfigurationType="1"
UseOfMFC="0"
ATLMinimizesCRunTimeLibraryUsage="FALSE"
CharacterSet="2">
<Tool
Name="VCCLCompilerTool"
Optimization="0"
PreprocessorDefinitions="WIN32;_DEBUG;_CONSOLE"
BasicRuntimeChecks="3"
RuntimeLibrary="1"
PrecompiledHeaderFile=".\Debug/DecomposeSample.pch"
AssemblerListingLocation=".\Debug/"
ObjectFile=".\Debug/"
ProgramDataBaseFileName=".\Debug/"
WarningLevel="3"
SuppressStartupBanner="TRUE"
DebugInformationFormat="4"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLinkerTool"
OutputFile=".\Debug/DecomposeSample.exe"
LinkIncremental="1"
SuppressStartupBanner="TRUE"
GenerateDebugInformation="TRUE"
ProgramDatabaseFile=".\Debug/DecomposeSample.pdb"
SubSystem="1"
TargetMachine="1"/>
<Tool
Name="VCMIDLTool"
TypeLibraryName=".\Debug/DecomposeSample.tlb"
HeaderFileName=""/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
<Tool
Name="VCResourceCompilerTool"
PreprocessorDefinitions="_DEBUG"
Culture="1033"/>
<Tool
Name="VCWebServiceProxyGeneratorTool"/>
<Tool
Name="VCXMLDataGeneratorTool"/>
<Tool
Name="VCWebDeploymentTool"/>
<Tool
Name="VCManagedWrapperGeneratorTool"/>
<Tool
Name="VCAuxiliaryManagedWrapperGeneratorTool"/>
</Configuration>
</Configurations>
<References>
</References>
<Files>
<Filter
Name="Source Files"
Filter="cpp;c;cxx;rc;def;r;odl;idl;hpj;bat">
<File
RelativePath=".\DecomposeSample.cpp">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="h;hpp;hxx;hm;inl">
</Filter>
<Filter
Name="Resource Files"
Filter="ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe">
</Filter>
<Filter
Name="ConvexDecomposition"
Filter="">
<File
RelativePath=".\ConvexDecomposition\bestfit.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\bestfit.h">
</File>
<File
RelativePath=".\ConvexDecomposition\bestfitobb.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\bestfitobb.h">
</File>
<File
RelativePath=".\ConvexDecomposition\cd_hull.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\cd_hull.h">
</File>
<File
RelativePath=".\ConvexDecomposition\cd_vector.h">
</File>
<File
RelativePath=".\ConvexDecomposition\cd_wavefront.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\cd_wavefront.h">
</File>
<File
RelativePath=".\ConvexDecomposition\concavity.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\concavity.h">
</File>
<File
RelativePath=".\ConvexDecomposition\ConvexDecomposition.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\ConvexDecomposition.h">
</File>
<File
RelativePath=".\ConvexDecomposition\fitsphere.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\fitsphere.h">
</File>
<File
RelativePath=".\ConvexDecomposition\float_math.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\float_math.h">
</File>
<File
RelativePath=".\ConvexDecomposition\meshvolume.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\meshvolume.h">
</File>
<File
RelativePath=".\ConvexDecomposition\planetri.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\planetri.h">
</File>
<File
RelativePath=".\ConvexDecomposition\raytri.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\raytri.h">
</File>
<File
RelativePath=".\ConvexDecomposition\splitplane.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\splitplane.h">
</File>
<File
RelativePath=".\ConvexDecomposition\triangulate.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\triangulate.h">
</File>
<File
RelativePath=".\ConvexDecomposition\vlookup.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\vlookup.h">
</File>
</Filter>
</Files>
<Globals>
</Globals>
</VisualStudioProject>

93
convex_decomposition/ConvexDecomposition/Makefile Normal file
View File

@ -0,0 +1,93 @@
OBJS = DecomposeSample.o \
ConvexDecomposition/bestfit.o ConvexDecomposition/float_math.o \
ConvexDecomposition/bestfitobb.o ConvexDecomposition/meshvolume.o \
ConvexDecomposition/cd_hull.o ConvexDecomposition/planetri.o \
ConvexDecomposition/cd_wavefront.o ConvexDecomposition/raytri.o \
ConvexDecomposition/concavity.o ConvexDecomposition/splitplane.o \
ConvexDecomposition/ConvexDecomposition.o ConvexDecomposition/triangulate.o \
ConvexDecomposition/fitsphere.o ConvexDecomposition/vlookup.o
HEADERS = \
ConvexDecomposition/bestfit.h \
ConvexDecomposition/bestfitobb.h \
ConvexDecomposition/cd_hull.h \
ConvexDecomposition/cd_vector.h \
ConvexDecomposition/cd_wavefront.h \
ConvexDecomposition/concavity.h \
ConvexDecomposition/ConvexDecomposition.h \
ConvexDecomposition/fitsphere.h \
ConvexDecomposition/float_math.h \
ConvexDecomposition/meshvolume.h \
ConvexDecomposition/planetri.h \
ConvexDecomposition/raytri.h \
ConvexDecomposition/splitplane.h \
ConvexDecomposition/triangulate.h \
ConvexDecomposition/vlookup.h
CC = g++
DEBUG = -ggdb
CFLAGS = -IConvexDecomposition -Wall -c $(DEBUG)
LFLAGS = $(DEBUG)
convex_decomposition: $(OBJS)
$(CC) $(LFLAGS) $(OBJS) -o convex_decomposition
DecomposeSample.o: DecomposeSample.cpp
$(CC) $(CFLAGS) DecomposeSample.cpp -o $@
ConvexDecomposition/bestfit.o: ConvexDecomposition/bestfit.cpp
$(CC) $(CFLAGS) ConvexDecomposition/bestfit.cpp -o $@
ConvexDecomposition/bestfitobb.o: ConvexDecomposition/bestfitobb.cpp
$(CC) $(CFLAGS) ConvexDecomposition/bestfitobb.cpp -o $@
ConvexDecomposition/cd_hull.o: ConvexDecomposition/cd_hull.cpp
$(CC) $(CFLAGS) ConvexDecomposition/cd_hull.cpp -o $@
ConvexDecomposition/cd_wavefront.o: ConvexDecomposition/cd_wavefront.cpp
$(CC) $(CFLAGS) ConvexDecomposition/cd_wavefront.cpp -o $@
ConvexDecomposition/concavity.o: ConvexDecomposition/concavity.cpp
$(CC) $(CFLAGS) ConvexDecomposition/concavity.cpp -o $@
ConvexDecomposition/ConvexDecomposition.o: ConvexDecomposition/ConvexDecomposition.cpp
$(CC) $(CFLAGS) ConvexDecomposition/ConvexDecomposition.cpp -o $@
ConvexDecomposition/fitsphere.o: ConvexDecomposition/fitsphere.cpp
$(CC) $(CFLAGS) ConvexDecomposition/fitsphere.cpp -o $@
ConvexDecomposition/float_math.o: ConvexDecomposition/float_math.cpp
$(CC) $(CFLAGS) ConvexDecomposition/float_math.cpp -o $@
ConvexDecomposition/meshvolume.o: ConvexDecomposition/meshvolume.cpp
$(CC) $(CFLAGS) ConvexDecomposition/meshvolume.cpp -o $@
ConvexDecomposition/planetri.o: ConvexDecomposition/planetri.cpp
$(CC) $(CFLAGS) ConvexDecomposition/planetri.cpp -o $@
ConvexDecomposition/raytri.o: ConvexDecomposition/raytri.cpp
$(CC) $(CFLAGS) ConvexDecomposition/raytri.cpp -o $@
ConvexDecomposition/splitplane.o: ConvexDecomposition/splitplane.cpp
$(CC) $(CFLAGS) ConvexDecomposition/splitplane.cpp -o $@
ConvexDecomposition/triangulate.o: ConvexDecomposition/triangulate.cpp
$(CC) $(CFLAGS) ConvexDecomposition/triangulate.cpp -o $@
ConvexDecomposition/vlookup.o: ConvexDecomposition/vlookup.cpp ConvexDecomposition/vlookup.cpp
$(CC) $(CFLAGS) ConvexDecomposition/vlookup.cpp -o $@
install:
cp convex_decomposition ../../convex_decomposition/bin/
clean:
\rm *.o */*.o convex_decomposition
tar:
tar cfv ConvexDecomposition.tar DecomposeSample.cpp convex_decomposition Makefile \
ConvexDecomposition

3083
convex_decomposition/ConvexDecomposition/chair.obj Normal file
View File

File diff suppressed because it is too large Load Diff

75
convex_decomposition/ConvexDecomposition/readme.txt Normal file
View File

@ -0,0 +1,75 @@
ConvexDecomposition library written by John W. Ratcliff
Usage: Test <meshanme.obj> (options)
Options:
-d<depth> : How deep to recursively split. Values of 3-7 are reasonable.
-c<percent> : Percentage of concavity to keep splitting. 0-20 4324640s reasonable.
-p<percent> : Percentage of volume delta to collapse hulls. 0-30 4324544s reasonable.
-v<maxverts> : Maximum number of vertices in the output hull. Default is 32.
-s<skinwidth> : Skin Width inflation. Default is 0.0
To run the sample provided with defaults:
DecomposeSample chair.obj
Will produce 'chair_convex.obj' a Wavefront OBJ file useful for debug visualization.
'chair.dae' a COLLADA physics representation of the model.
'chair.xml' an Ageia PhysX NxuStream representation of the model.
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

33
convex_decomposition/Makefile Normal file
View File

@ -0,0 +1,33 @@
all: installed
TARBALL = build/ConvexDecomposition.zip
TARBALL_URL = http://pr.willowgarage.com/downloads/ConvexDecomposition.zip
SOURCE_DIR = build/convex_decomposition
INITIAL_DIR = build/ConvexDecomposition
UNPACK_CMD = unzip
TARBALL_PATCH=convex_decomposition.patch
include $(shell rospack find mk)/download_unpack_build.mk
ROOT = $(shell rospack find convex_decomposition)/convex_decomposition
installed: wiped $(SOURCE_DIR)/unpacked
@echo "making it"
@echo "ROOT is: $(ROOT)"
-mkdir -p $(ROOT)
-mkdir -p $(ROOT)/bin
cd $(SOURCE_DIR) ; make $(ROS_PARALLEL_JOBS); make install
touch installed
wiped: Makefile
make wipe
touch wiped
clean:
-cd $(SOURCE_DIR) && make clean
rm -rf $(ROOT) installed
wipe: clean
rm -rf build
.PHONY : clean download wipe

429
convex_decomposition/convex_decomposition.patch Normal file
View File

@ -0,0 +1,429 @@
Index: ConvexDecomposition/vlookup.cpp
===================================================================
--- ConvexDecomposition/vlookup.cpp (revision 8299)
+++ ConvexDecomposition/vlookup.cpp (working copy)
@@ -164,7 +164,7 @@
};
-template <class Type> class VertexLess
+template <typename Type> class VertexLess
{
public:
typedef std::vector< Type > VertexVector;
@@ -188,7 +188,7 @@
static VertexVector *mList;
};
-template <class Type> class VertexPool
+template <typename Type> class VertexPool
{
public:
typedef std::set<int, VertexLess<Type> > VertexSet;
@@ -197,7 +197,7 @@
int GetVertex(const Type& vtx)
{
VertexLess<Type>::SetSearch(vtx,&mVtxs);
- VertexSet::iterator found;
+ typename VertexSet::iterator found;
found = mVertSet.find( -1 );
if ( found != mVertSet.end() )
{
@@ -254,10 +254,10 @@
VertexVector mVtxs; // set of vertices.
};
+double tmpp[3] = {0,0,0};
+template<> VertexPosition VertexLess<VertexPosition>::mFind = tmpp;
+template<> std::vector<VertexPosition > *VertexLess<VertexPosition>::mList =0;
-VertexPosition VertexLess<VertexPosition>::mFind;
-std::vector<VertexPosition > *VertexLess<VertexPosition>::mList=0;
-
enum RDIFF
{
RD_EQUAL,
@@ -288,6 +288,7 @@
}
+template<>
bool VertexLess<VertexPosition>::operator()(int v1,int v2) const
{
bool ret = false;
Index: ConvexDecomposition/ConvexDecomposition.cpp
===================================================================
--- ConvexDecomposition/ConvexDecomposition.cpp (revision 8299)
+++ ConvexDecomposition/ConvexDecomposition.cpp (working copy)
@@ -66,7 +66,7 @@
#include "cd_vector.h"
#include "cd_hull.h"
#include "bestfit.h"
-#include "PlaneTri.h"
+#include "planetri.h"
#include "vlookup.h"
#include "splitplane.h"
#include "meshvolume.h"
@@ -760,3 +760,3 @@
- assert( fcount >= 3 && fcount <= 4);
- assert( bcount >= 3 && bcount <= 4);
-
+ if ( fcount >= 3 && fcount <= 4)
+ if ( bcount >= 3 && bcount <= 4)
+ {
@@ -777,1 +777,1 @@
-
+ }
Index: ConvexDecomposition/splitplane.cpp
===================================================================
--- ConvexDecomposition/splitplane.cpp (revision 8299)
+++ ConvexDecomposition/splitplane.cpp (working copy)
@@ -67,7 +67,7 @@
#include "cd_hull.h"
#include "cd_wavefront.h"
#include "bestfit.h"
-#include "PlaneTri.h"
+#include "planetri.h"
#include "vlookup.h"
#include "meshvolume.h"
#include "bestfitobb.h"
Index: ConvexDecomposition/cd_vector.h
===================================================================
--- ConvexDecomposition/cd_vector.h (revision 8299)
+++ ConvexDecomposition/cd_vector.h (working copy)
@@ -309,12 +309,12 @@
Type FastMagnitude(void) const
{
- return Type(fast_sqrt(x * x + y * y + z * z));
+ return Type(sqrt(x * x + y * y + z * z));
};
Type FasterMagnitude(void) const
{
- return Type(faster_sqrt(x * x + y * y + z * z));
+ return Type(sqrt(x * x + y * y + z * z));
};
void Lerp(const Vector3d<Type>& from,const Vector3d<Type>& to,double slerp)
@@ -436,13 +436,13 @@
Type FastLength(void) const // length of vector.
{
- return Type(fast_sqrt( x*x + y*y + z*z ));
+ return Type(sqrt( x*x + y*y + z*z ));
};
Type FasterLength(void) const // length of vector.
{
- return Type(faster_sqrt( x*x + y*y + z*z ));
+ return Type(sqrt( x*x + y*y + z*z ));
};
Type Length2(void) const // squared distance, prior to square root.
@@ -518,7 +518,7 @@
inline double FastNormalize(void) // normalize to a unit vector, returns distance.
{
- double d = fast_sqrt( static_cast< double >( x*x + y*y + z*z ) );
+ double d = sqrt( static_cast< double >( x*x + y*y + z*z ) );
if ( d > 0 )
{
double r = 1.0f / d;
@@ -535,7 +535,7 @@
inline double FasterNormalize(void) // normalize to a unit vector, returns distance.
{
- double d = faster_sqrt( static_cast< double >( x*x + y*y + z*z ) );
+ double d = sqrt( static_cast< double >( x*x + y*y + z*z ) );
if ( d > 0 )
{
double r = 1.0f / d;
@@ -1029,7 +1029,8 @@
void Zero(void)
{
- x = y = z = 0;
+ x = 0;
+ y = 0;
};
Vector2d negative(void) const
@@ -1047,12 +1048,12 @@
Type fastmagnitude(void) const
{
- return (Type) fast_sqrt(x * x + y * y );
+ return (Type) sqrt(x * x + y * y );
}
Type fastermagnitude(void) const
{
- return (Type) faster_sqrt( x * x + y * y );
+ return (Type) sqrt( x * x + y * y );
}
void Reflection(Vector2d &a,Vector2d &b); // compute reflection vector.
@@ -1064,12 +1065,12 @@
Type FastLength(void) const // length of vector.
{
- return Type(fast_sqrt( x*x + y*y ));
+ return Type(sqrt( x*x + y*y ));
};
Type FasterLength(void) const // length of vector.
{
- return Type(faster_sqrt( x*x + y*y ));
+ return Type(sqrt( x*x + y*y ));
};
Type Length2(void) // squared distance, prior to square root.
@@ -1090,7 +1091,7 @@
Type dx = a.x - x;
Type dy = a.y - y;
Type d = dx*dx+dy*dy;
- return fast_sqrt(d);
+ return sqrt(d);
};
Type FasterDistance(const Vector2d &a) const // distance between two points.
@@ -1098,7 +1099,7 @@
Type dx = a.x - x;
Type dy = a.y - y;
Type d = dx*dx+dy*dy;
- return faster_sqrt(d);
+ return sqrt(d);
};
Type Distance2(Vector2d &a) // squared distance.
Index: ConvexDecomposition/float_math.cpp
===================================================================
--- ConvexDecomposition/float_math.cpp (revision 8299)
+++ ConvexDecomposition/float_math.cpp (working copy)
@@ -212,8 +212,14 @@
matrix[1*4+2] = 2 * ( yz + wx );
matrix[2*4+2] = 1 - 2 * ( xx + yy );
- matrix[3*4+0] =(double) matrix[3*4+1] = matrix[3*4+2] = 0.0f;
- matrix[0*4+3] =(double) matrix[1*4+3] = matrix[2*4+3] = 0.0f;
+ matrix[3*4+0] = 0.0f;
+ matrix[3*4+1] = 0.0f;
+ matrix[3*4+2] = 0.0f;
+
+ matrix[0*4+3] = 0.0f;
+ matrix[1*4+3] = 0.0f;
+ matrix[2*4+3] = 0.0f;
+
matrix[3*4+3] =(double) 1.0f;
}
Index: ConvexDecomposition/cd_wavefront.cpp
===================================================================
--- ConvexDecomposition/cd_wavefront.cpp (revision 8299)
+++ ConvexDecomposition/cd_wavefront.cpp (working copy)
@@ -672,7 +672,7 @@
const char *foo = argv[0];
if ( *foo != '#' )
{
- if ( stricmp(argv[0],"v") == 0 && argc == 4 )
+ if ( strcmp(argv[0],"v") == 0 && argc == 4 )
{
double vx = (double) atof( argv[1] );
double vy = (double) atof( argv[2] );
@@ -681,14 +681,14 @@
mVerts.push_back(vy);
mVerts.push_back(vz);
}
- else if ( stricmp(argv[0],"vt") == 0 && argc == 3 )
+ else if ( strcmp(argv[0],"vt") == 0 && argc == 3 )
{
double tx = (double) atof( argv[1] );
double ty = (double) atof( argv[2] );
mTexels.push_back(tx);
mTexels.push_back(ty);
}
- else if ( stricmp(argv[0],"vn") == 0 && argc == 4 )
+ else if ( strcmp(argv[0],"vn") == 0 && argc == 4 )
{
double normalx = (double) atof(argv[1]);
double normaly = (double) atof(argv[2]);
@@ -697,7 +697,7 @@
mNormals.push_back(normaly);
mNormals.push_back(normalz);
}
- else if ( stricmp(argv[0],"f") == 0 && argc >= 4 )
+ else if ( strcmp(argv[0],"f") == 0 && argc >= 4 )
{
GeometryVertex v[32];
Index: Makefile
===================================================================
--- Makefile (revision 0)
+++ Makefile (revision 8581)
@@ -0,0 +1,93 @@
+
+
+OBJS = DecomposeSample.o \
+ ConvexDecomposition/bestfit.o ConvexDecomposition/float_math.o \
+ ConvexDecomposition/bestfitobb.o ConvexDecomposition/meshvolume.o \
+ ConvexDecomposition/cd_hull.o ConvexDecomposition/planetri.o \
+ ConvexDecomposition/cd_wavefront.o ConvexDecomposition/raytri.o \
+ ConvexDecomposition/concavity.o ConvexDecomposition/splitplane.o \
+ ConvexDecomposition/ConvexDecomposition.o ConvexDecomposition/triangulate.o \
+ ConvexDecomposition/fitsphere.o ConvexDecomposition/vlookup.o
+
+HEADERS = \
+ ConvexDecomposition/bestfit.h \
+ ConvexDecomposition/bestfitobb.h \
+ ConvexDecomposition/cd_hull.h \
+ ConvexDecomposition/cd_vector.h \
+ ConvexDecomposition/cd_wavefront.h \
+ ConvexDecomposition/concavity.h \
+ ConvexDecomposition/ConvexDecomposition.h \
+ ConvexDecomposition/fitsphere.h \
+ ConvexDecomposition/float_math.h \
+ ConvexDecomposition/meshvolume.h \
+ ConvexDecomposition/planetri.h \
+ ConvexDecomposition/raytri.h \
+ ConvexDecomposition/splitplane.h \
+ ConvexDecomposition/triangulate.h \
+ ConvexDecomposition/vlookup.h
+
+CC = g++
+
+DEBUG = -ggdb
+
+CFLAGS = -IConvexDecomposition -Wall -c $(DEBUG)
+
+LFLAGS = $(DEBUG)
+
+convex_decomposition: $(OBJS)
+ $(CC) $(LFLAGS) $(OBJS) -o convex_decomposition
+
+DecomposeSample.o: DecomposeSample.cpp
+ $(CC) $(CFLAGS) DecomposeSample.cpp -o $@
+
+ConvexDecomposition/bestfit.o: ConvexDecomposition/bestfit.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/bestfit.cpp -o $@
+
+ConvexDecomposition/bestfitobb.o: ConvexDecomposition/bestfitobb.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/bestfitobb.cpp -o $@
+
+ConvexDecomposition/cd_hull.o: ConvexDecomposition/cd_hull.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/cd_hull.cpp -o $@
+
+ConvexDecomposition/cd_wavefront.o: ConvexDecomposition/cd_wavefront.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/cd_wavefront.cpp -o $@
+
+ConvexDecomposition/concavity.o: ConvexDecomposition/concavity.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/concavity.cpp -o $@
+
+ConvexDecomposition/ConvexDecomposition.o: ConvexDecomposition/ConvexDecomposition.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/ConvexDecomposition.cpp -o $@
+
+ConvexDecomposition/fitsphere.o: ConvexDecomposition/fitsphere.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/fitsphere.cpp -o $@
+
+ConvexDecomposition/float_math.o: ConvexDecomposition/float_math.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/float_math.cpp -o $@
+
+ConvexDecomposition/meshvolume.o: ConvexDecomposition/meshvolume.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/meshvolume.cpp -o $@
+
+ConvexDecomposition/planetri.o: ConvexDecomposition/planetri.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/planetri.cpp -o $@
+
+ConvexDecomposition/raytri.o: ConvexDecomposition/raytri.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/raytri.cpp -o $@
+
+ConvexDecomposition/splitplane.o: ConvexDecomposition/splitplane.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/splitplane.cpp -o $@
+
+ConvexDecomposition/triangulate.o: ConvexDecomposition/triangulate.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/triangulate.cpp -o $@
+
+ConvexDecomposition/vlookup.o: ConvexDecomposition/vlookup.cpp ConvexDecomposition/vlookup.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/vlookup.cpp -o $@
+
+install:
+ cp convex_decomposition ../../convex_decomposition/bin/
+
+clean:
+ \rm *.o */*.o convex_decomposition
+
+tar:
+ tar cfv ConvexDecomposition.tar DecomposeSample.cpp convex_decomposition Makefile \
+ ConvexDecomposition
Index: DecomposeSample.cpp
===================================================================
--- DecomposeSample.cpp (revision 8299)
+++ DecomposeSample.cpp (working copy)
@@ -6,7 +6,7 @@
#include <vector>
-#include "./ConvexDecomposition/convexdecomposition.h"
+#include "./ConvexDecomposition/ConvexDecomposition.h"
#include "./ConvexDecomposition/cd_wavefront.h"
using namespace ConvexDecomposition;
@@ -227,7 +227,7 @@
if ( fph )
{
- printf("Saving convex decomposition of %d hulls to COLLADA file '%s'\r\n", mHulls.size(), scratch );
+ printf("Saving convex decomposition of %d hulls to COLLADA file '%s'\r\n", (int)mHulls.size(), scratch );
fprintf(fph,"<?xml version=\"1.0\" encoding=\"utf-8\"?>\r\n");
fprintf(fph,"<COLLADA version=\"1.4.0\" xmlns=\"http://www.collada.org/2005/11/COLLADASchema\">\r\n");
@@ -537,7 +537,7 @@
};
-void main(int argc,const char **argv)
+int main(int argc,const char **argv)
{
if ( argc < 2 )
{
Index: DecomposeSample.cpp
===================================================================
--- DecomposeSample.cpp (revision 8299)
+++ DecomposeSample.cpp (working copy)
@@ -67,3 +67,3 @@
strcpy(mBaseName,fname);
- char *dot = strstr(mBaseName,".");
+ char *dot = strstr(mBaseName,".obj");
if ( dot ) *dot = 0;
Index: ConvexDecomposition/cd_wavefront.cpp
===================================================================
--- ConvexDecomposition/cd_wavefront.cpp (revision 8299)
+++ ConvexDecomposition/cd_wavefront.cpp (working copy)
@@ -573,6 +573,7 @@
FloatVector mNormals;
GeometryInterface *mCallback;
+ friend class WavefrontObj;
};
@@ -839,7 +840,17 @@
memcpy(mIndices, &indices[0], sizeof(int)*mTriCount*3);
ret = mTriCount;
}
-
+ else if( obj.mVerts.size() > 0 ) {
+ // take consecutive vertices
+ mVertexCount = obj.mVerts.size()/3;
+ mVertices = new double[mVertexCount*3];
+ memcpy( mVertices, &obj.mVerts[0], sizeof(double)*mVertexCount*3 );
+ mTriCount = mVertexCount/3;
+ mIndices = new int[mTriCount*3*sizeof(int)];
+ for(int i = 0; i < mVertexCount; ++i)
+ mIndices[i] = i;
+ ret = mTriCount;
+ }
return ret;
}

14
convex_decomposition/manifest.xml Normal file
View File

@ -0,0 +1,14 @@
<package>
<description brief="Convex Mesh Generation Library">
Convex Mesh Generation Library
</description>
<author>John Ratcliff</author>
<license>MIT</license>
<review status="3rdparty" notes=""/>
<url>http://www.amillionpixels.us/ConvexDecomposition.zip</url>
<export>
<cpp lflags="-L${prefix}/ConvexDecomposition/lib " cflags="-I${prefix}/ConvexDecomposition/include"/>
</export>
</package>

9
ivcon/CMakeLists.txt Normal file
View File

@ -0,0 +1,9 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(ivcon)
#uncomment for profiling
set(ROS_LINK_FLAGS "-lm" ${ROS_LINK_FLAGS})
rospack_add_executable(bin/ivcon src/ivcon.c)

1
ivcon/Makefile Normal file
View File

@ -0,0 +1 @@
include $(shell rospack find mk)/cmake.mk

14
ivcon/manifest.xml Normal file
View File

@ -0,0 +1,14 @@
<package>
<description brief="Mesh Conversion Utility">
Mesh Conversion Utility
</description>
<author>John Burkardt</author>
<license>GPL</license>
<review status="3rdparty" notes=""/>
<url>https://sourceforge.net/projects/ivcon/</url>
<export>
<cpp lflags="" cflags=""/>
</export>
</package>

16707
ivcon/src/ivcon.c Normal file
View File

File diff suppressed because it is too large Load Diff

38
kdl_parser/CMakeLists.txt Normal file
View File

@ -0,0 +1,38 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
rosbuild_add_boost_directories()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#genmsg()
#uncomment if you have defined services
#gensrv()
rosbuild_add_library(${PROJECT_NAME} src/xml_parser.cpp src/dom_parser.cpp)
rosbuild_add_executable(example_xml test/example_xml.cpp )
target_link_libraries(example_xml ${PROJECT_NAME})
rosbuild_add_executable(example_dom test/example_dom.cpp )
target_link_libraries(example_dom ${PROJECT_NAME})
rosbuild_add_executable(test_parser test/test_kdl_parser.cpp )
target_link_libraries(test_parser ${PROJECT_NAME})
rosbuild_add_gtest_build_flags(test_parser)
rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_kdl_parser.launch)

1
kdl_parser/Makefile Normal file
View File

@ -0,0 +1 @@
include $(shell rospack find mk)/cmake.mk

54
kdl_parser/include/kdl_parser/dom_parser.hpp Normal file
View File

@ -0,0 +1,54 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef DOM_PARSER_H
#define DOM_PARSER_H
#include <kdl/tree.hpp>
#include <string>
#include <urdf/model.h>
using namespace std;
namespace kdl_parser{
bool treeFromFile(const string& file, KDL::Tree& tree);
bool treeFromString(const string& xml, KDL::Tree& tree);
bool treeFromXml(TiXmlDocument *xml_doc, KDL::Tree& tree);
bool treeFromRobotModel(urdf::Model& robot_model, KDL::Tree& tree);
}
#endif

53
kdl_parser/include/kdl_parser/xml_parser.hpp Normal file
View File

@ -0,0 +1,53 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef XML_PARSER_H
#define XML_PARSER_H
#include <kdl/tree.hpp>
#include <string>
#include <tinyxml/tinyxml.h>
using namespace std;
namespace KDL{
bool treeFromFile(const string& file, Tree& tree);
bool treeFromString(const string& xml, Tree& tree);
bool treeFromXml(TiXmlElement *root, Tree& tree);
}
#endif

23
kdl_parser/manifest.xml Normal file
View File

@ -0,0 +1,23 @@
<package>
<description brief="Package to parse urdf in to kdl tree">
The Kinematics and Dynamics Library (KDL) defines a tree structure to represent the kinematic and dynamic parameters of a robot mechanism. This package provides tools to construct a KDL tree from the robot representation in the urdf package.
</description>
<author>Wim Meeussen meeussen@willowgarage.com </author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/</url>
<depend package="kdl" />
<depend package="tinyxml" />
<depend package="urdf" />
<depend package="roscpp" />
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lkdl_parser"/>
</export>
</package>

2542
kdl_parser/pr2.urdf Normal file
View File

File diff suppressed because it is too large Load Diff

173
kdl_parser/src/dom_parser.cpp Normal file
View File

@ -0,0 +1,173 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include "kdl_parser/dom_parser.hpp"
#include <kdl/frames_io.hpp>
using namespace std;
using namespace KDL;
namespace kdl_parser{
// construct vector
Vector toKdl(urdf::Vector3 v)
{
return Vector(v.x, v.y, v.z);
}
// construct rotation
Rotation toKdl(urdf::Rotation r)
{
double roll, pitch, yaw;
r.getRPY(roll, pitch, yaw);
return Rotation::RPY(roll, pitch, yaw);
}
// construct pose
Frame toKdl(urdf::Pose p)
{
return Frame(toKdl(p.rotation), toKdl(p.position));
}
// construct joint
Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
{
Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
switch (jnt->type){
case urdf::Joint::FIXED:{
return Joint(jnt->name, Joint::None);
}
case urdf::Joint::REVOLUTE:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
}
case urdf::Joint::CONTINUOUS:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
}
case urdf::Joint::PRISMATIC:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
}
default:{
return Joint(jnt->name, Joint::None);
}
}
return Joint();
}
// construct inertia
RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i)
{
Frame origin = toKdl(i->origin);
return origin.M * RigidBodyInertia(i->mass, origin.p, RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz));
}
// recursive function to walk through tree
bool addChildrenToTree(boost::shared_ptr<const urdf::Link> root, Tree& tree)
{
std::vector<boost::shared_ptr<urdf::Link> > children = root->child_links;
ROS_DEBUG("Link %s had %i children", root->name.c_str(), children.size());
// constructs the optional inertia
RigidBodyInertia inert(0);
if (root->inertial)
inert = toKdl(root->inertial);
// constructs the kdl joint
Joint jnt = toKdl(root->parent_joint);
// construct the kdl segment
Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform));
// add segment to tree
tree.addSegment(sgm, root->parent_joint->parent_link_name);
// recurslively add all children
for (size_t i=0; i<children.size(); i++){
if (!addChildrenToTree(children[i], tree))
return false;
}
return true;
}
bool treeFromFile(const string& file, Tree& tree)
{
TiXmlDocument urdf_xml;
urdf_xml.LoadFile(file);
return treeFromXml(&urdf_xml, tree);
}
bool treeFromString(const string& xml, Tree& tree)
{
TiXmlDocument urdf_xml;
urdf_xml.Parse(xml.c_str());
return treeFromXml(&urdf_xml, tree);
}
bool treeFromXml(TiXmlDocument *xml_doc, Tree& tree)
{
urdf::Model robot_model;
if (!robot_model.initXml(xml_doc)){
ROS_ERROR("Could not generate robot model");
return false;
}
return treeFromRobotModel(robot_model, tree);
}
bool treeFromRobotModel(urdf::Model& robot_model, Tree& tree)
{
tree = Tree(robot_model.getRoot()->name);
// add all children
for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
return false;
return true;
}
}

365
kdl_parser/src/xml_parser.cpp Normal file
View File

@ -0,0 +1,365 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include <boost/algorithm/string.hpp>
#include "kdl_parser/xml_parser.hpp"
using namespace std;
namespace KDL{
bool isNumber(const char& c)
{
return (c=='1' || c=='2' ||c=='3' ||c=='4' ||c=='5' ||c=='6' ||c=='7' ||c=='8' ||c=='9' ||c=='0' ||c=='.' ||c=='-' ||c==' ');
}
bool isNumber(const std::string& s)
{
for (unsigned int i=0; i<s.size(); i++)
if (!isNumber(s[i])) return false;
return true;
}
bool getAtribute(TiXmlElement *xml, const string& name, string& attr)
{
if (!xml) return false;
const char *attr_char = xml->Attribute(name.c_str());
if (!attr_char){
cerr << "No " << name << " found in xml" << endl;
return false;
}
attr = string(attr_char);
return true;
}
bool getVector(TiXmlElement *vector_xml, const string& field, Vector& vector)
{
if (!vector_xml) return false;
string vector_str;
if (!getAtribute(vector_xml, field, vector_str))
return false;
std::vector<std::string> pieces;
boost::split( pieces, vector_str, boost::is_any_of(" "));
unsigned int pos=0;
for (unsigned int i = 0; i < pieces.size(); ++i){
if (pieces[i] != ""){
if (pos < 3){
if (!isNumber(pieces[i]))
{cerr << "This is not a valid number: '" << pieces[i] << "'" << endl; return false;}
vector(pos) = atof(pieces[i].c_str());
}
pos++;
}
}
if (pos != 3) {
cerr << "Vector did not contain 3 pieces:" << endl;
pos = 1;
for (unsigned int i = 0; i < pieces.size(); ++i){
if (pieces[i] != ""){
cerr << " " << pos << ": '" << pieces[i] << "'" << endl;
pos++;
}
}
return false;
}
return true;
}
bool getValue(TiXmlElement *value_xml, const string& field, double& value)
{
if (!value_xml) return false;
string value_str;
if (!getAtribute(value_xml, field, value_str)) return false;
if (!isNumber(value_str))
{cerr << "This is not a valid number: '" << value_str << "'" << endl; return false;}
value = atof(value_str.c_str());
return true;
}
bool getFrame(TiXmlElement *frame_xml, Frame& frame)
{
if (!frame_xml) return false;
Vector origin, rpy;
if (!getVector(frame_xml, "xyz", origin))
{cerr << "Frame does not have xyz" << endl; return false;}
if (!getVector(frame_xml, "rpy", rpy))
{cerr << "Frame does not have rpy" << endl; return false;}
frame = Frame(Rotation::RPY(rpy(0), rpy(1), rpy(2)), origin);
return true;
}
bool getRotInertia(TiXmlElement *rot_inertia_xml, RotationalInertia& rot_inertia)
{
if (!rot_inertia_xml) return false;
double Ixx=0, Iyy=0, Izz=0, Ixy=0, Ixz=0, Iyz=0;
if (!getValue(rot_inertia_xml, "ixx", Ixx)) return false;
if (!getValue(rot_inertia_xml, "iyy", Iyy)) return false;
if (!getValue(rot_inertia_xml, "izz", Izz)) return false;
if (!getValue(rot_inertia_xml, "ixy", Ixy)) return false;
if (!getValue(rot_inertia_xml, "ixz", Ixz)) return false;
if (!getValue(rot_inertia_xml, "iyz", Iyz)) return false;
rot_inertia = RotationalInertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz);
return true;
}
bool getInertia(TiXmlElement *inertia_xml, RigidBodyInertia& inertia)
{
if (!inertia_xml) return false;
Vector cog;
if (!getVector(inertia_xml->FirstChildElement("com"), "xyz", cog))
{cerr << "Inertia does not specify center of gravity" << endl; return false;}
double mass = 0.0;
if (!getValue(inertia_xml->FirstChildElement("mass"), "value", mass))
{cerr << "Inertia does not specify mass" << endl; return false;}
RotationalInertia rot_inertia;
if (!getRotInertia(inertia_xml->FirstChildElement("inertia"), rot_inertia))
{cerr << "Inertia does not specify rotational inertia" << endl; return false;}
inertia = RigidBodyInertia(mass, cog, rot_inertia);
return true;
}
bool getJoint(TiXmlElement *joint_xml, Joint& joint)
{
if (!joint_xml) return false;
// get joint name
string joint_name;
if (!getAtribute(joint_xml, "name", joint_name))
{cerr << "Joint does not have name" << endl; return false;}
// get joint type
string joint_type;
if (!getAtribute(joint_xml, "type", joint_type))
{cerr << "Joint does not have type" << endl; return false;}
if (joint_type == "revolute"){
Vector axis, origin;
// mandatory axis
if (!getVector(joint_xml->FirstChildElement("axis"), "xyz", axis))
{cerr << "Revolute joint does not spacify axis" << endl; return false;}
// optional origin
if (!getVector(joint_xml->FirstChildElement("anchor"), "xyz", origin))
origin = Vector::Zero();
joint = Joint(joint_name, origin, axis, Joint::RotAxis);
}
else if (joint_type == "prismatic"){
Vector axis, origin;
// mandatory axis
if (!getVector(joint_xml->FirstChildElement("axis"), "xyz", axis))
{cerr << "Prismatic joint does not spacify axis" << endl; return false;};
// optional origin
if (!getVector(joint_xml->FirstChildElement("anchor"), "xyz", origin))
origin = Vector::Zero();
joint = Joint(joint_name, origin, axis, Joint::TransAxis);
}
else if (joint_type == "fixed"){
joint = Joint(joint_name, Joint::None);
}
else{
cerr << "Unknown joint type '" << joint_type << "'. Using fixed joint instead" << endl;
joint = Joint(joint_name, Joint::None);
}
return true;
}
bool getSegment(TiXmlElement *segment_xml, map<string, Joint>& joints, Segment& segment)
{
if (!segment_xml) return false;
// get segment name
string segment_name;
if (!getAtribute(segment_xml, "name", segment_name))
{cerr << "Segment does not have name" << endl; return false;}
// get mandetory frame
Frame frame;
if (!getFrame(segment_xml->FirstChildElement("origin"), frame))
{cerr << "Segment does not have origin" << endl; return false;}
// get mandetory joint
string joint_name;
if (!getAtribute(segment_xml->FirstChildElement("joint"), "name", joint_name))
{cerr << "Segment does not specify joint name" << endl; return false;}
Joint joint;
map<string, Joint>::iterator it = joints.find(joint_name);
if (it != joints.end())
joint = it->second;
else if (getJoint(segment_xml->FirstChildElement("joint"), joint));
else {cerr << "Could not find joint " << joint_name << " in segment" << endl; return false;}
if (joint.getType() != Joint::None)
joint = Joint(joint_name, frame*(joint.JointOrigin()), joint.JointAxis(), joint.getType());
// get optional inertia
RigidBodyInertia inertia(0.0);
getInertia(segment_xml->FirstChildElement("inertial"), inertia);
segment = Segment(segment_name, joint, frame, inertia);
return true;
}
void addChildrenToTree(const string& root, const map<string, Segment>& segments, const map<string, string>& parents, Tree& tree)
{
// add root segments
if (tree.addSegment(segments.find(root)->second, parents.find(root)->second)){
cout << "Added segment " << root << " to " << parents.find(root)->second << endl;
// find children
for (map<string, string>::const_iterator p=parents.begin(); p!=parents.end(); p++)
if (p->second == root) addChildrenToTree(p->first, segments, parents, tree);
}
else {cerr << "Failed to add segment to tree" << endl;}
}
bool getTree(TiXmlElement *robot_xml, Tree& tree)
{
cout << "Parsing robot xml" << endl;
if (!robot_xml) return false;
// Constructs the joints
TiXmlElement *joint_xml = NULL;
Joint joint;
map<string, Joint> joints;
for (joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")){
// get joint name
string joint_name;
if (!getAtribute(joint_xml, "name", joint_name))
{cerr << "Joint does not have name" << endl; return false;}
// build joint
if (!getJoint(joint_xml, joint))
{cerr << "Constructing joint " << joint_name << " failed" << endl; return false;}
joints[joint.getName()] = joint;
}
// Constructs the segments
TiXmlElement *segment_xml = NULL;
Segment segment;
map<string, Segment> segments;
map<string, string> parents;
for (segment_xml = robot_xml->FirstChildElement("link"); segment_xml; segment_xml = segment_xml->NextSiblingElement("link")){
// get segment name
string segment_name;
if (!getAtribute(segment_xml, "name", segment_name))
{cerr << "Segment does not have name" << endl; return false;}
// get segment parent
string segment_parent;
if (!getAtribute(segment_xml->FirstChildElement("parent"), "name", segment_parent))
{cerr << "Segment " << segment_name << " does not have parent" << endl; return false;}
// build segment
if (!getSegment(segment_xml, joints, segment))
{cerr << "Constructing segment " << segment_name << " failed" << endl; return false;}
segments[segment.getName()] = segment;
parents[segment.getName()] = segment_parent;
}
// fail if no segments were found
if (segments.empty()){
cerr << "Did not find any segments" << endl;
return false;
}
// find the root segment: the parent segment that does not exist
string root;
for (map<string, string>::const_iterator p=parents.begin(); p!=parents.end(); p++)
if (segments.find(p->second) == segments.end())
root = p->first;
cout << parents[root] << " is root segment " << endl;
tree = Tree(parents[root]);
// add all segments to tree
addChildrenToTree(root, segments, parents, tree);
return true;
}
bool treeFromFile(const string& file, Tree& tree)
{
TiXmlDocument urdf_xml;
urdf_xml.LoadFile(file);
TiXmlElement *root = urdf_xml.FirstChildElement("robot");
if (!root){
cerr << "Could not parse the xml" << endl;
return false;
}
return getTree(root, tree);
}
bool treeFromString(const string& xml, Tree& tree)
{
TiXmlDocument urdf_xml;
urdf_xml.Parse(xml.c_str());
TiXmlElement *root = urdf_xml.FirstChildElement("robot");
if (!root){
cerr << "Could not parse the xml" << endl;
return false;
}
return getTree(root, tree);
}
bool treeFromXml(TiXmlElement *root, Tree& tree)
{
return getTree(root, tree);
}
}

105
kdl_parser/test/example_dom.cpp Normal file
View File

@ -0,0 +1,105 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include "kdl_parser/dom_parser.hpp"
#include <iostream>
using namespace KDL;
using namespace std;
using namespace urdf;
int main()
{
Model robot_model;
if (!robot_model.initFile("pr2.urdf"))
{cerr << "Could not generate robot model" << endl; return false;}
Tree my_tree;
if (!kdl_parser::treeFromRobotModel(robot_model, my_tree))
{cerr << "Could not extract kdl tree" << endl; return false;}
// walk through tree
cout << " ======================================" << endl;
SegmentMap::const_iterator root = my_tree.getRootSegment();
cout << "Found root segment '" << root->second.segment.getName() << "' with " << root->second.children.size() << " children" << endl;
for (unsigned int i=0; i<root->second.children.size(); i++){
SegmentMap::const_iterator child = root->second.children[i];
cout << " - child " << i+1 << ": " << child->second.segment.getName() << " has joint " << child->second.segment.getJoint().getName()
<< " and " << child->second.children.size() << " children" << endl;
for (unsigned int j=0; j<child->second.children.size(); j++){
SegmentMap::const_iterator grandchild = child->second.children[j];
cout << " - grandchild " << j+1 << ": " << grandchild->second.segment.getName() << " has joint " << grandchild->second.segment.getJoint().getName()
<< " and " << grandchild->second.children.size() << " children" << endl;
}
}
// extract chains from tree
Chain chain1, chain2;
my_tree.getChain("l_gripper_palm_link", "r_gripper_palm_link", chain1);
my_tree.getChain("r_gripper_palm_link", "l_gripper_palm_link", chain2);
cout << "Got chain1 with " << chain1.getNrOfJoints() << " joints and " << chain1.getNrOfSegments() << " segments" << endl;
cout << "Got chain2 with " << chain2.getNrOfJoints() << " joints and " << chain2.getNrOfSegments() << " segments" << endl;
JntArray jnt1(chain1.getNrOfJoints());
JntArray jnt2(chain1.getNrOfJoints());
for (int i=0; i<(int)chain1.getNrOfJoints(); i++){
jnt1(i) = (i+1)*2;
jnt2((int)chain1.getNrOfJoints()-i-1) = -jnt1(i);
}
for (int i=0; i<(int)chain1.getNrOfJoints(); i++)
cout << "jnt 1 -- jnt 2 " << jnt1(i) << " -- " << jnt2(i) << endl;
ChainFkSolverPos_recursive solver1(chain1);
ChainFkSolverPos_recursive solver2(chain2);
Frame f1, f2;
solver1.JntToCart(jnt1, f1);
solver2.JntToCart(jnt2, f2);
cout << "frame 1 " << f1 << endl;
cout << "frame 2 " << f2.Inverse() << endl;
// copy tree
Tree copy = my_tree;
copy.getChain("l_gripper_palm_link", "r_gripper_palm_link", chain1);
copy.getChain("r_gripper_palm_link", "l_gripper_palm_link", chain2);
cout << "Got chain1 with " << chain1.getNrOfJoints() << " joints and " << chain1.getNrOfSegments() << " segments" << endl;
cout << "Got chain2 with " << chain2.getNrOfJoints() << " joints and " << chain2.getNrOfSegments() << " segments" << endl;
}

99
kdl_parser/test/example_xml.cpp Normal file
View File

@ -0,0 +1,99 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include "kdl_parser/xml_parser.hpp"
#include <iostream>
using namespace KDL;
using namespace std;
int main()
{
Tree my_tree;
if (!treeFromFile("pr2_desc.xml", my_tree)) return -1;
// walk through tree
SegmentMap::const_iterator root = my_tree.getRootSegment();
cout << "Found root " << root->second.segment.getName() << " with " << root->second.children.size() << " children" << endl;
for (unsigned int i=0; i<root->second.children.size(); i++){
SegmentMap::const_iterator child = root->second.children[i];
cout << " - child " << i+1 << ": " << child->second.segment.getName() << " has joint " << child->second.segment.getJoint().getName()
<< " and " << child->second.children.size() << " children" << endl;
for (unsigned int j=0; j<child->second.children.size(); j++){
SegmentMap::const_iterator grandchild = child->second.children[j];
cout << " - grandchild " << j+1 << ": " << grandchild->second.segment.getName() << " has joint " << grandchild->second.segment.getJoint().getName()
<< " and " << grandchild->second.children.size() << " children" << endl;
}
}
// extract chains from tree
Chain chain1, chain2;
my_tree.getChain("l_gripper_palm_link", "r_gripper_palm_link", chain1);
my_tree.getChain("r_gripper_palm_link", "l_gripper_palm_link", chain2);
cout << "Got chain1 with " << chain1.getNrOfJoints() << " joints and " << chain1.getNrOfSegments() << " segments" << endl;
cout << "Got chain2 with " << chain2.getNrOfJoints() << " joints and " << chain2.getNrOfSegments() << " segments" << endl;
JntArray jnt1(chain1.getNrOfJoints());
JntArray jnt2(chain1.getNrOfJoints());
for (int i=0; i<(int)chain1.getNrOfJoints(); i++){
jnt1(i) = (i+1)*2;
jnt2((int)chain1.getNrOfJoints()-i-1) = -jnt1(i);
}
for (int i=0; i<(int)chain1.getNrOfJoints(); i++)
cout << "jnt 1 -- jnt 2 " << jnt1(i) << " -- " << jnt2(i) << endl;
ChainFkSolverPos_recursive solver1(chain1);
ChainFkSolverPos_recursive solver2(chain2);
Frame f1, f2;
solver1.JntToCart(jnt1, f1);
solver2.JntToCart(jnt2, f2);
cout << "frame 1 " << f1 << endl;
cout << "frame 2 " << f2.Inverse() << endl;
// copy tree
Tree copy = my_tree;
copy.getChain("l_gripper_palm_link", "r_gripper_palm_link", chain1);
copy.getChain("r_gripper_palm_link", "l_gripper_palm_link", chain2);
cout << "Got chain1 with " << chain1.getNrOfJoints() << " joints and " << chain1.getNrOfSegments() << " segments" << endl;
cout << "Got chain2 with " << chain2.getNrOfJoints() << " joints and " << chain2.getNrOfSegments() << " segments" << endl;
}

3541
kdl_parser/test/pr2_desc.xml Normal file
View File

File diff suppressed because it is too large Load Diff

22
kdl_parser/test/pr2_desc_bracket.xml Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0" ?>
<robot name="pr2">
<joint name="fl_caster_rotation_joint" type="revolute">
<axis xyz="0 0 1"/>
<anchor xyz="0 0 0"/>
<limit effort="100" k_velocity="10" velocity="100"/>
<calibration reference_position="0.0" values="1.5 -1"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="fl_caster_rotation_link">
<parent name="base_link"/>
<origin rpy="0 0 1 }" xyz="0.2225 0.2225 0.0282"/>
<joint name="fl_caster_rotation_joint"/>
<inertial>
<mass value="3.473082"/>
<com xyz="0 0 0.07"/>
<inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
</inertial>
</robot>

22
kdl_parser/test/pr2_desc_bracket2.xml Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0" ?>
<robot name="pr2">
<joint name="fl_caster_rotation_joint" type="revolute">
<axis xyz="0 0 1"/>
<anchor xyz="0 0 0"/>
<limit effort="100" k_velocity="10" velocity="100"/>
<calibration reference_position="0.0" values="1.5 -1"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="fl_caster_rotation_link">
<parent name="base_link"/>
<origin rpy="0 0 1}" xyz="0.2225 0.2225 0.0282"/>
<joint name="fl_caster_rotation_joint"/>
<inertial>
<mass value="3.473082"/>
<com xyz="0 0 0.07"/>
<inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
</inertial>
</robot>

22
kdl_parser/test/pr2_desc_vector.xml Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0" ?>
<robot name="pr2">
<joint name="fl_caster_rotation_joint" type="revolute">
<axis xyz="0 0 1"/>
<anchor xyz="0 0 0"/>
<limit effort="100" k_velocity="10" velocity="100"/>
<calibration reference_position="0.0" values="1.5 -1"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="fl_caster_rotation_link">
<parent name="base_link"/>
<origin rpy="0 0 1 9" xyz="0.2225 0.2225 0.0282"/>
<joint name="fl_caster_rotation_joint"/>
<inertial>
<mass value="3.473082"/>
<com xyz="0 0 0.07"/>
<inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
</inertial>
</robot>

93
kdl_parser/test/test_kdl_parser.cpp Normal file
View File

@ -0,0 +1,93 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include <string>
#include <gtest/gtest.h>
#include <ros/ros.h>
#include "kdl_parser/xml_parser.hpp"
using namespace KDL;
int g_argc;
char** g_argv;
class TestParser : public testing::Test
{
public:
Tree my_tree;
protected:
/// constructor
TestParser()
{
}
/// Destructor
~TestParser()
{
}
};
TEST_F(TestParser, test)
{
for (int i=1; i<g_argc-2; i++){
ASSERT_FALSE(treeFromFile(g_argv[i], my_tree));
}
ASSERT_TRUE(treeFromFile(g_argv[g_argc-1], my_tree));
ASSERT_TRUE(my_tree.getNrOfJoints() == 38);
ASSERT_TRUE(my_tree.getNrOfSegments() == 51);
ASSERT_TRUE(my_tree.getSegment("world") == my_tree.getRootSegment());
ASSERT_TRUE(my_tree.getRootSegment()->second.children.size() == 1);
ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment());
SUCCEED();
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test_kdl_parser");
g_argc = argc;
g_argv = argv;
return RUN_ALL_TESTS();
}

6
kdl_parser/test/test_kdl_parser.launch Normal file
View File

@ -0,0 +1,6 @@
<launch>
<test test-name="test_kdl_parser" pkg="kdl_parser" type="test_parser" args="$(find kdl_parser)/test/pr2_desc_vector.xml \
$(find kdl_parser)/test/pr2_desc_bracket.xml \
$(find kdl_parser)/test/pr2_desc_bracket2.xml \
$(find kdl_parser)/test/pr2_desc.xml" />
</launch>

35
resource_retriever/CMakeLists.txt Normal file
View File

@ -0,0 +1,35 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
include(FindCURL)
if(NOT CURL_FOUND)
message("CURL not found! Aborting...")
fail()
endif(NOT CURL_FOUND)
include_directories(${CURL_INCLUDE_DIRS})
rosbuild_add_library(${PROJECT_NAME} src/retriever.cpp)
target_link_libraries(${PROJECT_NAME} ${CURL_LIBRARIES})
add_subdirectory(test EXCLUDE_FROM_ALL)

1
resource_retriever/Makefile Normal file
View File

@ -0,0 +1 @@
include $(shell rospack find mk)/cmake.mk

85
resource_retriever/include/resource_retriever/retriever.h Normal file
View File

@ -0,0 +1,85 @@
/*
* Copyright (C) 2009, Willow Garage, Inc.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RESOURCE_RETRIEVER_RETRIEVER_H
#define RESOURCE_RETRIEVER_RETRIEVER_H
#include <string>
#include <boost/shared_array.hpp>
#include <stdexcept>
typedef void CURL;
namespace resource_retriever
{
class Exception : public std::runtime_error
{
public:
Exception(const std::string& file, const std::string& error_msg)
: std::runtime_error("Error retrieving file [" + file + "]: " + error_msg)
{}
};
/**
* \brief A combination of a pointer to data in memory along with the data's size.
*/
struct MemoryResource
{
MemoryResource()
: size(0)
{}
boost::shared_array<uint8_t> data;
uint32_t size;
};
/**
* \brief Retrieves files from from a url. Caches a CURL handle so multiple accesses to a single url
* will keep connections open.
*/
class Retriever
{
public:
Retriever();
~Retriever();
/**
* \brief Get a file and store it in memory
* \param url The url to retrieve. package://package/file will be turned into the correct file:// invocation
* \return The file, loaded into memory
* \throws resource_retriever::Exception if anything goes wrong.
*/
MemoryResource get(const std::string& url);
private:
CURL* curl_handle_;
};
} // namespace resource_retriever
#endif // RESOURCE_RETRIEVER_RETRIEVER_H

14
resource_retriever/mainpage.dox Normal file
View File

@ -0,0 +1,14 @@
/**
\mainpage
\htmlinclude manifest.html
\b resource_retriever is a package used for downloading files from a url. It also provides special handling of the package:// prefix, allowing things to be referenced
by path relative to a package.
\section codeapi Code API
- resource_retriever::Retriever -- Use this class to download files.
*/

21
resource_retriever/manifest.xml Normal file
View File

@ -0,0 +1,21 @@
<package>
<description brief="resource_retriever">
A package for retrieving resources used in a urdf. Uses libcurl to support url-format files, such as http://, ftp://, etc. Translates package:// to a local file://
</description>
<author>Josh Faust (jfaust@willowgarage.com)</author>
<license>BSD</license>
<review status="API reviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/resource_retriever</url>
<depend package="roslib"/>
<depend package="rosconsole"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -Wl,-rpath,${prefix}/lib -lresource_retriever"/>
</export>
<rosdep name="curl"/>
</package>

147
resource_retriever/src/retriever.cpp Normal file
View File

@ -0,0 +1,147 @@
/*
* Copyright (C) 2009, Willow Garage, Inc.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "resource_retriever/retriever.h"
#include <string.h>
#include <ros/package.h>
#include <ros/console.h>
#include <curl/curl.h>
namespace resource_retriever
{
class CURLStaticInit
{
public:
CURLStaticInit()
: initialized_(false)
{
CURLcode ret = curl_global_init(CURL_GLOBAL_ALL);
if (ret != 0)
{
ROS_ERROR("Error initializing libcurl! retcode = %d", ret);
}
else
{
initialized_ = true;
}
}
~CURLStaticInit()
{
if (initialized_)
{
curl_global_cleanup();
}
}
bool initialized_;
};
static CURLStaticInit g_curl_init;
Retriever::Retriever()
{
curl_handle_ = curl_easy_init();
}
Retriever::~Retriever()
{
if (curl_handle_)
{
curl_easy_cleanup(curl_handle_);
}
}
struct MemoryBuffer
{
std::vector<uint8_t> v;
};
size_t curlWriteFunc(void* buffer, size_t size, size_t nmemb, void* userp)
{
MemoryBuffer* membuf = (MemoryBuffer*)userp;
size_t prev_size = membuf->v.size();
membuf->v.resize(prev_size + size * nmemb);
memcpy(&membuf->v[prev_size], buffer, size * nmemb);
return size * nmemb;
}
MemoryResource Retriever::get(const std::string& url)
{
std::string mod_url = url;
if (url.find("package://") == 0)
{
mod_url.erase(0, strlen("package://"));
size_t pos = mod_url.find("/");
if (pos == std::string::npos)
{
throw Exception(url, "Could not parse package:// format into file:// format");
}
std::string package = mod_url.substr(0, pos);
mod_url.erase(0, pos);
std::string package_path = ros::package::getPath(package);
if (package_path.empty())
{
throw Exception(url, "Package [" + package + "] does not exist");
}
mod_url = "file://" + package_path + mod_url;
}
curl_easy_setopt(curl_handle_, CURLOPT_URL, mod_url.c_str());
curl_easy_setopt(curl_handle_, CURLOPT_WRITEFUNCTION, curlWriteFunc);
char error_buffer[CURL_ERROR_SIZE];
curl_easy_setopt(curl_handle_, CURLOPT_ERRORBUFFER , error_buffer);
MemoryResource res;
MemoryBuffer buf;
curl_easy_setopt(curl_handle_, CURLOPT_WRITEDATA, &buf);
CURLcode ret = curl_easy_perform(curl_handle_);
if (ret != 0)
{
throw Exception(mod_url, error_buffer);
}
else if (!buf.v.empty())
{
res.size = buf.v.size();
res.data.reset(new uint8_t[res.size]);
memcpy(res.data.get(), &buf.v[0], res.size);
}
return res;
}
}

4
resource_retriever/test/CMakeLists.txt Normal file
View File

@ -0,0 +1,4 @@
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
rospack_add_gtest(test/utest test.cpp)
target_link_libraries(test/utest ${PROJECT_NAME})

140
resource_retriever/test/test.cpp Normal file
View File

@ -0,0 +1,140 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <gtest/gtest.h>
#include <resource_retriever/retriever.h>
#include <ros/package.h>
#include <ros/console.h>
using namespace resource_retriever;
TEST(Retriever, getByPackage)
{
try
{
Retriever r;
MemoryResource res = r.get("package://"ROS_PACKAGE_NAME"/test/test.txt");
ASSERT_EQ(res.size, 1);
ASSERT_EQ(res.data[0], 'A');
}
catch (Exception& e)
{
FAIL();
}
}
TEST(Retriever, largeFile)
{
try
{
std::string path = ros::package::getPath(ROS_PACKAGE_NAME) + "/test/large_file.dat";
FILE* f = fopen(path.c_str(), "w");
ASSERT_TRUE(f);
for (int i = 0; i < 1024*1024*50; ++i)
{
fprintf(f, "A");
}
fclose(f);
Retriever r;
MemoryResource res = r.get("package://"ROS_PACKAGE_NAME"/test/large_file.dat");
ASSERT_EQ(res.size, 1024*1024*50);
}
catch (Exception& e)
{
FAIL();
}
}
TEST(Retriever, http)
{
try
{
Retriever r;
MemoryResource res = r.get("http://pr.willowgarage.com/downloads/svnmerge.py");
ASSERT_GT(res.size, 0);
}
catch (Exception& e)
{
FAIL();
}
}
TEST(Retriever, invalidFiles)
{
Retriever r;
try
{
r.get("file://fail");
FAIL();
}
catch (Exception& e)
{
ROS_INFO("%s", e.what());
}
try
{
r.get("package://roscpp");
FAIL();
}
catch (Exception& e)
{
ROS_INFO("%s", e.what());
}
try
{
r.get("package://invalid_package_blah/test.xml");
FAIL();
}
catch (Exception& e)
{
ROS_INFO("%s", e.what());
}
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

1
resource_retriever/test/test.txt Normal file
View File

@ -0,0 +1 @@
A

38
robot_state_publisher/CMakeLists.txt Normal file
View File

@ -0,0 +1,38 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
rosbuild_add_boost_directories()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#genmsg()
#uncomment if you have defined services
#gensrv()
rosbuild_add_library(${PROJECT_NAME} src/joint_state_listener.cpp src/robot_state_publisher.cpp src/treefksolverposfull_recursive.cpp)
rosbuild_add_executable(state_publisher src/state_publisher.cpp )
target_link_libraries(state_publisher ${PROJECT_NAME})
rosbuild_add_executable(test_publisher test/test_publisher.cpp )
target_link_libraries(test_publisher ${PROJECT_NAME})
rosbuild_add_gtest_build_flags(test_publisher)
rosbuild_declare_test(test_publisher)
rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_publisher.launch)
# Download needed data file
rosbuild_download_test_data(http://pr.willowgarage.com/data/robot_state_publisher/joint_states.bag test/joint_states.bag)

1
robot_state_publisher/Makefile Normal file
View File

@ -0,0 +1 @@
include $(shell rospack find mk)/cmake.mk

69
robot_state_publisher/include/robot_state_publisher/joint_state_listener.h Normal file
View File

@ -0,0 +1,69 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef JOINT_STATE_LISTENER_H
#define JOINT_STATE_LISTENER_H
#include <kdl/tree.hpp>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include "robot_state_publisher/robot_state_publisher.h"
using namespace std;
using namespace ros;
using namespace KDL;
typedef boost::shared_ptr<sensor_msgs::JointState const> JointStateConstPtr;
namespace robot_state_publisher{
class JointStateListener{
public:
JointStateListener(const KDL::Tree& tree);
~JointStateListener();
private:
void callbackJointState(const JointStateConstPtr& state);
NodeHandle n_;
Rate publish_rate_;
robot_state_publisher::RobotStatePublisher state_publisher_;
Subscriber joint_state_sub_;
};
}
#endif

76
robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h Normal file
View File

@ -0,0 +1,76 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef ROBOT_STATE_PUBLISHER_H
#define ROBOT_STATE_PUBLISHER_H
#include <ros/ros.h>
#include <boost/scoped_ptr.hpp>
#include <tf/tf.h>
#include <tf/tfMessage.h>
#include "robot_state_publisher/treefksolverposfull_recursive.hpp"
namespace robot_state_publisher{
class RobotStatePublisher
{
public:
RobotStatePublisher(const KDL::Tree& tree);
~RobotStatePublisher(){};
bool publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time);
private:
ros::NodeHandle n_;
ros::Publisher tf_publisher_;
KDL::Tree tree_;
boost::scoped_ptr<KDL::TreeFkSolverPosFull_recursive> solver_;
std::string root_;
std::string tf_prefix_;
tf::tfMessage tf_msg_;
class empty_tree_exception: public std::exception{
virtual const char* what() const throw(){
return "Tree is empty";}
} empty_tree_ex;
};
}
#endif

48
robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp Normal file
View File

@ -0,0 +1,48 @@
// Copyright (C) 2009 Willow Garage Inc
// Version: 1.0
// Author: Wim Meeussen <meeussen at willowgarage dot com>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDLTREEFKSOLVERPOSFULL_RECURSIVE_HPP
#define KDLTREEFKSOLVERPOSFULL_RECURSIVE_HPP
#include <kdl/tree.hpp>
namespace KDL {
class TreeFkSolverPosFull_recursive
{
public:
TreeFkSolverPosFull_recursive(const Tree& _tree);
~TreeFkSolverPosFull_recursive();
int JntToCart(const std::map<std::string, double>& q_in, std::map<std::string, Frame>& p_out);
private:
void addFrameToMap(const std::map<std::string, double>& q_in, std::map<std::string, Frame>& p_out,
const Frame& previous_frame, const SegmentMap::const_iterator this_segment);
Tree tree;
};
}
#endif

27
robot_state_publisher/manifest.xml Normal file
View File

@ -0,0 +1,27 @@
<package>
<description brief="This package allows you to publish the state of a robot to the transform library topic">
This package allows you to publish the state of a robot to the transform library topic. Once the state
gets published, it is available to all components in the system using the transform library.
The package takes the joint angles of the robot as input and publishes the 3D poses
of the robot links, using a kinematic tree model of the robot. The package can both be used
as a library and as a ROS node.
</description>
<author>Wim Meeussen meeussen@willowgarage.com </author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/robot_state_publisher</url>
<depend package="kdl_parser" />
<depend package="sensor_msgs" />
<depend package="roscpp" />
<depend package="tf" />
<depend package="tf_conversions" />
<export>
<cpp cflags="-I${prefix}/include/"/>
</export>
</package>

86
robot_state_publisher/src/joint_state_listener.cpp Normal file
View File

@ -0,0 +1,86 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include <kdl/tree.hpp>
#include <ros/ros.h>
#include "robot_state_publisher/robot_state_publisher.h"
#include "robot_state_publisher/joint_state_listener.h"
using namespace std;
using namespace ros;
using namespace KDL;
using namespace robot_state_publisher;
JointStateListener::JointStateListener(const KDL::Tree& tree)
: publish_rate_(0.0), state_publisher_(tree)
{
// set publish frequency
double publish_freq;
n_.param("~publish_frequency", publish_freq, 50.0);
publish_rate_ = Rate(publish_freq);
// subscribe to mechanism state
string joint_state_topic;
n_.param("~joint_state_topic", joint_state_topic, string("joint_states"));
joint_state_sub_ = n_.subscribe(joint_state_topic, 1, &JointStateListener::callbackJointState, this);;
};
JointStateListener::~JointStateListener()
{};
void JointStateListener::callbackJointState(const JointStateConstPtr& state)
{
if (state->get_name_size() == 0){
ROS_ERROR("Robot state publisher received an empty joint state vector");
return;
}
if (state->get_name_size() != state->get_position_size()){
ROS_ERROR("Robot state publisher received an invalid joint state vector");
return;
}
// get joint positions from state message
map<string, double> joint_positions;
for (unsigned int i=0; i<state->name.size(); i++)
joint_positions.insert(make_pair(state->name[i], state->position[i]));
state_publisher_.publishTransforms(joint_positions, state->header.stamp);
publish_rate_.sleep();
}

106
robot_state_publisher/src/robot_state_publisher.cpp Normal file
View File

@ -0,0 +1,106 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include "robot_state_publisher/robot_state_publisher.h"
#include <kdl/frames_io.hpp>
#include <tf_conversions/tf_kdl.h>
using namespace std;
using namespace ros;
using namespace KDL;
namespace robot_state_publisher{
RobotStatePublisher::RobotStatePublisher(const Tree& tree)
:tree_(tree)
{
// get tf prefix
n_.param("~tf_prefix", tf_prefix_, string());
// build tree solver
solver_.reset(new TreeFkSolverPosFull_recursive(tree_));
// advertise tf message
tf_publisher_ = n_.advertise<tf::tfMessage>("/tf_message", 5);
tf_msg_.transforms.resize(tree.getNrOfSegments()-1);
// get the 'real' root segment of the tree, which is the first child of "root"
SegmentMap::const_iterator root = tree.getRootSegment();
if (root->second.children.empty())
throw empty_tree_ex;
root_ = (*root->second.children.begin())->first;
}
bool RobotStatePublisher::publishTransforms(const map<string, double>& joint_positions, const Time& time)
{
// calculate transforms form root to every segment in tree
map<string, Frame> link_poses;
solver_->JntToCart(joint_positions, link_poses);
// publish the transforms to tf, converting the transforms from "root" to the 'real' root
geometry_msgs::TransformStamped trans;
map<string, Frame>::const_iterator root = link_poses.find(root_);
if (root == link_poses.end()){
ROS_ERROR("Did not find root of tree");
return false;
}
// remove root from published poses
Frame offset = root->second.Inverse();
unsigned int i = 0;
// send transforms to tf
for (map<string, Frame>::const_iterator f=link_poses.begin(); f!=link_poses.end(); f++){
if (f != root){
Frame frame = offset * f->second;
tf::Transform tf_frame;
tf::TransformKDLToTF(frame, tf_frame);
trans.header.stamp = time;
trans.header.frame_id = tf::remap(tf_prefix_, root->first);
trans.child_frame_id = tf::remap(tf_prefix_, f->first);
tf::transformTFToMsg(tf_frame, trans.transform);
tf_msg_.transforms[i++] = trans;
}
}
tf_publisher_.publish(tf_msg_);
return true;
}
}

77
robot_state_publisher/src/state_publisher.cpp Normal file
View File

@ -0,0 +1,77 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include <kdl/tree.hpp>
#include <ros/ros.h>
#include <kdl_parser/dom_parser.hpp>
#include <urdf/model.h>
#include "robot_state_publisher/joint_state_listener.h"
using namespace std;
using namespace ros;
using namespace KDL;
using namespace robot_state_publisher;
// ----------------------------------
// ----- MAIN -----------------------
// ----------------------------------
int main(int argc, char** argv)
{
// Initialize ros
ros::init(argc, argv, "robot_state_publisher");
NodeHandle node;
// gets the location of the robot description on the parameter server
string param_name, full_param_name;
node.param("~/robot_desc_param", param_name, string("robot_description"));
node.searchParam(param_name,full_param_name);
string robot_desc;
// constructs a kdl tree from the robot model
node.param(full_param_name, robot_desc, string());
Tree tree;
if (!kdl_parser::treeFromString(robot_desc, tree)){
ROS_ERROR("Failed to extract kdl tree from xml robot description");
return -1;
}
JointStateListener state_publisher(tree);
ros::spin();
return 0;
}

76
robot_state_publisher/src/treefksolverposfull_recursive.cpp Normal file
View File

@ -0,0 +1,76 @@
// Copyright (C) 2009 Willow Garage Inc
// Version: 1.0
// Author: Wim Meeussen <meeussen at willowgarage dot com>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#include "robot_state_publisher/treefksolverposfull_recursive.hpp"
#include <iostream>
using namespace std;
namespace KDL {
TreeFkSolverPosFull_recursive::TreeFkSolverPosFull_recursive(const Tree& _tree):
tree(_tree)
{
}
TreeFkSolverPosFull_recursive::~TreeFkSolverPosFull_recursive()
{
}
int TreeFkSolverPosFull_recursive::JntToCart(const map<string, double>& q_in, map<string, Frame>& p_out)
{
// clear output
p_out.clear();
addFrameToMap(q_in, p_out, Frame::Identity(), tree.getRootSegment());
return 0;
}
void TreeFkSolverPosFull_recursive::addFrameToMap(const map<string, double>& q_in, map<string, Frame>& p_out,
const Frame& previous_frame, const SegmentMap::const_iterator this_segment)
{
// get pose of this segment
Frame this_frame = previous_frame;
double jnt_p = 0;
if (this_segment->second.segment.getJoint().getType() != Joint::None){
map<string, double>::const_iterator jnt_pos = q_in.find(this_segment->second.segment.getJoint().getName());
if (jnt_pos == q_in.end()){
printf("Could not find value for joint %s\n", this_segment->first.c_str());
return;
}
jnt_p = jnt_pos->second;
}
this_frame = this_frame * this_segment->second.segment.pose(jnt_p);
if (this_segment->first != tree.getRootSegment()->first)
p_out.insert(make_pair(this_segment->first, this_frame));
// get poses of child segments
for (vector<SegmentMap::const_iterator>::const_iterator child=this_segment->second.children.begin(); child !=this_segment->second.children.end(); child++)
addFrameToMap(q_in, p_out, this_frame, *child);
}
}

3117
robot_state_publisher/test/pr2.urdf Normal file
View File

File diff suppressed because it is too large Load Diff

136
robot_state_publisher/test/test_publisher.cpp Normal file
View File

@ -0,0 +1,136 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include <string>
#include <gtest/gtest.h>
#include <ros/node.h>
#include <tf/transform_listener.h>
#include <boost/thread/thread.hpp>
#include <urdf/model.h>
#include <kdl_parser/dom_parser.hpp>
#include "robot_state_publisher/joint_state_listener.h"
using namespace ros;
using namespace tf;
using namespace robot_state_publisher;
int g_argc;
char** g_argv;
#define EPS 0.01
class TestPublisher : public testing::Test
{
public:
JointStateListener* publisher;
protected:
/// constructor
TestPublisher()
{
// constructs a robot model from the xml file
urdf::Model robot_model;
if (g_argc == 2){
if (!robot_model.initFile(g_argv[1]))
ROS_ERROR("Failed to construct robot model from xml string");
}
else
ROS_ERROR("No robot model as argument given");
// constructs a kdl tree from the robot model
Tree tree;
if (!kdl_parser::treeFromRobotModel(robot_model, tree))
ROS_ERROR("Failed to extract kdl tree from robot model");
publisher = new JointStateListener(tree);
}
/// Destructor
~TestPublisher()
{
delete publisher;
}
};
TEST_F(TestPublisher, test)
{
ROS_INFO("Creating tf listener");
TransformListener tf;
ROS_INFO("Waiting for bag to complete");
Duration(15.0).sleep();
ASSERT_TRUE(tf.canTransform("base_link", "torso_lift_link", Time()));
ASSERT_TRUE(tf.canTransform("base_link", "r_gripper_palm_link", Time()));
ASSERT_TRUE(tf.canTransform("base_link", "r_gripper_palm_link", Time()));
ASSERT_TRUE(tf.canTransform("l_gripper_palm_link", "r_gripper_palm_link", Time()));
ASSERT_TRUE(tf.canTransform("l_gripper_palm_link", "fl_caster_r_wheel_link", Time()));
ASSERT_FALSE(tf.canTransform("base_link", "wim_link", Time()));
tf::Stamped<tf::Transform> t;
tf.lookupTransform("base_link", "r_gripper_palm_link",Time(), t );
EXPECT_NEAR(t.getOrigin().x(), 0.769198, EPS);
EXPECT_NEAR(t.getOrigin().y(), -0.188800, EPS);
EXPECT_NEAR(t.getOrigin().z(), 0.764914, EPS);
tf.lookupTransform("l_gripper_palm_link", "r_gripper_palm_link",Time(), t );
EXPECT_NEAR(t.getOrigin().x(), 0.000384222, EPS);
EXPECT_NEAR(t.getOrigin().y(), -0.376021, EPS);
EXPECT_NEAR(t.getOrigin().z(), -1.0858e-05, EPS);
SUCCEED();
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test_robot_state_publisher");
ros::NodeHandle node;
boost::thread ros_thread(boost::bind(&ros::spin));
g_argc = argc;
g_argv = argv;
return RUN_ALL_TESTS();
}

5
robot_state_publisher/test/test_publisher.launch Normal file
View File

@ -0,0 +1,5 @@
<launch>
<node pkg="rosrecord" type="rosplay" args="$(find robot_state_publisher)/test/joint_states.bag" />
<test test-name="test_publisher" pkg="robot_state_publisher" type="test_publisher" args="$(find robot_state_publisher)/test/pr2.urdf" />
</launch>

15
stack.xml Normal file
View File

@ -0,0 +1,15 @@
<stack name="robot_model" version="0.1">
<description brief="robot_model packages from ros-pkg">
These are robot_model-related packages.
</description>
<author>John Hsu johnhsu@willowgarage.com</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/robot_model</url>
<depend stack="common_msgs"/> <!-- sensor_msgs -->
<depend stack="common"/> <!-- tinyxml -->
<depend stack="ros"/> <!-- rosconsole -->
<depend stack="geometry"/> <!-- kdl tf tf_conversions -->
</stack>

36
urdf/CMakeLists.txt Normal file
View File

@ -0,0 +1,36 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
set(ROS_BUILD_TYPE Debug)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#genmsg()
rosbuild_gensrv()
#common commands for building c++ executables and libraries
rosbuild_add_library(${PROJECT_NAME} src/link.cpp src/joint.cpp src/model.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
rosbuild_add_executable(parse_test test/parse_test.cpp)
target_link_libraries(parse_test ${PROJECT_NAME})
rosbuild_add_executable(test_parser EXCLUDE_FROM_ALL test/test_robot_model_parser.cpp)
rosbuild_add_gtest_build_flags(test_parser)
target_link_libraries(test_parser ${PROJECT_NAME})
rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_robot_model_parser.launch)

1
urdf/Makefile Normal file
View File

@ -0,0 +1 @@
include $(shell rospack find mk)/cmake.mk

96
urdf/include/urdf/color.h Normal file
View File

@ -0,0 +1,96 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Josh Faust */
#ifndef URDF_COLOR_H
#define URDF_COLOR_H
#include <string>
#include <vector>
#include <math.h>
#include <ros/ros.h>
#include <boost/algorithm/string.hpp>
namespace urdf
{
class Color
{
public:
Color() {this->clear();};
float r;
float g;
float b;
float a;
void clear()
{
r = g = b = 0.0f;
a = 1.0f;
}
bool init(const std::string &vector_str)
{
this->clear();
std::vector<std::string> pieces;
std::vector<float> rgba;
boost::split( pieces, vector_str, boost::is_any_of(" "));
for (unsigned int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
///@todo: do better atof check if string is a number
rgba.push_back(atof(pieces[i].c_str()));
}
}
if (rgba.size() != 4)
{
ROS_ERROR("Color contains %i elements instead of 4 elements", (int)rgba.size());
return false;
}
this->r = rgba[0];
this->g = rgba[1];
this->b = rgba[2];
this->a = rgba[3];
return true;
};
};
}
#endif

204
urdf/include/urdf/joint.h Normal file
View File

@ -0,0 +1,204 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef URDF_JOINT_H
#define URDF_JOINT_H
#include <string>
#include <vector>
#include <tinyxml/tinyxml.h>
#include <boost/shared_ptr.hpp>
#include "pose.h"
namespace urdf{
class Link;
class JointDynamics
{
public:
JointDynamics() { this->clear(); };
double damping;
double friction;
void clear()
{
damping = 0;
friction = 0;
};
bool initXml(TiXmlElement* config);
};
class JointLimits
{
public:
JointLimits() { this->clear(); };
double lower;
double upper;
double effort;
double velocity;
void clear()
{
lower = 0;
upper = 0;
effort = 0;
velocity = 0;
};
bool initXml(TiXmlElement* config);
};
class JointSafety
{
public:
JointSafety() { this->clear(); };
double soft_upper_limit;
double soft_lower_limit;
double k_position;
double k_velocity;
void clear()
{
soft_upper_limit = 0;
soft_lower_limit = 0;
k_position = 0;
k_velocity = 0;
};
bool initXml(TiXmlElement* config);
};
class JointCalibration
{
public:
JointCalibration() { this->clear(); };
double reference_position;
void clear()
{
reference_position = 0;
};
bool initXml(TiXmlElement* config);
};
class JointMimic
{
public:
JointMimic() { this->clear(); };
double offset;
double multiplier;
std::string joint_name;
void clear()
{
offset = 0;
multiplier = 0;
joint_name.clear();
};
bool initXml(TiXmlElement* config);
};
class Joint
{
public:
Joint() { this->clear(); };
std::string name;
enum
{
UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED
} type;
/// \brief type_ meaning of axis_
/// ------------------------------------------------------
/// UNKNOWN unknown type
/// REVOLUTE rotation axis
/// PRISMATIC translation axis
/// FLOATING N/A
/// PLANAR plane normal axis
/// FIXED N/A
Vector3 axis;
/// child Link element
/// child link frame is the same as the Joint frame
std::string child_link_name;
/// parent Link element
/// origin specifies the transform from Parent Link to Joint Frame
std::string parent_link_name;
/// transform from Parent Link frame to Joint frame
Pose parent_to_joint_origin_transform;
/// @todo: should use weak pointer here
//boost::shared_ptr<Link> link;
//boost::shared_ptr<Link> parent_link;
/// Joint Dynamics
boost::shared_ptr<JointDynamics> dynamics;
/// Joint Limits
boost::shared_ptr<JointLimits> limits;
/// Unsupported Hidden Feature
boost::shared_ptr<JointSafety> safety;
/// Unsupported Hidden Feature
boost::shared_ptr<JointCalibration> calibration;
/// Option to Mimic another Joint
boost::shared_ptr<JointMimic> mimic;
bool initXml(TiXmlElement* xml);
void clear()
{
this->axis.clear();
this->child_link_name.clear();
this->parent_link_name.clear();
this->parent_to_joint_origin_transform.clear();
this->dynamics.reset();
this->limits.reset();
this->safety.reset();
this->calibration.reset();
this->type = UNKNOWN;
};
};
}
#endif

238
urdf/include/urdf/link.h Normal file
View File

@ -0,0 +1,238 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef URDF_LINK_H
#define URDF_LINK_H
#include <string>
#include <vector>
#include <tinyxml/tinyxml.h>
#include <boost/shared_ptr.hpp>
#include "joint.h"
#include "color.h"
namespace urdf{
class Geometry
{
public:
enum {SPHERE, BOX, CYLINDER, MESH} type;
virtual bool initXml(TiXmlElement *) = 0;
};
class Sphere : public Geometry
{
public:
Sphere() { this->clear(); };
double radius;
void clear()
{
radius = 0;
};
bool initXml(TiXmlElement *);
};
class Box : public Geometry
{
public:
Box() { this->clear(); };
Vector3 dim;
void clear()
{
dim.clear();
};
bool initXml(TiXmlElement *);
};
class Cylinder : public Geometry
{
public:
Cylinder() { this->clear(); };
double length;
double radius;
void clear()
{
length = 0;
radius = 0;
};
bool initXml(TiXmlElement *);
};
class Mesh : public Geometry
{
public:
Mesh() { this->clear(); };
std::string filename;
Vector3 scale;
void clear()
{
filename.clear();
// default scale
scale.x = 1;
scale.y = 1;
scale.z = 1;
};
bool initXml(TiXmlElement *);
};
class Material
{
public:
Material() { this->clear(); };
std::string name;
std::string texture_filename;
Color color;
void clear()
{
color.clear();
texture_filename.clear();
name.clear();
};
bool initXml(TiXmlElement* config);
};
class Inertial
{
public:
Inertial() { this->clear(); };
Pose origin;
double mass;
double ixx,ixy,ixz,iyy,iyz,izz;
void clear()
{
origin.clear();
mass = 0;
ixx = ixy = ixz = iyy = iyz = izz = 0;
};
bool initXml(TiXmlElement* config);
};
class Visual
{
public:
Visual() { this->clear(); };
Pose origin;
boost::shared_ptr<Geometry> geometry;
std::string material_name;
boost::shared_ptr<Material> material;
void clear()
{
origin.clear();
material_name.clear();
material.reset();
geometry.reset();
};
bool initXml(TiXmlElement* config);
};
class Collision
{
public:
Collision() { this->clear(); };
Pose origin;
boost::shared_ptr<Geometry> geometry;
void clear()
{
origin.clear();
geometry.reset();
};
bool initXml(TiXmlElement* config);
};
class Link
{
public:
Link() { this->clear(); };
std::string name;
/// inertial element
boost::shared_ptr<Inertial> inertial;
/// visual element
boost::shared_ptr<Visual> visual;
/// collision element
boost::shared_ptr<Collision> collision;
/// Parent Joint element
/// explicitly stating "parent" because we want directional-ness for tree structure
/// every link can have one parent
boost::shared_ptr<Joint> parent_joint;
/// Get Parent Link throught the Parent Joint
boost::shared_ptr<Link> parent_link;
std::vector<boost::shared_ptr<Joint> > child_joints;
std::vector<boost::shared_ptr<Link> > child_links;
bool initXml(TiXmlElement* config);
void setParent(boost::shared_ptr<Link> parent);
void clear()
{
this->name.clear();
this->inertial.reset();
this->visual.reset();
this->collision.reset();
this->parent_joint.reset();
this->parent_link.reset();
this->child_joints.clear();
this->child_links.clear();
};
void setParentJoint(boost::shared_ptr<Joint> child);
void addChild(boost::shared_ptr<Link> child);
void addChildJoint(boost::shared_ptr<Joint> child);
};
}
#endif

111
urdf/include/urdf/model.h Normal file
View File

@ -0,0 +1,111 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef ROBOT_MODEL_PARSER_H
#define ROBOT_MODEL_PARSER_H
#include <string>
#include <map>
#include <tinyxml/tinyxml.h>
#include <boost/function.hpp>
#include "link.h"
namespace urdf{
class Model
{
public:
Model();
bool initXml(TiXmlElement *xml);
bool initXml(TiXmlDocument *xml);
bool initFile(const std::string& filename);
bool initString(const std::string& xmlstring);
boost::shared_ptr<const Link> getRoot(void) const{return this->root_link_;};
boost::shared_ptr<const Link> getLink(const std::string& name) const;
boost::shared_ptr<const Joint> getJoint(const std::string& name) const;
const std::string& getName() const {return name_;};
void getLinks(std::vector<boost::shared_ptr<Link> >& links) const;
/// Some accessor functions
boost::shared_ptr<const Link> getParentLink(const std::string& name) const;
boost::shared_ptr<const Joint> getParentJoint(const std::string& name) const;
boost::shared_ptr<const Link> getChildLink(const std::string& name) const;
boost::shared_ptr<const Joint> getChildJoint(const std::string& name) const;
/// Every Robot Description File can be described as a
/// list of Links and Joints
/// The connection between links(nodes) and joints(edges)
/// should define a tree (i.e. 1 parent link, 0+ children links)
std::map<std::string, boost::shared_ptr<Link> > links_;
std::map<std::string, boost::shared_ptr<Joint> > joints_;
std::map<std::string, boost::shared_ptr<Material> > materials_;
private:
void clear();
std::string name_;
/// non-const getLink()
void getLink(const std::string& name,boost::shared_ptr<Link> &link) const;
/// non-const getMaterial()
boost::shared_ptr<Material> getMaterial(const std::string& name) const;
/// in initXml(), onece all links are loaded,
/// it's time to build a tree
bool initTree(std::map<std::string, std::string> &parent_link_tree);
/// in initXml(), onece tree is built,
/// it's time to find the root Link
bool initRoot(std::map<std::string, std::string> &parent_link_tree);
/// Model is restricted to a tree for now, which means there exists one root link
/// typically, root link is the world(inertial). Where world is a special link
/// or is the root_link_ the link attached to the world by PLANAR/FLOATING joint?
/// hmm...
boost::shared_ptr<Link> root_link_;
};
}
#endif

232
urdf/include/urdf/pose.h Normal file
View File

@ -0,0 +1,232 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef URDF_POSE_H
#define URDF_POSE_H
#include <string>
#include <vector>
#include <math.h>
#include <ros/ros.h>
#include <boost/algorithm/string.hpp>
namespace urdf{
class Vector3
{
public:
Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;};
Vector3() {this->clear();};
double x;
double y;
double z;
void clear() {this->x=this->y=this->z=0.0;};
bool init(const std::string &vector_str)
{
this->clear();
std::vector<std::string> pieces;
std::vector<double> xyz;
boost::split( pieces, vector_str, boost::is_any_of(" "));
for (unsigned int i = 0; i < pieces.size(); ++i){
if (pieces[i] != ""){
///@todo: do better atof check if string is a number
xyz.push_back(atof(pieces[i].c_str()));
}
}
if (xyz.size() != 3) {
ROS_ERROR("Vector contains %i elements instead of 3 elements", (int)xyz.size());
return false;
}
this->x = xyz[0];
this->y = xyz[1];
this->z = xyz[2];
return true;
};
};
class Rotation
{
public:
Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;};
Rotation() {this->clear();};
void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w)
{
quat_x = this->x;
quat_y = this->y;
quat_z = this->z;
quat_w = this->w;
};
void getRPY(double &roll,double &pitch,double &yaw)
{
double sqw;
double sqx;
double sqy;
double sqz;
sqx = this->x * this->x;
sqy = this->y * this->y;
sqz = this->z * this->z;
sqw = this->w * this->w;
roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
pitch = asin(-2 * (this->x*this->z - this->w*this->y));
yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz);
};
void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w)
{
this->x = quat_x;
this->y = quat_y;
this->z = quat_z;
this->w = quat_w;
this->normalize();
};
void setFromRPY(double roll, double pitch, double yaw)
{
double phi, the, psi;
phi = roll / 2.0;
the = pitch / 2.0;
psi = yaw / 2.0;
this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi);
this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi);
this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi);
this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi);
this->normalize();
};
double x,y,z,w;
bool init(const std::string &rotation_str)
{
this->clear();
Vector3 rpy;
if (!rpy.init(rotation_str))
return false;
else
{
this->setFromRPY(rpy.x,rpy.y,rpy.z);
return true;
}
};
void clear() { this->x=this->y=this->z=0.0;this->w=1.0; }
void normalize()
{
double s = sqrt(this->x * this->x +
this->y * this->y +
this->z * this->z +
this->w * this->w);
this->x /= s;
this->y /= s;
this->z /= s;
this->w /= s;
};
};
class Pose
{
public:
Pose() { this->clear(); };
Vector3 position;
Rotation rotation;
void clear()
{
this->position.clear();
this->rotation.clear();
};
bool initXml(TiXmlElement* xml)
{
this->clear();
if (!xml)
{
ROS_DEBUG("parsing pose: xml empty");
return false;
}
else
{
const char* xyz_str = xml->Attribute("xyz");
if (xyz_str == NULL)
{
ROS_DEBUG("parsing pose: no xyz, using default values.");
return true;
}
else
{
if (!this->position.init(xyz_str))
{
ROS_ERROR("malformed xyz");
this->position.clear();
return false;
}
}
const char* rpy_str = xml->Attribute("rpy");
if (rpy_str == NULL)
{
ROS_DEBUG("parsing pose: no rpy, using default values.");
return true;
}
else
{
if (!this->rotation.init(rpy_str))
{
ROS_ERROR("malformed rpy");
return false;
this->rotation.clear();
}
}
return true;
}
};
};
}
#endif

209
urdf/mainpage.dox Normal file
View File

@ -0,0 +1,209 @@
/**
\mainpage
\htmlinclude manifest.html
\b robot_model is ...
<!--
In addition to providing an overview of your package,
this is the section where the specification and design/architecture
should be detailed. While the original specification may be done on the
wiki, it should be transferred here once your package starts to take shape.
You can then link to this documentation page from the Wiki.
-->
\li RobotModel is a class containing robot model data structure.
\li Below is an example Robot Description Describing a Parent Link "P", a Child Link "C", and a Joint "J"
\li OLD URDF:
@verbatim
<link name="C">
<inertial>
<mass value="10"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="1 1 1"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="1.01 1.01 1.01"/>
</geometry>
</collision>
<parent name="P"/> <!-- name of the parent link. in new RobotModel, this is in <link><joint>, not here -->
<!-- <origin> is the transform from parent Link to this Joint in parent Link frame -->
<origin xyz="0 0 0" rpy="0 0 0"/> <!-- in new RobotModel, this is in <joint><parent>, not here -->
<joint name="J" type="revolute">
<!-- joint properties -->
<axis xyz="0 1 0"/>
<joint_properties damping="1" friction="0"/>
<limit min="0" max="1" effort="1000" velocity="1"/>
<!-- OPTIONAL: transform from this Joint in child Link frame to child Link (equivalent to <child> in new RobotModel) -->
<anchor xyz="0 0 0"/>
</joint>
</link>
@endverbatim
\li NEW URDF XML that corresponds to the current RobotModel data structure:
@verbatim
<joint name="J" type="revolute">
<dynamics damping="1" friction="0"/>
<limit lower="0.9" upper="2.1" effort="1000" velocity="1"/>
<safety_controller soft_lower_limit="0.7" soft_upper_limit="2.1" k_position="1" k_velocity="1" />
<calibration reference_position="0.7" />
<mimic joint="J100" offset="0" multiplier="0.7" />
<!-- origin: origin of the joint in the parent frame -->
<!-- child link frame is the joint frame -->
<!-- axis is in the joint frame -->
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<parent link="P"/>
<child link="C"/>
</joint>
<link name="C">
<inertial>
<mass value="10"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="1 1 1"/>
</geometry>
<material name="Green"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="1.01 1.01 1.01"/>
</geometry>
<contact_coefficient mu="0" resitution="0" k_p="0" k_d="0" />
</collision>
</link>
<material name="Green">
<texture filename="...texture file..." />
<!--color rgb="255 255 255" /-->
</material>
@endverbatim
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
\section rosapi ROS API
<!--
Names are very important in ROS because they can be remapped on the
command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
APPEAR IN THE CODE. You should list names of every topic, service and
parameter used in your code. There is a template below that you can
use to document each node separately.
List of nodes:
- \b node_name1
- \b node_name2
-->
<!-- START: copy from here to 'END' for each node
<hr>
\subsection node_name node_name
node_name does (provide a basic description of your node)
\subsubsection Usage
\verbatim
$ node_type1 [standard ROS args]
\endverbatim
\par Example
\verbatim
$ node_type1
\endverbatim
\subsubsection topics ROS topics
Subscribes to:
- \b "in": [std_msgs/FooType] description of in
Publishes to:
- \b "out": [std_msgs/FooType] description of out
\subsubsection parameters ROS parameters
Reads the following parameters from the parameter server
- \b "~param_name" : \b [type] description of param_name
- \b "~my_param" : \b [string] description of my_param
Sets the following parameters on the parameter server
- \b "~param_name" : \b [type] description of param_name
\subsubsection services ROS services
- \b "foo_service": [std_srvs/FooType] description of foo_service
END: copy for each node -->
<!-- START: Uncomment if you have any command-line tools
\section commandline Command-line tools
This section is a catch-all for any additional tools that your package
provides or uses that may be of use to the reader. For example:
- tools/scripts (e.g. rospack, roscd)
- roslaunch .launch files
- xmlparam files
\subsection script_name script_name
Description of what this script/file does.
\subsubsection Usage
\verbatim
$ ./script_name [args]
\endverbatim
\par Example
\verbatim
$ ./script_name foo bar
\endverbatim
END: Command-Line Tools Section -->
*/

19
urdf/manifest.xml Normal file
View File

@ -0,0 +1,19 @@
<package>
<description brief="urdf">
This package containt a parser for the Xml Robot Description Format (URDF).
</description>
<author>Wim Meeussen, John Hsu</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/urdf</url>
<depend package="tinyxml" />
<depend package="roscpp" />
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lurdf"/>
</export>
</package>

387
urdf/src/joint.cpp Normal file
View File

@ -0,0 +1,387 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: John Hsu */
#include <urdf/joint.h>
#include <ros/ros.h>
namespace urdf{
bool JointDynamics::initXml(TiXmlElement* config)
{
this->clear();
// Get joint damping
const char* damping_str = config->Attribute("damping");
if (damping_str == NULL)
ROS_DEBUG("joint dynamics: no damping");
else
this->damping = atof(damping_str);
// Get joint friction
const char* friction_str = config->Attribute("friction");
if (friction_str == NULL)
ROS_DEBUG("joint dynamics: no friction");
else
this->friction = atof(friction_str);
if (damping_str == NULL && friction_str == NULL)
{
ROS_ERROR("joint dynamics element specified with no damping and no friction");
return false;
}
else{
ROS_DEBUG("joint dynamics: damping %f and friction %f", damping, friction);
return true;
}
}
bool JointLimits::initXml(TiXmlElement* config)
{
this->clear();
// Get lower joint limit
const char* lower_str = config->Attribute("lower");
if (lower_str == NULL)
ROS_DEBUG("joint limit: no lower");
else
this->lower = atof(lower_str);
// Get upper joint limit
const char* upper_str = config->Attribute("upper");
if (upper_str == NULL)
ROS_DEBUG("joint limit: no upper");
else
this->upper = atof(upper_str);
// Get joint effort limit
const char* effort_str = config->Attribute("effort");
if (effort_str == NULL)
ROS_DEBUG("joint limit: no effort");
else
this->effort = atof(effort_str);
// Get joint velocity limit
const char* velocity_str = config->Attribute("velocity");
if (velocity_str == NULL)
ROS_DEBUG("joint limit: no velocity");
else
this->velocity = atof(velocity_str);
if (lower_str == NULL && upper_str == NULL && effort_str == NULL && velocity_str == NULL)
{
ROS_ERROR("joint limit element specified with no readable attributes");
return false;
}
else
return true;
}
bool JointSafety::initXml(TiXmlElement* config)
{
this->clear();
// Get soft_lower_limit joint limit
const char* soft_lower_limit_str = config->Attribute("soft_lower_limit");
if (soft_lower_limit_str == NULL)
{
ROS_DEBUG("joint safety: no soft_lower_limit, using default value");
this->soft_lower_limit = 0;
}
else
this->soft_lower_limit = atof(soft_lower_limit_str);
// Get soft_upper_limit joint limit
const char* soft_upper_limit_str = config->Attribute("soft_upper_limit");
if (soft_upper_limit_str == NULL)
{
ROS_DEBUG("joint safety: no soft_upper_limit, using default value");
this->soft_upper_limit = 0;
}
else
this->soft_upper_limit = atof(soft_upper_limit_str);
// Get k_position_ safety "position" gain - not exactly position gain
const char* k_position_str = config->Attribute("k_position");
if (k_position_str == NULL)
{
ROS_DEBUG("joint safety: no k_position, using default value");
this->k_position = 0;
}
else
this->k_position = atof(k_position_str);
// Get k_velocity_ safety velocity gain
const char* k_velocity_str = config->Attribute("k_velocity");
if (k_velocity_str == NULL)
{
ROS_DEBUG("joint safety: no k_velocity, using default value");
this->k_velocity = 0;
}
else
this->k_velocity = atof(k_velocity_str);
return true;
}
bool JointCalibration::initXml(TiXmlElement* config)
{
this->clear();
// Get reference_position
const char* reference_position_str = config->Attribute("reference_position");
if (reference_position_str == NULL)
{
ROS_DEBUG("joint calibration: no reference_position, using default value");
this->reference_position = 0;
}
else
this->reference_position = atof(reference_position_str);
return true;
}
bool JointMimic::initXml(TiXmlElement* config)
{
this->clear();
// Get name of joint to mimic
const char* joint_name_str = config->Attribute("joint");
if (joint_name_str == NULL)
{
ROS_WARN("joint mimic: no mimic joint specified");
return false;
}
else
this->joint_name = joint_name_str;
// Get mimic multiplier
const char* multiplier_str = config->Attribute("multiplier");
if (multiplier_str == NULL)
{
ROS_DEBUG("joint mimic: no multiplier, using default value of 1");
this->multiplier = 1;
}
else
this->multiplier = atof(multiplier_str);
// Get mimic offset
const char* offset_str = config->Attribute("offset");
if (offset_str == NULL)
{
ROS_DEBUG("joint mimic: no offset, using default value of 0");
this->offset = 0;
}
else
this->offset = atof(offset_str);
return true;
}
bool Joint::initXml(TiXmlElement* config)
{
this->clear();
// Get Joint Name
const char *name = config->Attribute("name");
if (!name)
{
ROS_ERROR("unnamed joint found");
return false;
}
this->name = name;
// Get transform from Parent Link to Joint Frame
TiXmlElement *origin_xml = config->FirstChildElement("origin");
if (!origin_xml)
{
ROS_DEBUG("Joint '%s' missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", this->name.c_str());
this->parent_to_joint_origin_transform.clear();
}
else
{
if (!this->parent_to_joint_origin_transform.initXml(origin_xml))
{
ROS_ERROR("Malformed parent origin element for joint '%s'", this->name.c_str());
this->parent_to_joint_origin_transform.clear();
return false;
}
}
// Get Parent Link
TiXmlElement *parent_xml = config->FirstChildElement("parent");
if (parent_xml)
{
const char *pname = parent_xml->Attribute("link");
if (!pname)
ROS_INFO("no parent link name specified for Joint link '%s'. this might be the root?", this->name.c_str());
else
{
this->parent_link_name = std::string(pname);
}
}
// Get Child Link
TiXmlElement *child_xml = config->FirstChildElement("child");
if (child_xml)
{
const char *pname = child_xml->Attribute("link");
if (!pname)
ROS_INFO("no child link name specified for Joint link '%s'.", this->name.c_str());
else
{
this->child_link_name = std::string(pname);
}
}
// Get Joint type
const char* type_char = config->Attribute("type");
if (!type_char)
{
ROS_ERROR("joint '%s' has no type, check to see if it's a reference.", this->name.c_str());
return false;
}
std::string type_str = type_char;
if (type_str == "planar")
type = PLANAR;
else if (type_str == "floating")
type = FLOATING;
else if (type_str == "revolute")
type = REVOLUTE;
else if (type_str == "continuous")
type = CONTINUOUS;
else if (type_str == "prismatic")
type = PRISMATIC;
else if (type_str == "fixed")
type = FIXED;
else
{
ROS_ERROR("Joint '%s' has no known type '%s'", this->name.c_str(), type_str.c_str());
return false;
}
// Get Joint Axis
if (this->type != FLOATING)
{
// axis
TiXmlElement *axis_xml = config->FirstChildElement("axis");
if (axis_xml)
{
if (!axis_xml->Attribute("xyz"))
ROS_INFO("no xyz attribute for axis element for Joint link '%s', using default values", this->name.c_str());
else
{
if (!this->axis.init(axis_xml->Attribute("xyz")))
{
if (this->type == PLANAR)
ROS_DEBUG("PLANAR Joint '%s' will require an axis tag in the future which indicates the surface normal of the plane.", this->name.c_str());
else
{
ROS_ERROR("Malformed axis element for joint '%s'", this->name.c_str());
this->axis.clear();
return false;
}
}
}
}
}
// Get limit
TiXmlElement *limit_xml = config->FirstChildElement("limit");
if (limit_xml)
{
limits.reset(new JointLimits);
if (!limits->initXml(limit_xml))
{
ROS_ERROR("Could not parse limit element for joint '%s'", this->name.c_str());
limits.reset();
}
}
// Get safety
TiXmlElement *safety_xml = config->FirstChildElement("safety_controller");
if (safety_xml)
{
safety.reset(new JointSafety);
if (!safety->initXml(safety_xml))
{
ROS_ERROR("Could not parse safety element for joint '%s'", this->name.c_str());
safety.reset();
}
}
// Get calibration
TiXmlElement *calibration_xml = config->FirstChildElement("calibration");
if (calibration_xml)
{
calibration.reset(new JointCalibration);
if (!calibration->initXml(calibration_xml))
{
ROS_ERROR("Could not parse calibration element for joint '%s'", this->name.c_str());
calibration.reset();
}
}
// Get Joint Mimic
TiXmlElement *mimic_xml = config->FirstChildElement("mimic");
if (mimic_xml)
{
mimic.reset(new JointMimic);
if (!mimic->initXml(mimic_xml))
{
ROS_WARN("Could not parse mimic element for joint '%s'", this->name.c_str());
mimic.reset();
}
}
// Get Dynamics
TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
if (prop_xml)
{
dynamics.reset(new JointDynamics);
if (!dynamics->initXml(prop_xml))
{
ROS_ERROR("Could not parse joint_dynamics element for joint '%s'", this->name.c_str());
dynamics.reset();
}
}
return true;
}
}

434
urdf/src/link.cpp Normal file
View File

@ -0,0 +1,434 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include "urdf/link.h"
#include <ros/ros.h>
namespace urdf{
boost::shared_ptr<Geometry> parseGeometry(TiXmlElement *g)
{
boost::shared_ptr<Geometry> geom;
if (!g) return geom;
TiXmlElement *shape = g->FirstChildElement();
if (!shape)
{
ROS_ERROR("Geometry tag contains no child element.");
return geom;
}
std::string type_name = shape->ValueStr();
if (type_name == "sphere")
geom.reset(new Sphere);
else if (type_name == "box")
geom.reset(new Box);
else if (type_name == "cylinder")
geom.reset(new Cylinder);
else if (type_name == "mesh")
geom.reset(new Mesh);
else
{
ROS_ERROR("Unknown geometry type '%s'", type_name.c_str());
return geom;
}
if (!geom->initXml(shape))
return geom;
return geom;
}
bool Material::initXml(TiXmlElement *config)
{
bool has_rgb = false;
bool has_filename = false;
this->clear();
if (!config->Attribute("name"))
{
ROS_ERROR("Material must contain a name attribute");
return false;
}
this->name = config->Attribute("name");
// texture
TiXmlElement *t = config->FirstChildElement("texture");
if (t)
{
if (t->Attribute("filename"))
{
this->texture_filename = t->Attribute("filename");
has_filename = true;
}
else
{
ROS_ERROR("texture has no filename for Material %s",this->name.c_str());
}
}
// color
TiXmlElement *c = config->FirstChildElement("color");
if (c)
{
if (c->Attribute("rgba"))
{
if (!this->color.init(c->Attribute("rgba")))
{
ROS_ERROR("Material %s has malformed color rgba values.",this->name.c_str());
this->color.clear();
return false;
}
else
has_rgb = true;
}
else
{
ROS_ERROR("Material %s color has no rgba",this->name.c_str());
}
}
return (has_rgb || has_filename);
}
bool Inertial::initXml(TiXmlElement *config)
{
this->clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (!o)
{
ROS_INFO("Origin tag not present for inertial element, using default (Identity)");
this->origin.clear();
}
else
{
if (!this->origin.initXml(o))
{
ROS_ERROR("Inertial has a malformed origin tag");
this->origin.clear();
return false;
}
}
TiXmlElement *mass_xml = config->FirstChildElement("mass");
if (!mass_xml)
{
ROS_ERROR("Inertial element must have mass element");
return false;
}
if (!mass_xml->Attribute("value"))
{
ROS_ERROR("Inertial: mass element must have value attributes");
return false;
}
mass = atof(mass_xml->Attribute("value"));
TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml)
{
ROS_ERROR("Inertial element must have inertia element");
return false;
}
if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
inertia_xml->Attribute("izz")))
{
ROS_ERROR("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
return false;
}
ixx = atof(inertia_xml->Attribute("ixx"));
ixy = atof(inertia_xml->Attribute("ixy"));
ixz = atof(inertia_xml->Attribute("ixz"));
iyy = atof(inertia_xml->Attribute("iyy"));
iyz = atof(inertia_xml->Attribute("iyz"));
izz = atof(inertia_xml->Attribute("izz"));
return true;
}
bool Visual::initXml(TiXmlElement *config)
{
this->clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (!o)
{
ROS_DEBUG("Origin tag not present for visual element, using default (Identity)");
this->origin.clear();
}
else if (!this->origin.initXml(o))
{
ROS_ERROR("Visual has a malformed origin tag");
this->origin.clear();
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
geometry = parseGeometry(geom);
if (!geometry)
{
ROS_ERROR("Malformed geometry for Visual element");
return false;
}
// Material
TiXmlElement *mat = config->FirstChildElement("material");
if (!mat)
{
ROS_DEBUG("visual element has no material tag.");
}
else
{
// get material name
if (!mat->Attribute("name"))
{
ROS_ERROR("Visual material must contain a name attribute");
return false;
}
this->material_name = mat->Attribute("name");
// try to parse material element in place
this->material.reset(new Material);
if (!this->material->initXml(mat))
{
ROS_DEBUG("Could not parse material element in Visual block, maybe defined outside.");
this->material.reset();
}
else
{
ROS_DEBUG("Parsed material element in Visual block.");
}
}
return true;
}
bool Collision::initXml(TiXmlElement* config)
{
this->clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (!o)
{
ROS_INFO("Origin tag not present for collision element, using default (Identity)");
this->origin.clear();
}
else if (!this->origin.initXml(o))
{
ROS_ERROR("Collision has a malformed origin tag");
this->origin.clear();
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
geometry = parseGeometry(geom);
if (!geometry)
{
ROS_ERROR("Malformed geometry for Collision element");
return false;
}
return true;
}
bool Sphere::initXml(TiXmlElement *c)
{
this->clear();
this->type = SPHERE;
if (!c->Attribute("radius"))
{
ROS_ERROR("Sphere shape must have a radius attribute");
return false;
}
radius = atof(c->Attribute("radius"));
return false;
}
bool Box::initXml(TiXmlElement *c)
{
this->clear();
this->type = BOX;
if (!c->Attribute("size"))
{
ROS_ERROR("Box shape has no size attribute");
return false;
}
if (!dim.init(c->Attribute("size")))
{
ROS_ERROR("Box shape has malformed size attribute");
dim.clear();
return false;
}
return true;
}
bool Cylinder::initXml(TiXmlElement *c)
{
this->clear();
this->type = CYLINDER;
if (!c->Attribute("length") ||
!c->Attribute("radius"))
{
ROS_ERROR("Cylinder shape must have both length and radius attributes");
return false;
}
length = atof(c->Attribute("length"));
radius = atof(c->Attribute("radius"));
return true;
}
bool Mesh::initXml(TiXmlElement *c)
{
this->clear();
this->type = MESH;
if (!c->Attribute("filename"))
{
ROS_ERROR("Mesh must contain a filename attribute");
return false;
}
filename = c->Attribute("filename");
if (c->Attribute("scale"))
{
if (!this->scale.init(c->Attribute("scale")))
{
ROS_ERROR("Mesh scale was specified, but could not be parsed");
this->scale.clear();
return false;
}
}
else
ROS_DEBUG("Mesh scale was not specified, default to (1,1,1)");
return true;
}
bool Link::initXml(TiXmlElement* config)
{
this->clear();
const char *name_char = config->Attribute("name");
if (!name_char)
{
ROS_ERROR("No name given for the link.");
return false;
}
name = std::string(name_char);
// Inertial
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)
{
inertial.reset(new Inertial);
if (!inertial->initXml(i))
{
ROS_ERROR("Could not parse inertial element for Link '%s'", this->name.c_str());
inertial.reset();
}
}
// Visual
TiXmlElement *v = config->FirstChildElement("visual");
if (v)
{
visual.reset(new Visual);
if (!visual->initXml(v))
{
ROS_ERROR("Could not parse visual element for Link '%s'", this->name.c_str());
visual.reset();
}
}
// Collision
TiXmlElement *col = config->FirstChildElement("collision");
if (col)
{
collision.reset(new Collision);
if (!collision->initXml(col))
{
ROS_ERROR("Could not parse collision element for Link '%s'", this->name.c_str());
collision.reset();
}
}
return true;
}
void Link::setParent(boost::shared_ptr<Link> parent)
{
this->parent_link = parent;
ROS_DEBUG("set parent Link '%s' for Link '%s'", parent->name.c_str(), this->name.c_str());
}
void Link::setParentJoint(boost::shared_ptr<Joint> parent)
{
this->parent_joint = parent;
ROS_DEBUG("set parent joint '%s' to Link '%s'", parent->name.c_str(), this->name.c_str());
}
void Link::addChild(boost::shared_ptr<Link> child)
{
this->child_links.push_back(child);
ROS_DEBUG("added child Link '%s' to Link '%s'", child->name.c_str(), this->name.c_str());
}
void Link::addChildJoint(boost::shared_ptr<Joint> child)
{
this->child_joints.push_back(child);
ROS_DEBUG("added child Joint '%s' to Link '%s'", child->name.c_str(), this->name.c_str());
}
}

409
urdf/src/model.cpp Normal file
View File

@ -0,0 +1,409 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include <boost/algorithm/string.hpp>
#include <ros/ros.h>
#include <vector>
#include "urdf/model.h"
namespace urdf{
Model::Model()
{
this->clear();
}
void Model::clear()
{
name_.clear();
this->links_.clear();
this->joints_.clear();
this->materials_.clear();
this->root_link_.reset();
}
bool Model::initFile(const std::string& filename)
{
TiXmlDocument xml_doc;
xml_doc.LoadFile(filename);
return initXml(&xml_doc);
}
bool Model::initString(const std::string& xml_string)
{
TiXmlDocument xml_doc;
xml_doc.Parse(xml_string.c_str());
return initXml(&xml_doc);
}
bool Model::initXml(TiXmlDocument *xml_doc)
{
if (!xml_doc)
{
ROS_ERROR("Could not parse the xml");
return false;
}
TiXmlElement *robot_xml = xml_doc->FirstChildElement("robot");
if (!robot_xml)
{
ROS_ERROR("Could not find the 'robot' element in the xml file");
return false;
}
return initXml(robot_xml);
}
bool Model::initXml(TiXmlElement *robot_xml)
{
this->clear();
ROS_DEBUG("Parsing robot xml");
if (!robot_xml) return false;
// Get robot name
const char *name = robot_xml->Attribute("name");
if (!name)
{
ROS_ERROR("No name given for the robot.");
return false;
}
this->name_ = std::string(name);
// Get all Material elements
for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
{
boost::shared_ptr<Material> material;
material.reset(new Material);
if (material->initXml(material_xml))
{
if (this->getMaterial(material->name))
{
ROS_ERROR("material '%s' is not unique.", material->name.c_str());
material.reset();
return false;
}
else
{
this->materials_.insert(make_pair(material->name,material));
ROS_DEBUG("successfully added a new material '%s'", material->name.c_str());
}
}
else
{
ROS_ERROR("material xml is not initialized correctly");
material.reset();
}
}
// Get all Link elements
for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
boost::shared_ptr<Link> link;
link.reset(new Link);
if (link->initXml(link_xml))
{
if (this->getLink(link->name))
{
ROS_ERROR("link '%s' is not unique.", link->name.c_str());
link.reset();
return false;
}
else
{
// set link visual material
ROS_DEBUG("setting link '%s' material", link->name.c_str());
if (link->visual)
{
if (!link->visual->material_name.empty())
{
if (this->getMaterial(link->visual->material_name))
{
ROS_DEBUG("setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str());
link->visual->material = this->getMaterial( link->visual->material_name.c_str() );
}
else
{
if (link->visual->material)
{
ROS_DEBUG("link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str());
this->materials_.insert(make_pair(link->visual->material->name,link->visual->material));
}
else
{
ROS_ERROR("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str());
link.reset();
return false;
}
}
}
}
this->links_.insert(make_pair(link->name,link));
ROS_DEBUG("successfully added a new link '%s'", link->name.c_str());
}
}
else
{
ROS_ERROR("link xml is not initialized correctly");
link.reset();
}
}
// Get all Joint elements
for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
boost::shared_ptr<Joint> joint;
joint.reset(new Joint);
if (joint->initXml(joint_xml))
{
if (this->getJoint(joint->name))
{
ROS_ERROR("joint '%s' is not unique.", joint->name.c_str());
joint.reset();
return false;
}
else
{
this->joints_.insert(make_pair(joint->name,joint));
ROS_DEBUG("successfully added a new joint '%s'", joint->name.c_str());
}
}
else
{
ROS_ERROR("joint xml is not initialized correctly");
joint.reset();
}
}
// every link has children links and joints, but no parents, so we create a
// local convenience data structure for keeping child->parent relations
std::map<std::string, std::string> parent_link_tree;
parent_link_tree.clear();
// building tree: name mapping
if (!this->initTree(parent_link_tree))
{
ROS_ERROR("failed to build tree");
return false;
}
// find the root link
if (!this->initRoot(parent_link_tree))
{
ROS_ERROR("failed to find root link");
return false;
}
return true;
}
bool Model::initTree(std::map<std::string, std::string> &parent_link_tree)
{
// loop through all joints, for every link, assign children links and children joints
for (std::map<std::string,boost::shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
{
std::string parent_link_name = joint->second->parent_link_name;
std::string child_link_name = joint->second->child_link_name;
ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str());
/// add an empty "world" link
if (parent_link_name == "world")
{
if (this->getLink(parent_link_name))
{
ROS_DEBUG(" parent link '%s' already exists.", parent_link_name.c_str());
}
else
{
ROS_DEBUG(" parent link '%s' is a special case, adding fake link.", parent_link_name.c_str());
boost::shared_ptr<Link> link;
link.reset(new Link);
link->name = "world";
this->links_.insert(make_pair(link->name,link));
ROS_DEBUG(" successfully added new link '%s'", link->name.c_str());
}
}
if (parent_link_name.empty())
{
ROS_DEBUG(" Joint %s: does not have parent link name specified. Joint is an abstract joint.",(joint->second)->name.c_str());
}
else if (child_link_name.empty())
{
ROS_DEBUG(" Joint %s: does not have child link name specified. Joint is an abstract joint.",(joint->second)->name.c_str());
}
else
{
// find parent link
boost::shared_ptr<Link> parent_link;
this->getLink(parent_link_name, parent_link);
if (!parent_link)
{
ROS_ERROR(" parent link '%s' is not found", parent_link_name.c_str());
return false;
}
else
{
// find child link
boost::shared_ptr<Link> child_link;
this->getLink(child_link_name, child_link);
if (!child_link)
{
ROS_ERROR(" for joint: %s child link '%s' is not found",joint->first.c_str(),child_link_name.c_str());
return false;
}
else
{
//set parent link for child link
child_link->setParent(parent_link);
//set parent joint for child link
child_link->setParentJoint(joint->second);
//set child joint for parent link
parent_link->addChildJoint(joint->second);
//set child link for parent link
parent_link->addChild(child_link);
// fill in child/parent string map
parent_link_tree[child_link->name] = parent_link_name;
ROS_DEBUG(" now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size());
}
}
}
}
return true;
}
bool Model::initRoot(std::map<std::string, std::string> &parent_link_tree)
{
this->root_link_.reset();
for (std::map<std::string, std::string>::iterator p=parent_link_tree.begin(); p!=parent_link_tree.end(); p++)
{
if (parent_link_tree.find(p->second) == parent_link_tree.end())
{
if (this->root_link_)
{
ROS_DEBUG("child '%s', parent '%s', root '%s'", p->first.c_str(), p->second.c_str(), this->root_link_->name.c_str());
if (this->root_link_->name != p->second)
{
ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), p->second.c_str());
return false;
}
}
else
getLink(p->second,this->root_link_);
}
}
if (!this->root_link_)
{
ROS_ERROR("No root link found. The robot xml is empty or not a tree.");
return false;
}
ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str());
return true;
}
boost::shared_ptr<Material> Model::getMaterial(const std::string& name) const
{
boost::shared_ptr<Material> ptr;
if (this->materials_.find(name) == this->materials_.end())
ptr.reset();
else
ptr = this->materials_.find(name)->second;
return ptr;
}
boost::shared_ptr<const Link> Model::getLink(const std::string& name) const
{
boost::shared_ptr<const Link> ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset();
else
ptr = this->links_.find(name)->second;
return ptr;
}
void Model::getLinks(std::vector<boost::shared_ptr<Link> >& links) const
{
for (std::map<std::string,boost::shared_ptr<Link> >::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
{
links.push_back(link->second);
}
}
void Model::getLink(const std::string& name,boost::shared_ptr<Link> &link) const
{
boost::shared_ptr<Link> ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset();
else
ptr = this->links_.find(name)->second;
link = ptr;
}
boost::shared_ptr<const Joint> Model::getJoint(const std::string& name) const
{
boost::shared_ptr<const Joint> ptr;
if (this->joints_.find(name) == this->joints_.end())
ptr.reset();
else
ptr = this->joints_.find(name)->second;
return ptr;
}
}

104
urdf/test/parse_test.cpp Normal file
View File

@ -0,0 +1,104 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include "urdf/model.h"
#include <iostream>
using namespace urdf;
void printTree(boost::shared_ptr<const Link> link,int level = 0)
{
level+=2;
int count = 0;
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
{
if (*child)
{
for(int j=0;j<level;j++) std::cout << " "; //indent
std::cout << "child(" << count++ << "): " << (*child)->name
<< " with mass: " << (*child)->inertial->mass
<< std::endl;
// first grandchild
printTree(*child,level);
}
else
{
for(int j=0;j<level;j++) std::cout << " "; //indent
std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
}
}
}
int main(int argc, char** argv)
{
if (argc < 2){
std::cerr << "Expect xml file to parse" << std::endl;
return -1;
}
TiXmlDocument robot_model_xml;
robot_model_xml.LoadFile(argv[1]);
TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
if (!robot_xml){
std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
return -1;
}
Model robot;
if (!robot.initXml(robot_xml)){
std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
return -1;
}
std::cout << "robot name is: " << robot.getName() << std::endl;
// get info from parser
std::cout << "---------- Finished Loading from Model XML, Now Checking Model structure ------------" << std::endl;
// get root link
boost::shared_ptr<const Link> root_link=robot.getRoot();
if (!root_link) return -1;
std::cout << "root Link: " << root_link->name << std::endl;
if (!root_link->child_links.empty())
std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " children" << std::endl;
// print entire tree
printTree(root_link);
return 0;
}

2542
urdf/test/pr2_desc.xml Normal file
View File

File diff suppressed because it is too large Load Diff

20
urdf/test/pr2_desc_bracket.xml Normal file
View File

@ -0,0 +1,20 @@
<?xml version="1.0" ?>
<robot name="pr2">
<joint name="fl_caster_rotation_joint" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="100" k_velocity="10" velocity="100"/>
<calibration reference_position="0.0" values="1.5 -1"/>
<joint_properties damping="0.0" friction="0.0"/>
<origin rpy="0 0 1 }" xyz="0.2225 0.2225 0.0282"/>
<parent link="base_link"/>
<child link="fl_caster_rotation_link"/>
</joint>
<link name="fl_caster_rotation_link">
<inertial>
<mass value="3.473082"/>
<origin xyz="0 0 0.07"/>
<inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
</inertial>
</link>
</robot>

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