15 #ifdef DEBUG_INTERNAL_EDGE
17 #endif //DEBUG_INTERNAL_EDGE
19 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
24 gDebugDrawer = debugDrawer;
30 gDebugDrawer->
drawLine(from, to, color);
32 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
68 int sharedVertsA[3] = {-1, -1, -1};
69 int sharedVertsB[3] = {-1, -1, -1};
72 btScalar crossBSqr = ((triangle[1] - triangle[0]).cross(triangle[2] - triangle[0])).length2();
73 if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)
78 if (crossASqr < m_triangleInfoMap->m_equalVertexThreshold)
82 printf(
"triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n",
87 printf(
"partId=%d, triangleIndex=%d\n",partId,triangleIndex);
88 printf(
"triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n",
89 triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(),
90 triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(),
91 triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());
94 for (
int i = 0; i < 3; i++)
96 for (
int j = 0; j < 3; j++)
100 sharedVertsA[numshared] = i;
101 sharedVertsB[numshared] = j;
127 if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
131 int tmp = sharedVertsB[1];
132 sharedVertsB[1] = sharedVertsB[0];
133 sharedVertsB[0] = tmp;
146 int sumvertsA = sharedVertsA[0] + sharedVertsA[1];
147 int otherIndexA = 3 - sumvertsA;
152 int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]);
154 btTriangleShape tB(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]);
166 if (edgeCrossA.
dot(tmp) < 0)
175 btVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]];
176 if (edgeCrossB.
dot(tmp) < 0)
190 bool isConvex =
false;
192 if (len2 < m_triangleInfoMap->m_planarEpsilon)
202 angle2 =
btGetAngle(calculatedNormalA, edgeCrossA, edgeCrossB);
206 isConvex = (dotA < 0.);
208 correctedAngle = isConvex ? ang4 : -ang4;
224 computedNormalB *= -1;
227 #ifdef DEBUG_INTERNAL_EDGE
228 if ((computedNormalB - normalB).
length() > 0.0001)
230 printf(
"warning: normals not identical\n");
232 #endif //DEBUG_INTERNAL_EDGE
245 if (computedNormalB.
dot(normalB) < 0)
247 computedNormalB *= -1;
251 #ifdef DEBUG_INTERNAL_EDGE
252 if ((computedNormalB - normalB).length() > 0.0001)
254 printf(
"warning: normals not identical\n");
256 #endif //DEBUG_INTERNAL_EDGE
267 if (computedNormalB.
dot(normalB) < 0)
270 computedNormalB *= -1;
272 #ifdef DEBUG_INTERNAL_EDGE
273 if ((computedNormalB - normalB).length() > 0.0001)
275 printf(
"warning: normals not identical\n");
277 #endif //DEBUG_INTERNAL_EDGE
311 connectivityProcessor.
m_partIdA = partId;
318 aabbMin.
setMin(triangle[0]);
319 aabbMax.
setMax(triangle[0]);
320 aabbMin.
setMin(triangle[1]);
321 aabbMax.
setMax(triangle[1]);
322 aabbMin.
setMin(triangle[2]);
323 aabbMax.
setMax(triangle[2]);
342 for (
int partId = 0; partId < meshInterface->
getNumSubParts(); partId++)
344 const unsigned char* vertexbase = 0;
348 const unsigned char* indexbase = 0;
358 for (
int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
360 unsigned int* gfxbase = (
unsigned int*)(indexbase + triangleIndex * indexstride);
362 for (
int j = 2; j >= 0; j--)
364 int graphicsindex = indicestype ==
PHY_SHORT ? ((
unsigned short*)gfxbase)[j] : gfxbase[j];
367 float* graphicsbase = (
float*)(vertexbase + graphicsindex * stride);
369 graphicsbase[0] * meshScaling.
getX(),
370 graphicsbase[1] * meshScaling.
getY(),
371 graphicsbase[2] * meshScaling.
getZ());
375 double* graphicsbase = (
double*)(vertexbase + graphicsindex * stride);
381 aabbMin.
setMin(triangleVerts[0]);
382 aabbMax.
setMax(triangleVerts[0]);
383 aabbMin.
setMin(triangleVerts[1]);
384 aabbMax.
setMax(triangleVerts[1]);
385 aabbMin.
setMin(triangleVerts[2]);
386 aabbMax.
setMax(triangleVerts[2]);
389 connectivityProcessor.
m_partIdA = partId;
430 nearestPoint = line0;
434 btScalar delta = (point - line0).
dot(lineDelta) / (lineDelta).
dot(lineDelta);
442 nearestPoint = line0 + lineDelta * delta;
455 if (correctedEdgeAngle < 0)
457 if (curAngle < correctedEdgeAngle)
459 btScalar diffAngle = correctedEdgeAngle - curAngle;
461 clampedLocalNormal =
btMatrix3x3(rotation) * localContactNormalOnB;
466 if (correctedEdgeAngle >= 0)
468 if (curAngle > correctedEdgeAngle)
470 btScalar diffAngle = correctedEdgeAngle - curAngle;
472 clampedLocalNormal =
btMatrix3x3(rotation) * localContactNormalOnB;
495 #ifdef USE_HEIGHTFIELD_TRIANGLES
501 newNormal = tri_normal;
531 if (!triangleInfoMapPtr)
550 btVector3 red(1, 0, 0), green(0, 1, 0), blue(0, 0, 1), white(1, 1, 1), black(0, 0, 0);
559 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
560 const btTransform& tr = colObj0->getWorldTransform();
562 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
564 bool isNearEdge =
false;
566 int numConcaveEdgeHits = 0;
567 int numConvexEdgeHits = 0;
583 if (len < disttobestedge)
586 disttobestedge = len;
596 if (len < disttobestedge)
599 disttobestedge = len;
609 if (len < disttobestedge)
612 disttobestedge = len;
616 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
618 btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
622 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
623 btDebugDrawLine(tr * contact, tr * (contact + cp.
m_normalWorldOnB * 10), black);
626 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
634 numConcaveEdgeHits++;
640 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
641 btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
642 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
649 computedNormalB *= -1;
650 btVector3 nB = swapFactor * computedNormalB;
654 bool backFacingNormal = (NdotA < triangleInfoMapPtr->
m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
656 #ifdef DEBUG_INTERNAL_EDGE
660 #endif //DEBUG_INTERNAL_EDGE
662 if (backFacingNormal)
664 numConcaveEdgeHits++;
689 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
691 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
693 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
694 btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
699 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
700 btDebugDrawLine(tr * contact, tr * (contact + cp.
m_normalWorldOnB * 10), black);
701 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
704 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
708 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
709 btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
710 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
718 numConcaveEdgeHits++;
724 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
725 btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
726 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
733 computedNormalB *= -1;
734 btVector3 nB = swapFactor * computedNormalB;
736 #ifdef DEBUG_INTERNAL_EDGE
740 #endif //DEBUG_INTERNAL_EDGE
744 bool backFacingNormal = (NdotA < triangleInfoMapPtr->
m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
746 if (backFacingNormal)
748 numConcaveEdgeHits++;
774 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
776 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
777 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
778 btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
783 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
784 btDebugDrawLine(tr * contact, tr * (contact + cp.
m_normalWorldOnB * 10), black);
785 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
788 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
792 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
793 btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
794 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
800 numConcaveEdgeHits++;
806 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
807 btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
808 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
814 computedNormalB *= -1;
815 btVector3 nB = swapFactor * computedNormalB;
817 #ifdef DEBUG_INTERNAL_EDGE
821 #endif //DEBUG_INTERNAL_EDGE
825 bool backFacingNormal = (NdotA < triangleInfoMapPtr->
m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
827 if (backFacingNormal)
829 numConcaveEdgeHits++;
856 #ifdef DEBUG_INTERNAL_EDGE
861 #endif //DEBUG_INTERNAL_EDGE
865 if (numConcaveEdgeHits > 0)
870 if (tri_normal.
dot(localContactNormalOnB) < 0)
878 btVector3 newNormal = tri_normal * frontFacing;