13 #ifdef DEBUG_INTERNAL_EDGE 15 #endif //DEBUG_INTERNAL_EDGE 18 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 23 gDebugDrawer = debugDrawer;
29 gDebugDrawer->
drawLine(from,to,color);
31 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 63 if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
72 int sharedVertsA[3]={-1,-1,-1};
73 int sharedVertsB[3]={-1,-1,-1};
76 btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2();
77 if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)
81 btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2();
83 if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold)
87 printf(
"triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n",
88 m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(),
89 m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(),
90 m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ());
92 printf(
"partId=%d, triangleIndex=%d\n",partId,triangleIndex);
93 printf(
"triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n",
94 triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(),
95 triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(),
96 triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());
101 for (
int j=0;j<3;j++)
105 sharedVertsA[numshared] = i;
106 sharedVertsB[numshared] = j;
132 if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
136 int tmp = sharedVertsB[1];
137 sharedVertsB[1] = sharedVertsB[0];
138 sharedVertsB[0] = tmp;
141 int hash =
btGetHash(m_partIdA,m_triangleIndexA);
147 m_triangleInfoMap->
insert(hash,tmp);
148 info = m_triangleInfoMap->
find(hash);
151 int sumvertsA = sharedVertsA[0]+sharedVertsA[1];
152 int otherIndexA = 3-sumvertsA;
155 btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]);
157 btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]);
158 int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]);
160 btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]);
171 btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]];
172 if (edgeCrossA.
dot(tmp) < 0)
181 btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]];
182 if (edgeCrossB.
dot(tmp) < 0)
197 bool isConvex =
false;
199 if (len2<m_triangleInfoMap->m_planarEpsilon)
209 angle2 =
btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB);
213 isConvex = (dotA<0.);
215 correctedAngle = isConvex ? ang4 : -ang4;
230 btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1];
239 #ifdef DEBUG_INTERNAL_EDGE 240 if ((computedNormalB-normalB).
length()>0.0001)
242 printf(
"warning: normals not identical\n");
244 #endif//DEBUG_INTERNAL_EDGE 254 btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0];
257 if (computedNormalB.
dot(normalB)<0)
263 #ifdef DEBUG_INTERNAL_EDGE 264 if ((computedNormalB-normalB).length()>0.0001)
266 printf(
"warning: normals not identical\n");
268 #endif //DEBUG_INTERNAL_EDGE 276 btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2];
279 if (computedNormalB.
dot(normalB)<0)
284 #ifdef DEBUG_INTERNAL_EDGE 285 if ((computedNormalB-normalB).length()>0.0001)
287 printf(
"warning: normals not identical\n");
289 #endif //DEBUG_INTERNAL_EDGE 322 for (
int partId = 0; partId< meshInterface->
getNumSubParts();partId++)
324 const unsigned char *vertexbase = 0;
328 const unsigned char *indexbase = 0;
338 for (
int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)
340 unsigned int* gfxbase = (
unsigned int*)(indexbase+triangleIndex*indexstride);
342 for (
int j=2;j>=0;j--)
345 int graphicsindex = indicestype==
PHY_SHORT?((
unsigned short*)gfxbase)[j]:gfxbase[j];
348 float* graphicsbase = (
float*)(vertexbase+graphicsindex*stride);
350 graphicsbase[0]*meshScaling.
getX(),
351 graphicsbase[1]*meshScaling.
getY(),
352 graphicsbase[2]*meshScaling.
getZ());
356 double* graphicsbase = (
double*)(vertexbase+graphicsindex*stride);
362 aabbMin.
setMin(triangleVerts[0]);
363 aabbMax.
setMax(triangleVerts[0]);
364 aabbMin.
setMin(triangleVerts[1]);
365 aabbMax.
setMax(triangleVerts[1]);
366 aabbMin.
setMin(triangleVerts[2]);
367 aabbMax.
setMax(triangleVerts[2]);
370 connectivityProcessor.
m_partIdA = partId;
394 nearestPoint = line0;
398 btScalar delta = (point-line0).
dot(lineDelta) / (lineDelta).
dot(lineDelta);
403 else if ( delta > 1 )
406 nearestPoint = line0 + lineDelta*delta;
422 if (correctedEdgeAngle<0)
424 if (curAngle < correctedEdgeAngle)
426 btScalar diffAngle = correctedEdgeAngle-curAngle;
428 clampedLocalNormal =
btMatrix3x3(rotation)*localContactNormalOnB;
433 if (correctedEdgeAngle>=0)
435 if (curAngle > correctedEdgeAngle)
437 btScalar diffAngle = correctedEdgeAngle-curAngle;
439 clampedLocalNormal =
btMatrix3x3(rotation)*localContactNormalOnB;
463 if (!triangleInfoMapPtr)
483 btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
492 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 493 const btTransform& tr = colObj0->getWorldTransform();
495 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 499 bool isNearEdge =
false;
501 int numConcaveEdgeHits = 0;
502 int numConvexEdgeHits = 0;
518 if( len < disttobestedge )
531 if( len < disttobestedge )
544 if( len < disttobestedge )
551 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 553 btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red );
557 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 561 if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
564 btVector3 edge(v0-v1);
569 numConcaveEdgeHits++;
575 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 576 btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
577 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 579 btVector3 nA = swapFactor * tri_normal;
582 btVector3 computedNormalB =
quatRotate(orn,tri_normal);
585 btVector3 nB = swapFactor*computedNormalB;
587 btScalar NdotA = localContactNormalOnB.
dot(nA);
588 btScalar NdotB = localContactNormalOnB.
dot(nB);
589 bool backFacingNormal = (NdotA< triangleInfoMapPtr->
m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
591 #ifdef DEBUG_INTERNAL_EDGE 596 #endif //DEBUG_INTERNAL_EDGE 599 if (backFacingNormal)
601 numConcaveEdgeHits++;
606 btVector3 clampedLocalNormal;
627 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 629 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 631 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 632 btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green );
637 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 639 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 644 if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
648 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 649 btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
650 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 652 btVector3 edge(v1-v2);
658 numConcaveEdgeHits++;
663 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 664 btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
665 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 667 btVector3 nA = swapFactor * tri_normal;
670 btVector3 computedNormalB =
quatRotate(orn,tri_normal);
673 btVector3 nB = swapFactor*computedNormalB;
675 #ifdef DEBUG_INTERNAL_EDGE 679 #endif //DEBUG_INTERNAL_EDGE 682 btScalar NdotA = localContactNormalOnB.
dot(nA);
683 btScalar NdotB = localContactNormalOnB.
dot(nB);
684 bool backFacingNormal = (NdotA< triangleInfoMapPtr->
m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
686 if (backFacingNormal)
688 numConcaveEdgeHits++;
694 btVector3 clampedLocalNormal;
714 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 716 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 717 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 718 btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue );
724 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 726 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 729 if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
733 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 734 btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
735 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 737 btVector3 edge(v2-v0);
741 numConcaveEdgeHits++;
747 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 748 btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
749 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 751 btVector3 nA = swapFactor * tri_normal;
753 btVector3 computedNormalB =
quatRotate(orn,tri_normal);
756 btVector3 nB = swapFactor*computedNormalB;
758 #ifdef DEBUG_INTERNAL_EDGE 762 #endif //DEBUG_INTERNAL_EDGE 764 btScalar NdotA = localContactNormalOnB.
dot(nA);
765 btScalar NdotB = localContactNormalOnB.
dot(nB);
766 bool backFacingNormal = (NdotA< triangleInfoMapPtr->
m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
768 if (backFacingNormal)
770 numConcaveEdgeHits++;
779 btVector3 clampedLocalNormal;
800 #ifdef DEBUG_INTERNAL_EDGE 802 btVector3 color(0,1,1);
805 #endif //DEBUG_INTERNAL_EDGE 810 if (numConcaveEdgeHits>0)
815 if (tri_normal.
dot(localContactNormalOnB) < 0)
822 btVector3 newNormal = tri_normal *frontFacing;
824 btScalar d = newNormal.
dot(localContactNormalOnB) ;
const btTriangleInfoMap * getTriangleInfoMap() const
void btNearestPointInLineSegment(const btVector3 &point, const btVector3 &line0, const btVector3 &line1, btVector3 &nearestPoint)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
#define TRI_INFO_V0V1_SWAP_NORMALB
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
#define TRI_INFO_V1V2_CONVEX
const Value * find(const Key &key) const
btScalar length2() const
Return the length of the vector squared.
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
btScalar m_maxEdgeAngleThreshold
used to determine edge contacts: if the closest distance between a contact point and an edge is small...
const btScalar & getY() const
Return the y value.
static int btGetHash(int partId, int triangleIndex)
ManifoldContactPoint collects and maintains persistent contactpoints.
const btCollisionShape * getCollisionShape() const
btScalar m_equalVertexThreshold
used to determine if a triangle edge is planar with zero angle
#define TRI_INFO_V1V2_SWAP_NORMALB
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
btMatrix3x3 transpose() const
Return the transpose of the matrix.
const btScalar & getZ() const
Return the z value.
The btBvhTriangleMeshShape is a static-triangle mesh shape, it can only be used for fixed/non-moving ...
#define MAX_NUM_PARTS_IN_BITS
void btAdjustInternalEdgeContacts(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, const btCollisionObjectWrapper *colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
Changes a btManifoldPoint collision normal to the normal from the mesh.
btVector3 m_normalWorldOnB
btVector3 m_positionWorldOnB
const btVector3 & getPositionWorldOnB() const
btTriangleInfoMap * m_triangleInfoMap
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
btScalar dot(const btVector3 &v) const
Return the dot product.
const btCollisionObject * getCollisionObject() const
btScalar btAtan2(btScalar x, btScalar y)
void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape *trimeshShape, btTriangleInfoMap *triangleInfoMap)
Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo'.
The btTriangleInfo structure stores information to adjust collision normals to avoid collisions again...
void insert(const Key &key, const Value &value)
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
btVector3 m_positionWorldOnA
m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity ...
btVector3 can be used to represent 3D points and vectors.
virtual int getNumSubParts() const =0
getNumSubParts returns the number of seperate subparts each subpart has a continuous array of vertice...
#define TRI_INFO_V0V1_CONVEX
for btTriangleInfo m_flags
#define TRI_INFO_V2V0_SWAP_NORMALB
The btStridingMeshInterface is the interface class for high performance generic access to triangle me...
virtual void processAllTriangles(btTriangleCallback *callback, const btVector3 &aabbMin, const btVector3 &aabbMax) const
const btVector3 & getScaling() const
const btTransform & getWorldTransform() const
static btScalar btGetAngle(const btVector3 &edgeA, const btVector3 &normalA, const btVector3 &normalB)
#define TRI_INFO_V2V0_CONVEX
virtual void getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int &numverts, PHY_ScalarType &type, int &stride, const unsigned char **indexbase, int &indexstride, int &numfaces, PHY_ScalarType &indicestype, int subpart=0) const =0
bool btClampNormal(const btVector3 &edge, const btVector3 &tri_normal_org, const btVector3 &localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 &clampedLocalNormal)
The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMesh...
void setTriangleInfoMap(btTriangleInfoMap *triangleInfoMap)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
The btTriangleInfoMap stores edge angle information for some triangles. You can compute this informat...
virtual void getVertex(int index, btVector3 &vert) const
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
btStridingMeshInterface * getMeshInterface()
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
btVector3 * m_triangleVerticesA
void calcNormal(btVector3 &normal) const
const btScalar & getX() const
Return the x value.
virtual void processTriangle(btVector3 *triangle, int partId, int triangleIndex)
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
const btCollisionShape * getCollisionShape() const
PHY_ScalarType
PHY_ScalarType enumerates possible scalar types.
btScalar btFabs(btScalar x)