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--)
365 switch (indicestype) {
366 case PHY_INTEGER: graphicsindex = gfxbase[j];
break;
367 case PHY_SHORT: graphicsindex = ((
unsigned short*)gfxbase)[j];
break;
368 case PHY_UCHAR: graphicsindex = ((
unsigned char*)gfxbase)[j];
break;
373 float* graphicsbase = (
float*)(vertexbase + graphicsindex * stride);
375 graphicsbase[0] * meshScaling.
getX(),
376 graphicsbase[1] * meshScaling.
getY(),
377 graphicsbase[2] * meshScaling.
getZ());
381 double* graphicsbase = (
double*)(vertexbase + graphicsindex * stride);
387 aabbMin.
setMin(triangleVerts[0]);
388 aabbMax.
setMax(triangleVerts[0]);
389 aabbMin.
setMin(triangleVerts[1]);
390 aabbMax.
setMax(triangleVerts[1]);
391 aabbMin.
setMin(triangleVerts[2]);
392 aabbMax.
setMax(triangleVerts[2]);
395 connectivityProcessor.
m_partIdA = partId;
436 nearestPoint = line0;
440 btScalar delta = (point - line0).
dot(lineDelta) / (lineDelta).
dot(lineDelta);
448 nearestPoint = line0 + lineDelta * delta;
461 if (correctedEdgeAngle < 0)
463 if (curAngle < correctedEdgeAngle)
465 btScalar diffAngle = correctedEdgeAngle - curAngle;
467 clampedLocalNormal =
btMatrix3x3(rotation) * localContactNormalOnB;
472 if (correctedEdgeAngle >= 0)
474 if (curAngle > correctedEdgeAngle)
476 btScalar diffAngle = correctedEdgeAngle - curAngle;
478 clampedLocalNormal =
btMatrix3x3(rotation) * localContactNormalOnB;
501 #ifdef USE_HEIGHTFIELD_TRIANGLES
507 newNormal = tri_normal;
537 if (!triangleInfoMapPtr)
556 btVector3 red(1, 0, 0), green(0, 1, 0), blue(0, 0, 1), white(1, 1, 1), black(0, 0, 0);
565 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
566 const btTransform& tr = colObj0->getWorldTransform();
568 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
570 bool isNearEdge =
false;
572 int numConcaveEdgeHits = 0;
573 int numConvexEdgeHits = 0;
589 if (len < disttobestedge)
592 disttobestedge = len;
602 if (len < disttobestedge)
605 disttobestedge = len;
615 if (len < disttobestedge)
618 disttobestedge = len;
622 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
624 btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
628 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
629 btDebugDrawLine(tr * contact, tr * (contact + cp.
m_normalWorldOnB * 10), black);
632 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
640 numConcaveEdgeHits++;
646 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
647 btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
648 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
655 computedNormalB *= -1;
656 btVector3 nB = swapFactor * computedNormalB;
660 bool backFacingNormal = (NdotA < triangleInfoMapPtr->
m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
662 #ifdef DEBUG_INTERNAL_EDGE
666 #endif //DEBUG_INTERNAL_EDGE
668 if (backFacingNormal)
670 numConcaveEdgeHits++;
695 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
697 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
699 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
700 btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
705 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
706 btDebugDrawLine(tr * contact, tr * (contact + cp.
m_normalWorldOnB * 10), black);
707 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
710 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
714 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
715 btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
716 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
724 numConcaveEdgeHits++;
730 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
731 btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
732 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
739 computedNormalB *= -1;
740 btVector3 nB = swapFactor * computedNormalB;
742 #ifdef DEBUG_INTERNAL_EDGE
746 #endif //DEBUG_INTERNAL_EDGE
750 bool backFacingNormal = (NdotA < triangleInfoMapPtr->
m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
752 if (backFacingNormal)
754 numConcaveEdgeHits++;
780 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
782 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
783 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
784 btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
789 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
790 btDebugDrawLine(tr * contact, tr * (contact + cp.
m_normalWorldOnB * 10), black);
791 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
794 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
798 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
799 btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
800 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
806 numConcaveEdgeHits++;
812 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
813 btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
814 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
820 computedNormalB *= -1;
821 btVector3 nB = swapFactor * computedNormalB;
823 #ifdef DEBUG_INTERNAL_EDGE
827 #endif //DEBUG_INTERNAL_EDGE
831 bool backFacingNormal = (NdotA < triangleInfoMapPtr->
m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
833 if (backFacingNormal)
835 numConcaveEdgeHits++;
862 #ifdef DEBUG_INTERNAL_EDGE
867 #endif //DEBUG_INTERNAL_EDGE
871 if (numConcaveEdgeHits > 0)
876 if (tri_normal.
dot(localContactNormalOnB) < 0)
884 btVector3 newNormal = tri_normal * frontFacing;