forked from LeenkxTeam/LNXSDK
Update Files
This commit is contained in:
@ -0,0 +1,500 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
#if defined (_WIN32) || defined (__i386__)
|
||||
#define BT_USE_SSE_IN_API
|
||||
#endif
|
||||
|
||||
#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
|
||||
#include "btConvexPolyhedron.h"
|
||||
#include "LinearMath/btConvexHullComputer.h"
|
||||
#include <new>
|
||||
#include "LinearMath/btGeometryUtil.h"
|
||||
#include "LinearMath/btGrahamScan2dConvexHull.h"
|
||||
|
||||
|
||||
btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(),
|
||||
m_polyhedron(0)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
btPolyhedralConvexShape::~btPolyhedralConvexShape()
|
||||
{
|
||||
if (m_polyhedron)
|
||||
{
|
||||
m_polyhedron->~btConvexPolyhedron();
|
||||
btAlignedFree(m_polyhedron);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
|
||||
{
|
||||
|
||||
if (m_polyhedron)
|
||||
{
|
||||
m_polyhedron->~btConvexPolyhedron();
|
||||
btAlignedFree(m_polyhedron);
|
||||
}
|
||||
|
||||
void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
|
||||
m_polyhedron = new (mem) btConvexPolyhedron;
|
||||
|
||||
btAlignedObjectArray<btVector3> orgVertices;
|
||||
|
||||
for (int i=0;i<getNumVertices();i++)
|
||||
{
|
||||
btVector3& newVertex = orgVertices.expand();
|
||||
getVertex(i,newVertex);
|
||||
}
|
||||
|
||||
btConvexHullComputer conv;
|
||||
|
||||
if (shiftVerticesByMargin)
|
||||
{
|
||||
btAlignedObjectArray<btVector3> planeEquations;
|
||||
btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations);
|
||||
|
||||
btAlignedObjectArray<btVector3> shiftedPlaneEquations;
|
||||
for (int p=0;p<planeEquations.size();p++)
|
||||
{
|
||||
btVector3 plane = planeEquations[p];
|
||||
// btScalar margin = getMargin();
|
||||
plane[3] -= getMargin();
|
||||
shiftedPlaneEquations.push_back(plane);
|
||||
}
|
||||
|
||||
btAlignedObjectArray<btVector3> tmpVertices;
|
||||
|
||||
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices);
|
||||
|
||||
conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
|
||||
} else
|
||||
{
|
||||
|
||||
conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
|
||||
}
|
||||
|
||||
|
||||
|
||||
btAlignedObjectArray<btVector3> faceNormals;
|
||||
int numFaces = conv.faces.size();
|
||||
faceNormals.resize(numFaces);
|
||||
btConvexHullComputer* convexUtil = &conv;
|
||||
|
||||
|
||||
btAlignedObjectArray<btFace> tmpFaces;
|
||||
tmpFaces.resize(numFaces);
|
||||
|
||||
int numVertices = convexUtil->vertices.size();
|
||||
m_polyhedron->m_vertices.resize(numVertices);
|
||||
for (int p=0;p<numVertices;p++)
|
||||
{
|
||||
m_polyhedron->m_vertices[p] = convexUtil->vertices[p];
|
||||
}
|
||||
|
||||
|
||||
for (int i=0;i<numFaces;i++)
|
||||
{
|
||||
int face = convexUtil->faces[i];
|
||||
//printf("face=%d\n",face);
|
||||
const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
|
||||
const btConvexHullComputer::Edge* edge = firstEdge;
|
||||
|
||||
btVector3 edges[3];
|
||||
int numEdges = 0;
|
||||
//compute face normals
|
||||
|
||||
do
|
||||
{
|
||||
|
||||
int src = edge->getSourceVertex();
|
||||
tmpFaces[i].m_indices.push_back(src);
|
||||
int targ = edge->getTargetVertex();
|
||||
btVector3 wa = convexUtil->vertices[src];
|
||||
|
||||
btVector3 wb = convexUtil->vertices[targ];
|
||||
btVector3 newEdge = wb-wa;
|
||||
newEdge.normalize();
|
||||
if (numEdges<2)
|
||||
edges[numEdges++] = newEdge;
|
||||
|
||||
edge = edge->getNextEdgeOfFace();
|
||||
} while (edge!=firstEdge);
|
||||
|
||||
btScalar planeEq = 1e30f;
|
||||
|
||||
|
||||
if (numEdges==2)
|
||||
{
|
||||
faceNormals[i] = edges[0].cross(edges[1]);
|
||||
faceNormals[i].normalize();
|
||||
tmpFaces[i].m_plane[0] = faceNormals[i].getX();
|
||||
tmpFaces[i].m_plane[1] = faceNormals[i].getY();
|
||||
tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
|
||||
tmpFaces[i].m_plane[3] = planeEq;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
btAssert(0);//degenerate?
|
||||
faceNormals[i].setZero();
|
||||
}
|
||||
|
||||
for (int v=0;v<tmpFaces[i].m_indices.size();v++)
|
||||
{
|
||||
btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
|
||||
if (planeEq>eq)
|
||||
{
|
||||
planeEq=eq;
|
||||
}
|
||||
}
|
||||
tmpFaces[i].m_plane[3] = -planeEq;
|
||||
}
|
||||
|
||||
//merge coplanar faces and copy them to m_polyhedron
|
||||
|
||||
btScalar faceWeldThreshold= 0.999f;
|
||||
btAlignedObjectArray<int> todoFaces;
|
||||
for (int i=0;i<tmpFaces.size();i++)
|
||||
todoFaces.push_back(i);
|
||||
|
||||
while (todoFaces.size())
|
||||
{
|
||||
btAlignedObjectArray<int> coplanarFaceGroup;
|
||||
int refFace = todoFaces[todoFaces.size()-1];
|
||||
|
||||
coplanarFaceGroup.push_back(refFace);
|
||||
btFace& faceA = tmpFaces[refFace];
|
||||
todoFaces.pop_back();
|
||||
|
||||
btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
|
||||
for (int j=todoFaces.size()-1;j>=0;j--)
|
||||
{
|
||||
int i = todoFaces[j];
|
||||
btFace& faceB = tmpFaces[i];
|
||||
btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);
|
||||
if (faceNormalA.dot(faceNormalB)>faceWeldThreshold)
|
||||
{
|
||||
coplanarFaceGroup.push_back(i);
|
||||
todoFaces.remove(i);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool did_merge = false;
|
||||
if (coplanarFaceGroup.size()>1)
|
||||
{
|
||||
//do the merge: use Graham Scan 2d convex hull
|
||||
|
||||
btAlignedObjectArray<GrahamVector3> orgpoints;
|
||||
btVector3 averageFaceNormal(0,0,0);
|
||||
|
||||
for (int i=0;i<coplanarFaceGroup.size();i++)
|
||||
{
|
||||
// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
|
||||
|
||||
btFace& face = tmpFaces[coplanarFaceGroup[i]];
|
||||
btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
|
||||
averageFaceNormal+=faceNormal;
|
||||
for (int f=0;f<face.m_indices.size();f++)
|
||||
{
|
||||
int orgIndex = face.m_indices[f];
|
||||
btVector3 pt = m_polyhedron->m_vertices[orgIndex];
|
||||
|
||||
bool found = false;
|
||||
|
||||
for (int i=0;i<orgpoints.size();i++)
|
||||
{
|
||||
//if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
|
||||
if (orgpoints[i].m_orgIndex == orgIndex)
|
||||
{
|
||||
found=true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found)
|
||||
orgpoints.push_back(GrahamVector3(pt,orgIndex));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
btFace combinedFace;
|
||||
for (int i=0;i<4;i++)
|
||||
combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
|
||||
|
||||
btAlignedObjectArray<GrahamVector3> hull;
|
||||
|
||||
averageFaceNormal.normalize();
|
||||
GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal);
|
||||
|
||||
for (int i=0;i<hull.size();i++)
|
||||
{
|
||||
combinedFace.m_indices.push_back(hull[i].m_orgIndex);
|
||||
for(int k = 0; k < orgpoints.size(); k++)
|
||||
{
|
||||
if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
|
||||
{
|
||||
orgpoints[k].m_orgIndex = -1; // invalidate...
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// are there rejected vertices?
|
||||
bool reject_merge = false;
|
||||
|
||||
|
||||
|
||||
for(int i = 0; i < orgpoints.size(); i++) {
|
||||
if(orgpoints[i].m_orgIndex == -1)
|
||||
continue; // this is in the hull...
|
||||
// this vertex is rejected -- is anybody else using this vertex?
|
||||
for(int j = 0; j < tmpFaces.size(); j++) {
|
||||
|
||||
btFace& face = tmpFaces[j];
|
||||
// is this a face of the current coplanar group?
|
||||
bool is_in_current_group = false;
|
||||
for(int k = 0; k < coplanarFaceGroup.size(); k++) {
|
||||
if(coplanarFaceGroup[k] == j) {
|
||||
is_in_current_group = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(is_in_current_group) // ignore this face...
|
||||
continue;
|
||||
// does this face use this rejected vertex?
|
||||
for(int v = 0; v < face.m_indices.size(); v++) {
|
||||
if(face.m_indices[v] == orgpoints[i].m_orgIndex) {
|
||||
// this rejected vertex is used in another face -- reject merge
|
||||
reject_merge = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(reject_merge)
|
||||
break;
|
||||
}
|
||||
if(reject_merge)
|
||||
break;
|
||||
}
|
||||
|
||||
if (!reject_merge)
|
||||
{
|
||||
// do this merge!
|
||||
did_merge = true;
|
||||
m_polyhedron->m_faces.push_back(combinedFace);
|
||||
}
|
||||
}
|
||||
if(!did_merge)
|
||||
{
|
||||
for (int i=0;i<coplanarFaceGroup.size();i++)
|
||||
{
|
||||
btFace face = tmpFaces[coplanarFaceGroup[i]];
|
||||
m_polyhedron->m_faces.push_back(face);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
m_polyhedron->initialize();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#ifndef MIN
|
||||
#define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b))
|
||||
#endif
|
||||
|
||||
btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
|
||||
{
|
||||
|
||||
|
||||
btVector3 supVec(0,0,0);
|
||||
#ifndef __SPU__
|
||||
int i;
|
||||
btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
|
||||
|
||||
btVector3 vec = vec0;
|
||||
btScalar lenSqr = vec.length2();
|
||||
if (lenSqr < btScalar(0.0001))
|
||||
{
|
||||
vec.setValue(1,0,0);
|
||||
} else
|
||||
{
|
||||
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
|
||||
vec *= rlen;
|
||||
}
|
||||
|
||||
btVector3 vtx;
|
||||
btScalar newDot;
|
||||
|
||||
for( int k = 0; k < getNumVertices(); k += 128 )
|
||||
{
|
||||
btVector3 temp[128];
|
||||
int inner_count = MIN(getNumVertices() - k, 128);
|
||||
for( i = 0; i < inner_count; i++ )
|
||||
getVertex(i,temp[i]);
|
||||
i = (int) vec.maxDot( temp, inner_count, newDot);
|
||||
if (newDot > maxDot)
|
||||
{
|
||||
maxDot = newDot;
|
||||
supVec = temp[i];
|
||||
}
|
||||
}
|
||||
|
||||
#endif //__SPU__
|
||||
return supVec;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
|
||||
{
|
||||
#ifndef __SPU__
|
||||
int i;
|
||||
|
||||
btVector3 vtx;
|
||||
btScalar newDot;
|
||||
|
||||
for (i=0;i<numVectors;i++)
|
||||
{
|
||||
supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
|
||||
}
|
||||
|
||||
for (int j=0;j<numVectors;j++)
|
||||
{
|
||||
const btVector3& vec = vectors[j];
|
||||
|
||||
for( int k = 0; k < getNumVertices(); k += 128 )
|
||||
{
|
||||
btVector3 temp[128];
|
||||
int inner_count = MIN(getNumVertices() - k, 128);
|
||||
for( i = 0; i < inner_count; i++ )
|
||||
getVertex(i,temp[i]);
|
||||
i = (int) vec.maxDot( temp, inner_count, newDot);
|
||||
if (newDot > supportVerticesOut[j][3])
|
||||
{
|
||||
supportVerticesOut[j] = temp[i];
|
||||
supportVerticesOut[j][3] = newDot;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif //__SPU__
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
|
||||
{
|
||||
#ifndef __SPU__
|
||||
//not yet, return box inertia
|
||||
|
||||
btScalar margin = getMargin();
|
||||
|
||||
btTransform ident;
|
||||
ident.setIdentity();
|
||||
btVector3 aabbMin,aabbMax;
|
||||
getAabb(ident,aabbMin,aabbMax);
|
||||
btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
|
||||
|
||||
btScalar lx=btScalar(2.)*(halfExtents.x()+margin);
|
||||
btScalar ly=btScalar(2.)*(halfExtents.y()+margin);
|
||||
btScalar lz=btScalar(2.)*(halfExtents.z()+margin);
|
||||
const btScalar x2 = lx*lx;
|
||||
const btScalar y2 = ly*ly;
|
||||
const btScalar z2 = lz*lz;
|
||||
const btScalar scaledmass = mass * btScalar(0.08333333);
|
||||
|
||||
inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
|
||||
#endif //__SPU__
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling)
|
||||
{
|
||||
btConvexInternalShape::setLocalScaling(scaling);
|
||||
recalcLocalAabb();
|
||||
}
|
||||
|
||||
btPolyhedralConvexAabbCachingShape::btPolyhedralConvexAabbCachingShape()
|
||||
:btPolyhedralConvexShape(),
|
||||
m_localAabbMin(1,1,1),
|
||||
m_localAabbMax(-1,-1,-1),
|
||||
m_isLocalAabbValid(false)
|
||||
{
|
||||
}
|
||||
|
||||
void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
|
||||
{
|
||||
getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
|
||||
}
|
||||
|
||||
void btPolyhedralConvexAabbCachingShape::recalcLocalAabb()
|
||||
{
|
||||
m_isLocalAabbValid = true;
|
||||
|
||||
#if 1
|
||||
static const btVector3 _directions[] =
|
||||
{
|
||||
btVector3( 1., 0., 0.),
|
||||
btVector3( 0., 1., 0.),
|
||||
btVector3( 0., 0., 1.),
|
||||
btVector3( -1., 0., 0.),
|
||||
btVector3( 0., -1., 0.),
|
||||
btVector3( 0., 0., -1.)
|
||||
};
|
||||
|
||||
btVector3 _supporting[] =
|
||||
{
|
||||
btVector3( 0., 0., 0.),
|
||||
btVector3( 0., 0., 0.),
|
||||
btVector3( 0., 0., 0.),
|
||||
btVector3( 0., 0., 0.),
|
||||
btVector3( 0., 0., 0.),
|
||||
btVector3( 0., 0., 0.)
|
||||
};
|
||||
|
||||
batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
|
||||
|
||||
for ( int i = 0; i < 3; ++i )
|
||||
{
|
||||
m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
|
||||
m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
vec[i] = btScalar(1.);
|
||||
btVector3 tmp = localGetSupportingVertex(vec);
|
||||
m_localAabbMax[i] = tmp[i];
|
||||
vec[i] = btScalar(-1.);
|
||||
tmp = localGetSupportingVertex(vec);
|
||||
m_localAabbMin[i] = tmp[i];
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user