21 #if defined(DEBUG) || defined(_DEBUG) 
   25 #include <spu_printf.h> 
   26 #define printf spu_printf 
   31 #ifdef BT_USE_DOUBLE_PRECISION 
   32 #define REL_ERROR2 btScalar(1.0e-12) 
   35 #define REL_ERROR2 btScalar(1.0e-6) 
   42           m_penetrationDepthSolver(penetrationDepthSolver),
 
   43           m_simplexSolver(simplexSolver),
 
   44           m_minkowskiA(objectA),
 
   45           m_minkowskiB(objectB),
 
   46           m_shapeTypeA(objectA->getShapeType()),
 
   47           m_shapeTypeB(objectB->getShapeType()),
 
   48           m_marginA(objectA->getMargin()),
 
   49           m_marginB(objectB->getMargin()),
 
   50           m_ignoreMargin(false),
 
   52           m_catchDegeneracies(1),
 
   53           m_fixContactNormalDirection(1)
 
   58           m_penetrationDepthSolver(penetrationDepthSolver),
 
   59           m_simplexSolver(simplexSolver),
 
   60           m_minkowskiA(objectA),
 
   61           m_minkowskiB(objectB),
 
   62           m_shapeTypeA(shapeTypeA),
 
   63           m_shapeTypeB(shapeTypeB),
 
   66           m_ignoreMargin(false),
 
   68           m_catchDegeneracies(1),
 
   69           m_fixContactNormalDirection(1)
 
   91         supAworld = localTransA(pInA);
 
   92         supBworld = localTransB(qInB);
 
  100         aMinb = supAworld - supBworld;
 
  350         double u, v, w, p, q, r;
 
  351         double s, t, dist, dist2;
 
  365         s = (q * r - w * p) / (w * v - r * r);
 
  366         t = (-s * r - q) / w;
 
  573         int B_on_ACD, C_on_ADB, D_on_ABC;
 
  574         int AB_O, AC_O, AD_O;
 
  630         if (AB_O && AC_O && AD_O)
 
  691         m_cachedSeparatingDistance = 0.f;
 
  700         localTransA.
getOrigin() -= positionOffset;
 
  701         localTransB.
getOrigin() -= positionOffset;
 
  703         bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
 
  717         int gGjkMaxIter = 1000;  
 
  718         m_cachedSeparatingAxis.setValue(0, 1, 0);
 
  720         bool isValid = 
false;
 
  721         bool checkSimplex = 
false;
 
  722         bool checkPenetration = 
true;
 
  723         m_degenerateSimplex = 0;
 
  725         m_lastUsedMethod = -1;
 
  728         btScalar margin = marginA + marginB;
 
  750                         btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
 
  762                         for (
int iterations = 0; iterations < gGjkMaxIter; iterations++)
 
  765                                 btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
 
  791                                 if (do_simplex_res == 1)
 
  796                                 else if (do_simplex_res == -1)
 
  825                 m_simplexSolver->reset();
 
  842                                 btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.
getBasis();
 
  845                                 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 
  846                                 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
 
  858                                 delta = m_cachedSeparatingAxis.
dot(w);
 
  863                                         m_degenerateSimplex = 10;
 
  870                                 if (m_simplexSolver->inSimplex(w))
 
  872                                         m_degenerateSimplex = 1;
 
  877                                 btScalar f0 = squaredDistance - delta;
 
  884                                                 m_degenerateSimplex = 2;
 
  888                                                 m_degenerateSimplex = 11;
 
  895                                 m_simplexSolver->addVertex(w, pWorld, qWorld);
 
  899                                 if (!m_simplexSolver->closest(newCachedSeparatingAxis))
 
  901                                         m_degenerateSimplex = 3;
 
  908                                         m_cachedSeparatingAxis = newCachedSeparatingAxis;
 
  909                                         m_degenerateSimplex = 6;
 
  914                                 btScalar previousSquaredDistance = squaredDistance;
 
  915                                 squaredDistance = newCachedSeparatingAxis.
length2();
 
  917                                 if (squaredDistance > previousSquaredDistance)
 
  920                                         m_degenerateSimplex = 7;
 
  921                                         squaredDistance = previousSquaredDistance;
 
  922                                         checkSimplex = 
