virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorldOrg, btScalar depthOrg)
{
m_reportedDistance = depthOrg;
m_reportedNormalOnWorld = normalOnBInWorld;
btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB;
m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB);
if (m_reportedDistance<0.f)
{
m_foundResult = true;
}
m_originalResult->addContactPoint(normalOnBInWorld, adjustedPointB, m_reportedDistance);
}
bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
{
gActualSATPairTests++;
//#ifdef TEST_INTERNAL_OBJECTS
const btVector3 c0 = transA * hullA.m_localCenter;
const btVector3 c1 = transB * hullB.m_localCenter;
const btVector3 DeltaC2 = c0 - c1;
//#endif
btScalar dmin = FLT_MAX;
int curPlaneTests=0;
int numFacesA = hullA.m_faces.size();
// Test normals from hullA
for(int i=0;i<numFacesA;i++)
{
const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
btVector3 faceANormalWS = transA.getBasis() * Normal;
if (DeltaC2.dot(faceANormalWS)<0)
faceANormalWS*=-1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar d;
btVector3 wA, wB;
if(!TestSepAxis( hullA, hullB, transA, transB, faceANormalWS, d, wA, wB))
return false;
if(d<dmin)
{
dmin = d;
sep = faceANormalWS;
}
}
int numFacesB = hullB.m_faces.size();
// Test normals from hullB
for(int i=0;i<numFacesB;i++)
{
const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
btVector3 WorldNormal = transB.getBasis() * Normal;
if (DeltaC2.dot(WorldNormal)<0)
WorldNormal *=-1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar d;
btVector3 wA, wB;
if(!TestSepAxis(hullA, hullB, transA, transB, WorldNormal, d,wA, wB))
return false;
if(d<dmin)
{
dmin = d;
sep = WorldNormal;
}
}
btVector3 edgeAstart, edgeAend, edgeBstart, edgeBend;
int edgeA=-1;
int edgeB=-1;
btVector3 worldEdgeA;
btVector3 worldEdgeB;
btVector3 witnessPointA, witnessPointB;
int curEdgeEdge = 0;
// Test edges
for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
{
const btVector3 edge0 = hullA.m_uniqueEdges[e0];
const btVector3 WorldEdge0 = transA.getBasis() * edge0;
for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
{
const btVector3 edge1 = hullB.m_uniqueEdges[e1];
const btVector3 WorldEdge1 = transB.getBasis() * edge1;
btVector3 Cross = WorldEdge0.cross(WorldEdge1);
curEdgeEdge++;
if(!IsAlmostZero(Cross))
{
Cross = Cross.normalize();
if (DeltaC2.dot(Cross)<0)
Cross *= -1.f;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar dist;
btVector3 wA, wB;
if(!TestSepAxis( hullA, hullB, transA, transB, Cross, dist, wA, wB))
return false;
if(dist<dmin)
{
dmin = dist;
sep = Cross;
edgeA=e0;
edgeB=e1;
worldEdgeA = WorldEdge0;
worldEdgeB = WorldEdge1;
witnessPointA=wA;
witnessPointB=wB;
}
}
}
}
if (edgeA>=0&&edgeB>=0)
{
// printf("edge-edge\n");
//add an edge-edge contact
btVector3 ptsVector;
btVector3 offsetA;
btVector3 offsetB;
btScalar tA;
btScalar tB;
btVector3 translation = witnessPointB-witnessPointA;
btVector3 dirA = worldEdgeA;
btVector3 dirB = worldEdgeB;
btScalar hlenB = 1e30f;
btScalar hlenA = 1e30f;
btSegmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB,
translation,
dirA, hlenA,
dirB, hlenB);
btScalar nlSqrt = ptsVector.length2();
if (nlSqrt>SIMD_EPSILON)
{
btScalar nl = btSqrt(nlSqrt);
ptsVector *= 1.f/nl;
if (ptsVector.dot(DeltaC2)<0.f)
{
ptsVector*=-1.f;
}
btVector3 ptOnB = witnessPointB + offsetB;
btScalar distance = nl;
resultOut.addContactPoint(ptsVector, ptOnB,-distance);
}
}
if((DeltaC2.dot(sep))<0.0f)
sep = -sep;
return true;
}
void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
{
btVertexArray worldVertsB2;
btVertexArray* pVtxIn = &worldVertsB1;
btVertexArray* pVtxOut = &worldVertsB2;
pVtxOut->reserve(pVtxIn->size());
int closestFaceA=-1;
{
btScalar dmin = FLT_MAX;
for(int face=0;face<hullA.m_faces.size();face++)
{
const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
const btVector3 faceANormalWS = transA.getBasis() * Normal;
btScalar d = faceANormalWS.dot(separatingNormal);
if (d < dmin)
{
dmin = d;
closestFaceA = face;
}
}
}
if (closestFaceA<0)
return;
const btFace& polyA = hullA.m_faces[closestFaceA];
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
int numContacts = pVtxIn->size();
int numVerticesA = polyA.m_indices.size();
for(int e0=0;e0<numVerticesA;e0++)
{
const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]];
const btVector3 edge0 = a - b;
const btVector3 WorldEdge0 = transA.getBasis() * edge0;
btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0);
btVector3 worldA1 = transA*a;
btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1);
//int otherFace=0;
#ifdef BLA1
int otherFace = polyA.m_connectedFaces[e0];
btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
#else
btVector3 planeNormalWS = planeNormalWS1;
btScalar planeEqWS=planeEqWS1;
#endif
//clip face
clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
btSwap(pVtxIn,pVtxOut);
pVtxOut->resize(0);
}
//#define ONLY_REPORT_DEEPEST_POINT
btVector3 point;
// only keep points that are behind the witness face
{
btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
btScalar localPlaneEq = polyA.m_plane[3];
btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
for (int i=0;i<pVtxIn->size();i++)
{
btScalar depth = planeNormalWS.dot(pVtxIn->at(i))+planeEqWS;
if (depth <=minDist)
{
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
depth = minDist;
}
if (depth <=maxDist)
{
btVector3 point = pVtxIn->at(i);
#ifdef ONLY_REPORT_DEEPEST_POINT
curMaxDist = depth;
#else
#if 0
if (depth<-3)
{
printf("error in btPolyhedralContactClipping depth = %f\n", depth);
printf("likely wrong separatingNormal passed in\n");
}
#endif
resultOut.addContactPoint(separatingNormal,point,depth);
#endif
}
}
}
#ifdef ONLY_REPORT_DEEPEST_POINT
if (curMaxDist<maxDist)
{
resultOut.addContactPoint(separatingNormal,point,curMaxDist);
}
#endif //ONLY_REPORT_DEEPEST_POINT
}
int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
const btVector3& side1, const btVector3& p2,
const dMatrix3 R2, const btVector3& side2,
btVector3& normal, btScalar *depth, int *return_code,
int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output)
{
const btScalar fudge_factor = btScalar(1.05);
btVector3 p,pp,normalC(0.f,0.f,0.f);
const btScalar *normalR = 0;
btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
int i,j,invert_normal,code;
// get vector from centers of box 1 to box 2, relative to box 1
p = p2 - p1;
dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
// get side lengths / 2
A[0] = side1[0]*btScalar(0.5);
A[1] = side1[1]*btScalar(0.5);
A[2] = side1[2]*btScalar(0.5);
B[0] = side2[0]*btScalar(0.5);
B[1] = side2[1]*btScalar(0.5);
B[2] = side2[2]*btScalar(0.5);
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13);
Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23);
Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33);
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
#define TST(expr1,expr2,norm,cc) \
s2 = btFabs(expr1) - (expr2); \
if (s2 > 0) return 0; \
if (s2 > s) { \
s = s2; \
normalR = norm; \
invert_normal = ((expr1) < 0); \
code = (cc); \
}
s = -dInfinity;
invert_normal = 0;
code = 0;
// separating axis = u1,u2,u3
TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
// separating axis = v1,v2,v3
TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
s2 = btFabs(expr1) - (expr2); \
if (s2 > SIMD_EPSILON) return 0; \
l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
if (l > SIMD_EPSILON) { \
s2 /= l; \
if (s2*fudge_factor > s) { \
s = s2; \
normalR = 0; \
normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
invert_normal = ((expr1) < 0); \
code = (cc); \
} \
}
btScalar fudge2 (1.0e-5f);
Q11 += fudge2;
Q12 += fudge2;
Q13 += fudge2;
Q21 += fudge2;
Q22 += fudge2;
Q23 += fudge2;
Q31 += fudge2;
Q32 += fudge2;
Q33 += fudge2;
// separating axis = u1 x (v1,v2,v3)
TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
// separating axis = u2 x (v1,v2,v3)
TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
// separating axis = u3 x (v1,v2,v3)
TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
#undef TST
if (!code) return 0;
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if (normalR) {
normal[0] = normalR[0];
normal[1] = normalR[4];
normal[2] = normalR[8];
}
else {
dMULTIPLY0_331 (normal,R1,normalC);
}
if (invert_normal) {
normal[0] = -normal[0];
normal[1] = -normal[1];
normal[2] = -normal[2];
}
*depth = -s;
// compute contact point(s)
if (code > 6) {
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
btVector3 pa;
btScalar sign;
for (i=0; i<3; i++) pa[i] = p1[i];
for (j=0; j<3; j++) {
sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0);
for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
}
// find a point pb on the intersecting edge of box 2
btVector3 pb;
for (i=0; i<3; i++) pb[i] = p2[i];
for (j=0; j<3; j++) {
sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0);
for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
}
btScalar alpha,beta;
btVector3 ua,ub;
for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
for (i=0; i<3; i++) pb[i] += ub[i]*beta;
{
//contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
//contact[0].depth = *depth;
btVector3 pointInWorld;
#ifdef USE_CENTER_POINT
for (i=0; i<3; i++)
pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5);
output.addContactPoint(-normal,pointInWorld,-*depth);
#else
output.addContactPoint(-normal,pb,-*depth);
#endif //
*return_code = code;
}
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb;
if (code <= 3) {
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else {
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
btVector3 normal2,nr,anr;
if (code <= 3) {
normal2[0] = normal[0];
normal2[1] = normal[1];
normal2[2] = normal[2];
}
else {
normal2[0] = -normal[0];
normal2[1] = -normal[1];
normal2[2] = -normal[2];
}
dMULTIPLY1_331 (nr,Rb,normal2);
anr[0] = btFabs (nr[0]);
anr[1] = btFabs (nr[1]);
anr[2] = btFabs (nr[2]);
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
// are stored in a1,a2.
int lanr,a1,a2;
if (anr[1] > anr[0]) {
if (anr[1] > anr[2]) {
a1 = 0;
lanr = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else {
if (anr[0] > anr[2]) {
lanr = 0;
a1 = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
btVector3 center;
if (nr[lanr] < 0) {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
}
else {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
}
// find the normal and non-normal axis numbers of the reference box
int codeN,code1,code2;
if (code <= 3) codeN = code-1; else codeN = code-4;
if (codeN==0) {
code1 = 1;
code2 = 2;
}
else if (codeN==1) {
code1 = 0;
code2 = 2;
}
else {
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
btScalar quad[8]; // 2D coordinate of incident face (x,y pairs)
btScalar c1,c2,m11,m12,m21,m22;
c1 = dDOT14 (center,Ra+code1);
c2 = dDOT14 (center,Ra+code2);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = dDOT44 (Ra+code1,Rb+a1);
m12 = dDOT44 (Ra+code1,Rb+a2);
m21 = dDOT44 (Ra+code2,Rb+a1);
m22 = dDOT44 (Ra+code2,Rb+a2);
{
btScalar k1 = m11*Sb[a1];
btScalar k2 = m21*Sb[a1];
btScalar k3 = m12*Sb[a2];
btScalar k4 = m22*Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
btScalar rect[2];
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
btScalar ret[16];
int n = intersectRectQuad2 (rect,quad,ret);
if (n < 1) return 0; // this should never happen
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
btScalar point[3*8]; // penetrating contact points
btScalar dep[8]; // depths for those points
btScalar det1 = 1.f/(m11*m22 - m12*m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
int cnum = 0; // number of penetrating contact points found
for (j=0; j < n; j++) {
btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
for (i=0; i<3; i++) point[cnum*3+i] =
center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
if (dep[cnum] >= 0) {
ret[cnum*2] = ret[j*2];
ret[cnum*2+1] = ret[j*2+1];
cnum++;
}
}
if (cnum < 1) return 0; // this should never happen
// we can't generate more contacts than we actually have
if (maxc > cnum) maxc = cnum;
if (maxc < 1) maxc = 1;
if (cnum <= maxc) {
if (code<4)
{
// we have less contacts than we need, so we use them all
for (j=0; j < cnum; j++)
{
btVector3 pointInWorld;
for (i=0; i<3; i++)
pointInWorld[i] = point[j*3+i] + pa[i];
output.addContactPoint(-normal,pointInWorld,-dep[j]);
}
} else
{
// we have less contacts than we need, so we use them all
for (j=0; j < cnum; j++)
{
btVector3 pointInWorld;
for (i=0; i<3; i++)
pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j];
//pointInWorld[i] = point[j*3+i] + pa[i];
output.addContactPoint(-normal,pointInWorld,-dep[j]);
}
}
}
else {
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
btScalar maxdepth = dep[0];
for (i=1; i<cnum; i++) {
if (dep[i] > maxdepth) {
maxdepth = dep[i];
i1 = i;
}
}
int iret[8];
cullPoints2 (cnum,ret,maxc,i1,iret);
for (j=0; j < maxc; j++) {
// dContactGeom *con = CONTACT(contact,skip*j);
// for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
// con->depth = dep[iret[j]];
btVector3 posInWorld;
for (i=0; i<3; i++)
posInWorld[i] = point[iret[j]*3+i] + pa[i];
if (code<4)
{
output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
} else
{
output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]);
}
}
cnum = maxc;
}
*return_code = code;
return cnum;
}