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 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.
getBasis();
845 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
846 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
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();
918 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(separatingAxisInA);
1123 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
1128 d2 = orgNormalInB.
dot(w) - margin;
1136 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
1137 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
1142 d1 = (-normalInB).
dot(w) - margin;
1149 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
1150 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
1155 d0 = normalInB.
dot(w) - margin;
1160 m_lastUsedMethod = 10;
1166 if (d2 > d0 && d2 > d1 && d2 > distance)
1168 normalInB = orgNormalInB;
1176 pointOnB + positionOffset,