void btUniformScalingShape::getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
{
#if 1
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.)
};
for (int i=0;i<6;i++)
{
_directions[i] = _directions[i]*t.getBasis();
}
batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
btVector3 aabbMin1(0,0,0), aabbMax1(0,0,0);
for ( int i = 0; i < 3; ++i )
{
aabbMax1[i] = t(_supporting[i])[i];
aabbMin1[i] = t(_supporting[i + 3])[i];
}
btVector3 marginVec(getMargin(), getMargin(), getMargin());
aabbMin = aabbMin1-marginVec;
aabbMax = aabbMax1+marginVec;
#else
btScalar margin = getMargin();
for (int i=0;i<3;i++)
{
btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.));
vec[i] = btScalar(1.);
btVector3 sv = localGetSupportingVertex(vec*t.getBasis());
btVector3 tmp = t(sv);
aabbMax[i] = tmp[i]+margin;
vec[i] = btScalar(-1.);
sv = localGetSupportingVertex(vec*t.getBasis());
tmp = t(sv);
aabbMin[i] = tmp[i]-margin;
}
#endif
}
void btTriangleMeshShape::recalcLocalAabb()
{
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]+m_collisionMargin;
vec[i] = btScalar(-1.);
tmp = localGetSupportingVertex(vec);
m_localAabbMin[i] = tmp[i]-m_collisionMargin;
}
}
void btPolyhedralConvexShape::recalcLocalAabb()
{
m_isLocalAabbValid = true;
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]+m_collisionMargin;
vec[i] = btScalar(-1.);
tmp = localGetSupportingVertex(vec);
m_localAabbMin[i] = tmp[i]-m_collisionMargin;
}
}
void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
{
btVector3 localAxis = dir*trans.getBasis();
btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
min = vtx1.dot(dir);
max = vtx2.dot(dir);
if(min>max)
{
btScalar tmp = min;
min = max;
max = tmp;
}
}
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
}
void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
{
#if 1
min = FLT_MAX;
max = -FLT_MAX;
btVector3 witnesPtMin;
btVector3 witnesPtMax;
int numVerts = m_unscaledPoints.size();
for(int i=0;i<numVerts;i++)
{
btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
btVector3 pt = trans * vtx;
btScalar dp = pt.dot(dir);
if(dp < min)
{
min = dp;
witnesPtMin = pt;
}
if(dp > max)
{
max = dp;
witnesPtMax=pt;
}
}
#else
btVector3 localAxis = dir*trans.getBasis();
btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
min = vtx1.dot(dir);
max = vtx2.dot(dir);
#endif
if(min>max)
{
btScalar tmp = min;
min = max;
max = tmp;
}
}
void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const
{
#ifndef __SPU__
//use localGetSupportingVertexWithoutMargin?
btScalar margin = getMargin();
for (int i=0;i<3;i++)
{
btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
vec[i] = btScalar(1.);
btVector3 sv = localGetSupportingVertex(vec*trans.getBasis());
btVector3 tmp = trans(sv);
maxAabb[i] = tmp[i]+margin;
vec[i] = btScalar(-1.);
tmp = trans(localGetSupportingVertex(vec*trans.getBasis()));
minAabb[i] = tmp[i]-margin;
}
#endif
}
void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
{
#if 1
minProj = FLT_MAX;
maxProj = -FLT_MAX;
int numVerts = m_unscaledPoints.size();
for(int i=0;i<numVerts;i++)
{
btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
btVector3 pt = trans * vtx;
btScalar dp = pt.dot(dir);
if(dp < minProj)
{
minProj = dp;
witnesPtMin = pt;
}
if(dp > maxProj)
{
maxProj = dp;
witnesPtMax=pt;
}
}
#else
btVector3 localAxis = dir*trans.getBasis();
witnesPtMin = trans(localGetSupportingVertex(localAxis));
witnesPtMax = trans(localGetSupportingVertex(-localAxis));
minProj = witnesPtMin.dot(dir);
maxProj = witnesPtMax.dot(dir);
#endif
if(minProj>maxProj)
{
btSwap(minProj,maxProj);
btSwap(witnesPtMin,witnesPtMax);
}
}
bool btPolyhedralConvexShape::initializePolyhedralFeatures()
{
if (m_polyhedron)
btAlignedFree(m_polyhedron);
void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
m_polyhedron = new (mem) btConvexPolyhedron;
btAlignedObjectArray<btVector3> tmpVertices;
for (int i=0;i<getNumVertices();i++)
{
btVector3& newVertex = tmpVertices.expand();
getVertex(i,newVertex);
}
btConvexHullComputer conv;
conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
btAlignedObjectArray<btVector3> faceNormals;
int numFaces = conv.faces.size();
faceNormals.resize(numFaces);
btConvexHullComputer* convexUtil = &conv;
m_polyhedron->m_faces.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
//btScalar maxCross2 = 0.f;
//int chosenEdge = -1;
do
{
int src = edge->getSourceVertex();
m_polyhedron->m_faces[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();
m_polyhedron->m_faces[i].m_plane[0] = -faceNormals[i].getX();
m_polyhedron->m_faces[i].m_plane[1] = -faceNormals[i].getY();
m_polyhedron->m_faces[i].m_plane[2] = -faceNormals[i].getZ();
m_polyhedron->m_faces[i].m_plane[3] = planeEq;
}
else
{
btAssert(0);//degenerate?
faceNormals[i].setZero();
}
for (int v=0;v<m_polyhedron->m_faces[i].m_indices.size();v++)
{
btScalar eq = m_polyhedron->m_vertices[m_polyhedron->m_faces[i].m_indices[v]].dot(faceNormals[i]);
if (planeEq>eq)
{
planeEq=eq;
}
}
m_polyhedron->m_faces[i].m_plane[3] = planeEq;
}
if (m_polyhedron->m_faces.size() && conv.vertices.size())
{
for (int f=0;f<m_polyhedron->m_faces.size();f++)
{
btVector3 planeNormal(m_polyhedron->m_faces[f].m_plane[0],m_polyhedron->m_faces[f].m_plane[1],m_polyhedron->m_faces[f].m_plane[2]);
btScalar planeEq = m_polyhedron->m_faces[f].m_plane[3];
btVector3 supVec = localGetSupportingVertex(-planeNormal);
if (supVec.dot(planeNormal)<planeEq)
{
m_polyhedron->m_faces[f].m_plane[0] *= -1;
m_polyhedron->m_faces[f].m_plane[1] *= -1;
m_polyhedron->m_faces[f].m_plane[2] *= -1;
m_polyhedron->m_faces[f].m_plane[3] *= -1;
int numVerts = m_polyhedron->m_faces[f].m_indices.size();
for (int v=0;v<numVerts/2;v++)
{
btSwap(m_polyhedron->m_faces[f].m_indices[v],m_polyhedron->m_faces[f].m_indices[numVerts-1-v]);
}
}
}
}
m_polyhedron->initialize();
return true;
}
