13 #ifdef DEBUG_INTERNAL_EDGE
15 #endif //DEBUG_INTERNAL_EDGE
17 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
22 gDebugDrawer = debugDrawer;
28 gDebugDrawer->
drawLine(from, to, color);
30 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
66 int sharedVertsA[3] = {-1, -1, -1};
67 int sharedVertsB[3] = {-1, -1, -1};
70 btScalar crossBSqr = ((triangle[1] - triangle[0]).cross(triangle[2] - triangle[0])).length2();
71 if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)
76 if (crossASqr < m_triangleInfoMap->m_equalVertexThreshold)
80 printf(
"triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n",
85 printf(
"partId=%d, triangleIndex=%d\n",partId,triangleIndex);
86 printf(
"triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n",
87 triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(),
88 triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(),
89 triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());
92 for (
int i = 0; i < 3; i++)
94 for (
int j = 0; j < 3; j++)
98 sharedVertsA[numshared] = i;
99 sharedVertsB[numshared] = j;
125 if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
129 int tmp = sharedVertsB[1];
130 sharedVertsB[1] = sharedVertsB[0];
131 sharedVertsB[0] = tmp;
144 int sumvertsA = sharedVertsA[0] + sharedVertsA[1];
145 int otherIndexA = 3 - sumvertsA;
150 int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]);
152 btTriangleShape tB(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]);
164 if (edgeCrossA.
dot(tmp) < 0)
173 btVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]];
174 if (edgeCrossB.
dot(tmp) < 0)
188 bool isConvex =
false;
190 if (len2 < m_triangleInfoMap->m_planarEpsilon)
200 angle2 =
btGetAngle(calculatedNormalA, edgeCrossA, edgeCrossB);
204 isConvex = (dotA < 0.);
206 correctedAngle = isConvex ? ang4 : -ang4;
222 computedNormalB *= -1;
225 #ifdef DEBUG_INTERNAL_EDGE
226 if ((computedNormalB - normalB).
length() > 0.0001)
228 printf(
"warning: normals not identical\n");
230 #endif //DEBUG_INTERNAL_EDGE
243 if (computedNormalB.
dot(normalB) < 0)
245 computedNormalB *= -1;
249 #ifdef DEBUG_INTERNAL_EDGE
250 if ((computedNormalB - normalB).length() > 0.0001)
252 printf(
"warning: normals not identical\n");
254 #endif //DEBUG_INTERNAL_EDGE
265 if (computedNormalB.
dot(normalB) < 0)
268 computedNormalB *= -1;
270 #ifdef DEBUG_INTERNAL_EDGE
271 if ((computedNormalB - normalB).length() > 0.0001)
273 printf(
"warning: normals not identical\n");
275 #endif //DEBUG_INTERNAL_EDGE
307 for (
int partId = 0; partId < meshInterface->
getNumSubParts(); partId++)
309 const unsigned char* vertexbase = 0;
313 const unsigned char* indexbase = 0;
323 for (
int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
325 unsigned int* gfxbase = (
unsigned int*)(indexbase + triangleIndex * indexstride);
327 for (
int j = 2; j >= 0; j--)
329 int graphicsindex = indicestype ==
PHY_SHORT ? ((
unsigned short*)gfxbase)[j] : gfxbase[j];
332 float* graphicsbase = (
float*)(vertexbase + graphicsindex * stride);
334 graphicsbase[0] * meshScaling.
getX(),
335 graphicsbase[1] * meshScaling.
getY(),
336 graphicsbase[2] * meshScaling.
getZ());
340 double* graphicsbase = (
double*)(vertexbase + graphicsindex * stride);
346 aabbMin.
setMin(triangleVerts[0]);
347 aabbMax.
setMax(triangleVerts[0]);
348 aabbMin.
setMin(triangleVerts[1]);
349 aabbMax.
setMax(triangleVerts[1]);
350 aabbMin.
setMin(triangleVerts[2]);
351 aabbMax.
setMax(triangleVerts[2]);
354 connectivityProcessor.
m_partIdA = partId;
373 nearestPoint = line0;
377 btScalar delta = (point - line0).
dot(lineDelta) / (lineDelta).
dot(lineDelta);
385 nearestPoint = line0 + lineDelta * delta;
398 if (correctedEdgeAngle < 0)
400 if (curAngle < correctedEdgeAngle)
402 btScalar diffAngle = correctedEdgeAngle - curAngle;
404 clampedLocalNormal =
btMatrix3x3(rotation) * localContactNormalOnB;
409 if (correctedEdgeAngle >= 0)
411 if (curAngle > correctedEdgeAngle)
413 btScalar diffAngle = correctedEdgeAngle - curAngle;
415 clampedLocalNormal =
btMatrix3x3(rotation) * localContactNormalOnB;
446 if (!triangleInfoMapPtr)
465 btVector3 red(1, 0, 0), green(0, 1, 0), blue(0, 0, 1), white(1, 1, 1), black(0, 0, 0);
474 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
475 const btTransform& tr = colObj0->getWorldTransform();
477 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
479 bool isNearEdge =
false;
481 int numConcaveEdgeHits = 0;
482 int numConvexEdgeHits = 0;
498 if (len < disttobestedge)
501 disttobestedge = len;
511 if (len < disttobestedge)
514 disttobestedge = len;
524 if (len < disttobestedge)
527 disttobestedge = len;
531 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
533 btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
537 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
538 btDebugDrawLine(tr * contact, tr * (contact + cp.
m_normalWorldOnB * 10), black);
541 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
549 numConcaveEdgeHits++;
555 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
556 btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
557 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
564 computedNormalB *= -1;
565 btVector3 nB = swapFactor * computedNormalB;
569 bool backFacingNormal = (NdotA < triangleInfoMapPtr->
m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
571 #ifdef DEBUG_INTERNAL_EDGE
575 #endif //DEBUG_INTERNAL_EDGE
577 if (backFacingNormal)
579 numConcaveEdgeHits++;
604 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
606 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
608 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
609 btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
614 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
615 btDebugDrawLine(tr * contact, tr * (contact + cp.
m_normalWorldOnB * 10), black);
616 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
619 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
623 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
624 btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
625 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
633 numConcaveEdgeHits++;
639 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
640 btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
641 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
648 computedNormalB *= -1;
649 btVector3 nB = swapFactor * computedNormalB;
651 #ifdef DEBUG_INTERNAL_EDGE
655 #endif //DEBUG_INTERNAL_EDGE
659 bool backFacingNormal = (NdotA < triangleInfoMapPtr->
m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
661 if (backFacingNormal)
663 numConcaveEdgeHits++;
689 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
691 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
692 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
693 btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
698 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
699 btDebugDrawLine(tr * contact, tr * (contact + cp.
m_normalWorldOnB * 10), black);
700 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
703 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
707 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
708 btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
709 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
715 numConcaveEdgeHits++;
721 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
722 btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
723 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
729 computedNormalB *= -1;
730 btVector3 nB = swapFactor * computedNormalB;
732 #ifdef DEBUG_INTERNAL_EDGE
736 #endif //DEBUG_INTERNAL_EDGE
740 bool backFacingNormal = (NdotA < triangleInfoMapPtr->
m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
742 if (backFacingNormal)
744 numConcaveEdgeHits++;
771 #ifdef DEBUG_INTERNAL_EDGE
776 #endif //DEBUG_INTERNAL_EDGE
780 if (numConcaveEdgeHits > 0)
785 if (tri_normal.
dot(localContactNormalOnB) < 0)
793 btVector3 newNormal = tri_normal * frontFacing;