more bt->b3*
This commit is contained in:
erwin coumans
@@ -37,8 +37,8 @@
|
||||
"../../../OpenGLTrueTypeFont/fontstash.h",
|
||||
"../../../OpenGLTrueTypeFont/opengl_fontstashcallbacks.cpp",
|
||||
"../../../OpenGLTrueTypeFont/opengl_fontstashcallbacks.h",
|
||||
"../../../../bullet2/LinearMath/btConvexHullComputer.cpp",
|
||||
"../../../../bullet2/LinearMath/btConvexHullComputer.h",
|
||||
"../../../../bullet2/LinearMath/b3ConvexHullComputer.cpp",
|
||||
"../../../../bullet2/LinearMath/b3ConvexHullComputer.h",
|
||||
"../../../../bullet2/LinearMath/btSerializer.cpp",
|
||||
"../../../../bullet2/LinearMath/btSerializer.h",
|
||||
"../../../../bullet2/LinearMath/b3AlignedAllocator.cpp",
|
||||
|
||||
@@ -37,8 +37,8 @@
|
||||
"../OpenGLTrueTypeFont/fontstash.h",
|
||||
"../OpenGLTrueTypeFont/opengl_fontstashcallbacks.cpp",
|
||||
"../OpenGLTrueTypeFont/opengl_fontstashcallbacks.h",
|
||||
"../../src/BulletGeometry/btConvexHullComputer.cpp",
|
||||
"../../src/BulletGeometry/btConvexHullComputer.h",
|
||||
"../../src/BulletGeometry/b3ConvexHullComputer.cpp",
|
||||
"../../src/BulletGeometry/b3ConvexHullComputer.h",
|
||||
"../../src/BulletCommon/b3AlignedAllocator.cpp",
|
||||
"../../src/BulletCommon/b3Quickprof.cpp",
|
||||
"../../src/BulletCommon/b3Quickprof.h",
|
||||
|
||||
@@ -41,8 +41,8 @@
|
||||
"../OpenGLTrueTypeFont/fontstash.h",
|
||||
"../OpenGLTrueTypeFont/opengl_fontstashcallbacks.cpp",
|
||||
"../OpenGLTrueTypeFont/opengl_fontstashcallbacks.h",
|
||||
"../../src/BulletGeometry/btConvexHullComputer.cpp",
|
||||
"../../src/BulletGeometry/btConvexHullComputer.h",
|
||||
"../../src/BulletGeometry/b3ConvexHullComputer.cpp",
|
||||
"../../src/BulletGeometry/b3ConvexHullComputer.h",
|
||||
"../../src/BulletCommon/b3AlignedAllocator.cpp",
|
||||
"../../src/BulletCommon/b3Quickprof.cpp",
|
||||
"../../src/BulletCommon/b3Quickprof.h"
|
||||
|
||||
@@ -45,8 +45,8 @@ function createProject(vendor)
|
||||
"../../src/BulletCommon/b3AlignedAllocator.h",
|
||||
"../../src/BulletCommon/b3Quickprof.cpp",
|
||||
"../../src/BulletCommon/b3Quickprof.h",
|
||||
"../../src/BulletGeometry/btConvexHullComputer.cpp",
|
||||
"../../src/BulletGeometry/btConvexHullComputer.h",
|
||||
"../../src/BulletGeometry/b3ConvexHullComputer.cpp",
|
||||
"../../src/BulletGeometry/b3ConvexHullComputer.h",
|
||||
|
||||
"../../btgui/OpenGLWindow/GLInstancingRenderer.cpp",
|
||||
"../../btgui/OpenGLWindow/GLInstancingRenderer.h",
|
||||
|
||||
@@ -38,7 +38,7 @@ typedef b3AlignedObjectArray<b3Vector3> btVertexArray;
|
||||
#include "../kernels/bvhTraversal.h"
|
||||
#include "../kernels/primitiveContacts.h"
|
||||
|
||||
#include "BulletGeometry/btAabbUtil2.h"
|
||||
#include "BulletGeometry/b3AabbUtil.h"
|
||||
|
||||
|
||||
#define dot3F4 btDot
|
||||
|
||||
@@ -15,8 +15,8 @@ subject to the following restrictions:
|
||||
|
||||
|
||||
#include "b3ConvexUtility.h"
|
||||
#include "BulletGeometry/btConvexHullComputer.h"
|
||||
#include "BulletGeometry/btGrahamScan2dConvexHull.h"
|
||||
#include "BulletGeometry/b3ConvexHullComputer.h"
|
||||
#include "BulletGeometry/b3GrahamScan2dConvexHull.h"
|
||||
#include "BulletCommon/b3Quaternion.h"
|
||||
#include "BulletCommon/b3HashMap.h"
|
||||
|
||||
@@ -33,13 +33,13 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
|
||||
|
||||
|
||||
|
||||
btConvexHullComputer conv;
|
||||
b3ConvexHullComputer conv;
|
||||
conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3),numPoints,0.f,0.f);
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> faceNormals;
|
||||
int numFaces = conv.faces.size();
|
||||
faceNormals.resize(numFaces);
|
||||
btConvexHullComputer* convexUtil = &conv;
|
||||
b3ConvexHullComputer* convexUtil = &conv;
|
||||
|
||||
|
||||
b3AlignedObjectArray<btMyFace> tmpFaces;
|
||||
@@ -57,8 +57,8 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
|
||||
{
|
||||
int face = convexUtil->faces[i];
|
||||
//printf("face=%d\n",face);
|
||||
const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
|
||||
const btConvexHullComputer::Edge* edge = firstEdge;
|
||||
const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
|
||||
const b3ConvexHullComputer::Edge* edge = firstEdge;
|
||||
|
||||
b3Vector3 edges[3];
|
||||
int numEdges = 0;
|
||||
|
||||
@@ -16,7 +16,7 @@ subject to the following restrictions:
|
||||
|
||||
#include "b3OptimizedBvh.h"
|
||||
#include "b3StridingMeshInterface.h"
|
||||
#include "BulletGeometry/btAabbUtil2.h"
|
||||
#include "BulletGeometry/b3AabbUtil.h"
|
||||
|
||||
|
||||
b3OptimizedBvh::b3OptimizedBvh()
|
||||
|
||||
@@ -15,7 +15,7 @@ subject to the following restrictions:
|
||||
|
||||
#include "b3QuantizedBvh.h"
|
||||
|
||||
#include "BulletGeometry/btAabbUtil2.h"
|
||||
#include "BulletGeometry/b3AabbUtil.h"
|
||||
|
||||
|
||||
#define RAYAABB2
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
#include "b3Config.h"
|
||||
#include "../../gpu_narrowphase/host/b3OptimizedBvh.h"
|
||||
#include "../../gpu_narrowphase/host/b3TriangleIndexVertexArray.h"
|
||||
#include "BulletGeometry/btAabbUtil2.h"
|
||||
#include "BulletGeometry/b3AabbUtil.h"
|
||||
|
||||
struct btGpuNarrowPhaseInternalData
|
||||
{
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
|
||||
#include "../../basic_initialize/b3OpenCLUtils.h"
|
||||
#include "b3GpuNarrowPhase.h"
|
||||
#include "BulletGeometry/btAabbUtil2.h"
|
||||
#include "BulletGeometry/b3AabbUtil.h"
|
||||
#include "../../gpu_broadphase/host/b3SapAabb.h"
|
||||
#include "../../gpu_broadphase/host/b3GpuSapBroadphase.h"
|
||||
#include "parallel_primitives/host/btLauncherCL.h"
|
||||
|
||||
@@ -14,7 +14,7 @@ subject to the following restrictions:
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include "btConvexHullComputer.h"
|
||||
#include "b3ConvexHullComputer.h"
|
||||
#include "BulletCommon/b3AlignedObjectArray.h"
|
||||
#include "BulletCommon/b3MinMax.h"
|
||||
#include "BulletCommon/b3Vector3.h"
|
||||
@@ -2638,7 +2638,7 @@ static int getVertexCopy(btConvexHullInternal::Vertex* vertex, b3AlignedObjectAr
|
||||
return index;
|
||||
}
|
||||
|
||||
b3Scalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
|
||||
b3Scalar b3ConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
|
||||
{
|
||||
if (count <= 0)
|
||||
{
|
||||
@@ -21,7 +21,7 @@ subject to the following restrictions:
|
||||
/// Convex hull implementation based on Preparata and Hong
|
||||
/// See http://code.google.com/p/bullet/issues/detail?id=275
|
||||
/// Ole Kniemeyer, MAXON Computer GmbH
|
||||
class btConvexHullComputer
|
||||
class b3ConvexHullComputer
|
||||
{
|
||||
private:
|
||||
b3Scalar compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp);
|
||||
@@ -35,7 +35,7 @@ class btConvexHullComputer
|
||||
int reverse;
|
||||
int targetVertex;
|
||||
|
||||
friend class btConvexHullComputer;
|
||||
friend class b3ConvexHullComputer;
|
||||
|
||||
public:
|
||||
int getSourceVertex() const
|
||||
@@ -14,7 +14,7 @@ subject to the following restrictions:
|
||||
|
||||
|
||||
|
||||
#include "btGeometryUtil.h"
|
||||
#include "b3GeometryUtil.h"
|
||||
|
||||
|
||||
/*
|
||||
@@ -30,7 +30,7 @@ extern "C"
|
||||
}
|
||||
|
||||
|
||||
bool btGeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin)
|
||||
bool b3GeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin)
|
||||
{
|
||||
int numbrushes = planeEquations.size();
|
||||
for (int i=0;i<numbrushes;i++)
|
||||
@@ -47,7 +47,7 @@ bool btGeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>&
|
||||
}
|
||||
|
||||
|
||||
bool btGeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin)
|
||||
bool b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin)
|
||||
{
|
||||
int numvertices = vertices.size();
|
||||
for (int i=0;i<numvertices;i++)
|
||||
@@ -78,7 +78,7 @@ bool notExist(const b3Vector3& planeEquation,const b3AlignedObjectArray<b3Vector
|
||||
return true;
|
||||
}
|
||||
|
||||
void btGeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut )
|
||||
void b3GeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut )
|
||||
{
|
||||
const int numvertices = vertices.size();
|
||||
// brute force:
|
||||
@@ -126,7 +126,7 @@ void btGeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector
|
||||
|
||||
}
|
||||
|
||||
void btGeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations , b3AlignedObjectArray<b3Vector3>& verticesOut )
|
||||
void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations , b3AlignedObjectArray<b3Vector3>& verticesOut )
|
||||
{
|
||||
const int numbrushes = planeEquations.size();
|
||||
// brute force:
|
||||
@@ -19,8 +19,8 @@ subject to the following restrictions:
|
||||
#include "BulletCommon/b3Vector3.h"
|
||||
#include "BulletCommon/b3AlignedObjectArray.h"
|
||||
|
||||
///The btGeometryUtil helper class provides a few methods to convert between plane equations and vertices.
|
||||
class btGeometryUtil
|
||||
///The b3GeometryUtil helper class provides a few methods to convert between plane equations and vertices.
|
||||
class b3GeometryUtil
|
||||
{
|
||||
public:
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,241 +0,0 @@
|
||||
|
||||
/*
|
||||
Stan Melax Convex Hull Computation
|
||||
Copyright (c) 2008 Stan Melax http://www.melax.com/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
///includes modifications/improvements by John Ratcliff, see BringOutYourDead below.
|
||||
|
||||
#ifndef BT_CD_HULL_H
|
||||
#define BT_CD_HULL_H
|
||||
|
||||
#include "BulletCommon/b3Vector3.h"
|
||||
#include "BulletCommon/b3AlignedObjectArray.h"
|
||||
|
||||
typedef b3AlignedObjectArray<unsigned int> TUIntArray;
|
||||
|
||||
class HullResult
|
||||
{
|
||||
public:
|
||||
HullResult(void)
|
||||
{
|
||||
mPolygons = true;
|
||||
mNumOutputVertices = 0;
|
||||
mNumFaces = 0;
|
||||
mNumIndices = 0;
|
||||
}
|
||||
bool mPolygons; // true if indices represents polygons, false indices are triangles
|
||||
unsigned int mNumOutputVertices; // number of vertices in the output hull
|
||||
b3AlignedObjectArray<b3Vector3> m_OutputVertices; // array of vertices
|
||||
unsigned int mNumFaces; // the number of faces produced
|
||||
unsigned int mNumIndices; // the total number of indices
|
||||
b3AlignedObjectArray<unsigned int> m_Indices; // 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_DEFAULT = QF_TRIANGLES
|
||||
};
|
||||
|
||||
|
||||
class HullDesc
|
||||
{
|
||||
public:
|
||||
HullDesc(void)
|
||||
{
|
||||
mFlags = QF_DEFAULT;
|
||||
mVcount = 0;
|
||||
mVertices = 0;
|
||||
mVertexStride = sizeof(b3Vector3);
|
||||
mNormalEpsilon = 0.001f;
|
||||
mMaxVertices = 4096; // maximum number of points to be considered for a convex hull.
|
||||
mMaxFaces = 4096;
|
||||
};
|
||||
|
||||
HullDesc(HullFlag flag,
|
||||
unsigned int vcount,
|
||||
const b3Vector3 *vertices,
|
||||
unsigned int stride = sizeof(b3Vector3))
|
||||
{
|
||||
mFlags = flag;
|
||||
mVcount = vcount;
|
||||
mVertices = vertices;
|
||||
mVertexStride = stride;
|
||||
mNormalEpsilon = b3Scalar(0.001);
|
||||
mMaxVertices = 4096;
|
||||
}
|
||||
|
||||
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 b3Vector3 *mVertices; // the array of vertices.
|
||||
unsigned int mVertexStride; // the stride of each vertex, in bytes.
|
||||
b3Scalar mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on.
|
||||
unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull!
|
||||
unsigned int mMaxFaces;
|
||||
};
|
||||
|
||||
enum HullError
|
||||
{
|
||||
QE_OK, // success!
|
||||
QE_FAIL // failed.
|
||||
};
|
||||
|
||||
class btPlane
|
||||
{
|
||||
public:
|
||||
b3Vector3 normal;
|
||||
b3Scalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0
|
||||
btPlane(const b3Vector3 &n,b3Scalar d):normal(n),dist(d){}
|
||||
btPlane():normal(),dist(0){}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
class ConvexH
|
||||
{
|
||||
public:
|
||||
class HalfEdge
|
||||
{
|
||||
public:
|
||||
short ea; // the other half of the edge (index into edges list)
|
||||
unsigned char v; // the vertex at the start of this edge (index into vertices list)
|
||||
unsigned char p; // the facet on which this edge lies (index into facets list)
|
||||
HalfEdge(){}
|
||||
HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){}
|
||||
};
|
||||
ConvexH()
|
||||
{
|
||||
}
|
||||
~ConvexH()
|
||||
{
|
||||
}
|
||||
b3AlignedObjectArray<b3Vector3> vertices;
|
||||
b3AlignedObjectArray<HalfEdge> edges;
|
||||
b3AlignedObjectArray<btPlane> facets;
|
||||
ConvexH(int vertices_size,int edges_size,int facets_size);
|
||||
};
|
||||
|
||||
|
||||
class int4
|
||||
{
|
||||
public:
|
||||
int x,y,z,w;
|
||||
int4(){};
|
||||
int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;}
|
||||
const int& operator[](int i) const {return (&x)[i];}
|
||||
int& operator[](int i) {return (&x)[i];}
|
||||
};
|
||||
|
||||
class PHullResult
|
||||
{
|
||||
public:
|
||||
|
||||
PHullResult(void)
|
||||
{
|
||||
mVcount = 0;
|
||||
mIndexCount = 0;
|
||||
mFaceCount = 0;
|
||||
mVertices = 0;
|
||||
}
|
||||
|
||||
unsigned int mVcount;
|
||||
unsigned int mIndexCount;
|
||||
unsigned int mFaceCount;
|
||||
b3Vector3* mVertices;
|
||||
TUIntArray m_Indices;
|
||||
};
|
||||
|
||||
|
||||
|
||||
///The HullLibrary class can create a convex hull from a collection of vertices, using the ComputeHull method.
|
||||
///The btShapeHull class uses this HullLibrary to create a approximate convex mesh given a general (non-polyhedral) convex shape.
|
||||
class HullLibrary
|
||||
{
|
||||
|
||||
b3AlignedObjectArray<class btHullTriangle*> m_tris;
|
||||
|
||||
public:
|
||||
|
||||
b3AlignedObjectArray<int> m_vertexIndexMapping;
|
||||
|
||||
|
||||
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.
|
||||
|
||||
private:
|
||||
|
||||
bool ComputeHull(unsigned int vcount,const b3Vector3 *vertices,PHullResult &result,unsigned int vlimit);
|
||||
|
||||
class btHullTriangle* allocateTriangle(int a,int b,int c);
|
||||
void deAllocateTriangle(btHullTriangle*);
|
||||
void b2bfix(btHullTriangle* s,btHullTriangle*t);
|
||||
|
||||
void removeb2b(btHullTriangle* s,btHullTriangle*t);
|
||||
|
||||
void checkit(btHullTriangle *t);
|
||||
|
||||
btHullTriangle* extrudable(b3Scalar epsilon);
|
||||
|
||||
int calchull(b3Vector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit);
|
||||
|
||||
int calchullgen(b3Vector3 *verts,int verts_count, int vlimit);
|
||||
|
||||
int4 FindSimplex(b3Vector3 *verts,int verts_count,b3AlignedObjectArray<int> &allow);
|
||||
|
||||
class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice);
|
||||
|
||||
void extrude(class btHullTriangle* t0,int v);
|
||||
|
||||
ConvexH* test_cube();
|
||||
|
||||
//BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'.
|
||||
//After the hull is generated it give you back a set of polygon faces which index the *original* point cloud.
|
||||
//The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull.
|
||||
//The routine 'BringOutYourDead' find only the referenced vertices, copies them to an new buffer, and re-indexes the hull so that it is a minimal representation.
|
||||
void BringOutYourDead(const b3Vector3* verts,unsigned int vcount, b3Vector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount);
|
||||
|
||||
bool CleanupVertices(unsigned int svcount,
|
||||
const b3Vector3* svertices,
|
||||
unsigned int stride,
|
||||
unsigned int &vcount, // output number of vertices
|
||||
b3Vector3* vertices, // location to store the results.
|
||||
b3Scalar normalepsilon,
|
||||
b3Vector3& scale);
|
||||
};
|
||||
|
||||
|
||||
#endif //BT_CD_HULL_H
|
||||
|
||||
@@ -1,99 +0,0 @@
|
||||
#include "btPolarDecomposition.h"
|
||||
#include "BulletCommon/b3MinMax.h"
|
||||
|
||||
namespace
|
||||
{
|
||||
b3Scalar abs_column_sum(const b3Matrix3x3& a, int i)
|
||||
{
|
||||
return btFabs(a[0][i]) + btFabs(a[1][i]) + btFabs(a[2][i]);
|
||||
}
|
||||
|
||||
b3Scalar abs_row_sum(const b3Matrix3x3& a, int i)
|
||||
{
|
||||
return btFabs(a[i][0]) + btFabs(a[i][1]) + btFabs(a[i][2]);
|
||||
}
|
||||
|
||||
b3Scalar p1_norm(const b3Matrix3x3& a)
|
||||
{
|
||||
const b3Scalar sum0 = abs_column_sum(a,0);
|
||||
const b3Scalar sum1 = abs_column_sum(a,1);
|
||||
const b3Scalar sum2 = abs_column_sum(a,2);
|
||||
return btMax(btMax(sum0, sum1), sum2);
|
||||
}
|
||||
|
||||
b3Scalar pinf_norm(const b3Matrix3x3& a)
|
||||
{
|
||||
const b3Scalar sum0 = abs_row_sum(a,0);
|
||||
const b3Scalar sum1 = abs_row_sum(a,1);
|
||||
const b3Scalar sum2 = abs_row_sum(a,2);
|
||||
return btMax(btMax(sum0, sum1), sum2);
|
||||
}
|
||||
}
|
||||
|
||||
const b3Scalar btPolarDecomposition::DEFAULT_TOLERANCE = b3Scalar(0.0001);
|
||||
const unsigned int btPolarDecomposition::DEFAULT_MAX_ITERATIONS = 16;
|
||||
|
||||
btPolarDecomposition::btPolarDecomposition(b3Scalar tolerance, unsigned int maxIterations)
|
||||
: m_tolerance(tolerance)
|
||||
, m_maxIterations(maxIterations)
|
||||
{
|
||||
}
|
||||
|
||||
unsigned int btPolarDecomposition::decompose(const b3Matrix3x3& a, b3Matrix3x3& u, b3Matrix3x3& h) const
|
||||
{
|
||||
// Use the 'u' and 'h' matrices for intermediate calculations
|
||||
u = a;
|
||||
h = a.inverse();
|
||||
|
||||
for (unsigned int i = 0; i < m_maxIterations; ++i)
|
||||
{
|
||||
const b3Scalar h_1 = p1_norm(h);
|
||||
const b3Scalar h_inf = pinf_norm(h);
|
||||
const b3Scalar u_1 = p1_norm(u);
|
||||
const b3Scalar u_inf = pinf_norm(u);
|
||||
|
||||
const b3Scalar h_norm = h_1 * h_inf;
|
||||
const b3Scalar u_norm = u_1 * u_inf;
|
||||
|
||||
// The matrix is effectively singular so we cannot invert it
|
||||
if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm))
|
||||
break;
|
||||
|
||||
const b3Scalar gamma = btPow(h_norm / u_norm, 0.25f);
|
||||
const b3Scalar inv_gamma = 1.0 / gamma;
|
||||
|
||||
// Determine the delta to 'u'
|
||||
const b3Matrix3x3 delta = (u * (gamma - 2.0) + h.transpose() * inv_gamma) * 0.5;
|
||||
|
||||
// Update the matrices
|
||||
u += delta;
|
||||
h = u.inverse();
|
||||
|
||||
// Check for convergence
|
||||
if (p1_norm(delta) <= m_tolerance * u_1)
|
||||
{
|
||||
h = u.transpose() * a;
|
||||
h = (h + h.transpose()) * 0.5;
|
||||
return i;
|
||||
}
|
||||
}
|
||||
|
||||
// The algorithm has failed to converge to the specified tolerance, but we
|
||||
// want to make sure that the matrices returned are in the right form.
|
||||
h = u.transpose() * a;
|
||||
h = (h + h.transpose()) * 0.5;
|
||||
|
||||
return m_maxIterations;
|
||||
}
|
||||
|
||||
unsigned int btPolarDecomposition::maxIterations() const
|
||||
{
|
||||
return m_maxIterations;
|
||||
}
|
||||
|
||||
unsigned int polarDecompose(const b3Matrix3x3& a, b3Matrix3x3& u, b3Matrix3x3& h)
|
||||
{
|
||||
static btPolarDecomposition polar;
|
||||
return polar.decompose(a, u, h);
|
||||
}
|
||||
|
||||
@@ -1,73 +0,0 @@
|
||||
#ifndef POLARDECOMPOSITION_H
|
||||
#define POLARDECOMPOSITION_H
|
||||
|
||||
#include "BulletCommon/b3Matrix3x3.h"
|
||||
|
||||
/**
|
||||
* This class is used to compute the polar decomposition of a matrix. In
|
||||
* general, the polar decomposition factorizes a matrix, A, into two parts: a
|
||||
* unitary matrix (U) and a positive, semi-definite Hermitian matrix (H).
|
||||
* However, in this particular implementation the original matrix, A, is
|
||||
* required to be a square 3x3 matrix with real elements. This means that U will
|
||||
* be an orthogonal matrix and H with be a positive-definite, symmetric matrix.
|
||||
*/
|
||||
class btPolarDecomposition
|
||||
{
|
||||
public:
|
||||
static const b3Scalar DEFAULT_TOLERANCE;
|
||||
static const unsigned int DEFAULT_MAX_ITERATIONS;
|
||||
|
||||
/**
|
||||
* Creates an instance with optional parameters.
|
||||
*
|
||||
* @param tolerance - the tolerance used to determine convergence of the
|
||||
* algorithm
|
||||
* @param maxIterations - the maximum number of iterations used to achieve
|
||||
* convergence
|
||||
*/
|
||||
btPolarDecomposition(b3Scalar tolerance = DEFAULT_TOLERANCE,
|
||||
unsigned int maxIterations = DEFAULT_MAX_ITERATIONS);
|
||||
|
||||
/**
|
||||
* Decomposes a matrix into orthogonal and symmetric, positive-definite
|
||||
* parts. If the number of iterations returned by this function is equal to
|
||||
* the maximum number of iterations, the algorithm has failed to converge.
|
||||
*
|
||||
* @param a - the original matrix
|
||||
* @param u - the resulting orthogonal matrix
|
||||
* @param h - the resulting symmetric matrix
|
||||
*
|
||||
* @return the number of iterations performed by the algorithm.
|
||||
*/
|
||||
unsigned int decompose(const b3Matrix3x3& a, b3Matrix3x3& u, b3Matrix3x3& h) const;
|
||||
|
||||
/**
|
||||
* Returns the maximum number of iterations that this algorithm will perform
|
||||
* to achieve convergence.
|
||||
*
|
||||
* @return maximum number of iterations
|
||||
*/
|
||||
unsigned int maxIterations() const;
|
||||
|
||||
private:
|
||||
b3Scalar m_tolerance;
|
||||
unsigned int m_maxIterations;
|
||||
};
|
||||
|
||||
/**
|
||||
* This functions decomposes the matrix 'a' into two parts: an orthogonal matrix
|
||||
* 'u' and a symmetric, positive-definite matrix 'h'. If the number of
|
||||
* iterations returned by this function is equal to
|
||||
* btPolarDecomposition::DEFAULT_MAX_ITERATIONS, the algorithm has failed to
|
||||
* converge.
|
||||
*
|
||||
* @param a - the original matrix
|
||||
* @param u - the resulting orthogonal matrix
|
||||
* @param h - the resulting symmetric matrix
|
||||
*
|
||||
* @return the number of iterations performed by the algorithm.
|
||||
*/
|
||||
unsigned int polarDecompose(const b3Matrix3x3& a, b3Matrix3x3& u, b3Matrix3x3& h);
|
||||
|
||||
#endif // POLARDECOMPOSITION_H
|
||||
|
||||
Reference in New Issue
Block a user