false;
 
  930                                 if (previousSquaredDistance - squaredDistance <= 
SIMD_EPSILON * previousSquaredDistance)
 
  934                                         m_degenerateSimplex = 12;
 
  939                                 m_cachedSeparatingAxis = newCachedSeparatingAxis;
 
  942                                 if (m_curIter++ > gGjkMaxIter)
 
  944 #if defined(DEBUG) || defined(_DEBUG) 
  946                                         printf(
"btGjkPairDetector maxIter exceeded:%i\n", m_curIter);
 
  947                                         printf(
"sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
 
  948                                                    m_cachedSeparatingAxis.getX(),
 
  949                                                    m_cachedSeparatingAxis.getY(),
 
  950                                                    m_cachedSeparatingAxis.getZ(),
 
  952                                                    m_minkowskiA->getShapeType(),
 
  953                                                    m_minkowskiB->getShapeType());
 
  959                                 bool check = (!m_simplexSolver->fullSimplex());
 
  966                                         m_degenerateSimplex = 13;
 
  973                                 m_simplexSolver->compute_points(pointOnA, pointOnB);
 
  974                                 normalInB = m_cachedSeparatingAxis;
 
  981                                         m_degenerateSimplex = 5;
 
  991                                         pointOnA -= m_cachedSeparatingAxis * (marginA / s);
 
  992                                         pointOnB += m_cachedSeparatingAxis * (marginB / s);
 
  993                                         distance = ((
btScalar(1.) / rlen) - margin);
 
  995                                         orgNormalInB = normalInB;
 
  997                                         m_lastUsedMethod = 1;
 
 1001                                         m_lastUsedMethod = 2;
 
 1006                 bool catchDegeneratePenetrationCase =
 
 1010                 if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase)) || (status == 0))
 
 1015                         if (m_penetrationDepthSolver)
 
 1020                                 m_cachedSeparatingAxis.
setZero();
 
 1022                                 bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
 
 1024                                         m_minkowskiA, m_minkowskiB,
 
 1025                                         localTransA, localTransB,
 
 1026                                         m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
 
 1029                                 if (m_cachedSeparatingAxis.length2())
 
 1033                                                 btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
 
 1037                                                         tmpNormalInB = m_cachedSeparatingAxis;
 
 1038                                                         lenSqr = m_cachedSeparatingAxis.
length2();
 
 1043                                                         tmpNormalInB /= 
btSqrt(lenSqr);
 
 1045                                                         m_lastUsedMethod = 3;
 
 1047                                                         if (!isValid || (distance2 < distance))
 
 1049                                                                 distance = distance2;
 
 1050                                                                 pointOnA = tmpPointOnA;
 
 1051                                                                 pointOnB = tmpPointOnB;
 
 1052                                                                 normalInB = tmpNormalInB;
 
 1057                                                                 m_lastUsedMethod = 8;
 
 1062                                                         m_lastUsedMethod = 9;
 
 1074                                                 if (m_cachedSeparatingAxis.length2() > 
btScalar(0.))
 
 1076                                                         btScalar distance2 = (tmpPointOnA - tmpPointOnB).
length() - margin;
 
 1078                                                         if (!isValid || (distance2 < distance))
 
 1080                                                                 distance = distance2;
 
 1081                                                                 pointOnA = tmpPointOnA;
 
 1082                                                                 pointOnB = tmpPointOnB;
 
 1083                                                                 pointOnA -= m_cachedSeparatingAxis * marginA;
 
 1084                                                                 pointOnB += m_cachedSeparatingAxis * marginB;
 
 1085                                                                 normalInB = m_cachedSeparatingAxis;
 
 1089                                                                 m_lastUsedMethod = 6;
 
 1093                                                                 m_lastUsedMethod = 5;
 
 1108                 m_cachedSeparatingAxis = normalInB;
 
 1109                 m_cachedSeparatingDistance = distance;
 
 1122                                 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 
 1123                                 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
 
 1128                                 d2 = orgNormalInB.
dot(w) - margin;
 
 1136                                 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 
 1137                                 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
 
 1142                                 d1 = (-normalInB).
dot(w) - margin;
 
 1149                                 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 
 1150                                 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
 
 1155                                 d0 = normalInB.
dot(w) - margin;
 
 1160                                 m_lastUsedMethod = 10;
 
 1166                                 if (d2 > d0 && d2 > d1 && d2 > distance)
 
 1168                                         normalInB = orgNormalInB;
 
 1176                         pointOnB + positionOffset,