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;
234 btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1];
243 #ifdef DEBUG_INTERNAL_EDGE 244 if ((computedNormalB-normalB).
length()>0.0001)
246 printf(
"warning: normals not identical\n");
248 #endif//DEBUG_INTERNAL_EDGE 258 btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0];
261 if (computedNormalB.
dot(normalB)<0)
267 #ifdef DEBUG_INTERNAL_EDGE 268 if ((computedNormalB-normalB).length()>0.0001)
270 printf(
"warning: normals not identical\n");
272 #endif //DEBUG_INTERNAL_EDGE 280 btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2];
283 if (computedNormalB.
dot(normalB)<0)
288 #ifdef DEBUG_INTERNAL_EDGE 289 if ((computedNormalB-normalB).length()>0.0001)
291 printf(
"warning: normals not identical\n");
293 #endif //DEBUG_INTERNAL_EDGE 326 for (
int partId = 0; partId< meshInterface->
getNumSubParts();partId++)
328 const unsigned char *vertexbase = 0;
332 const unsigned char *indexbase = 0;
342 for (
int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)
344 unsigned int* gfxbase = (
unsigned int*)(indexbase+triangleIndex*indexstride);
346 for (
int j=2;j>=0;j--)
349 int graphicsindex = indicestype==
PHY_SHORT?((
unsigned short*)gfxbase)[j]:gfxbase[j];
352 float* graphicsbase = (
float*)(vertexbase+graphicsindex*stride);
354 graphicsbase[0]*meshScaling.
getX(),
355 graphicsbase[1]*meshScaling.
getY(),
356 graphicsbase[2]*meshScaling.
getZ());
360 double* graphicsbase = (
double*)(vertexbase+graphicsindex*stride);
366 aabbMin.
setMin(triangleVerts[0]);
367 aabbMax.
setMax(triangleVerts[0]);
368 aabbMin.
setMin(triangleVerts[1]);
369 aabbMax.
setMax(triangleVerts[1]);
370 aabbMin.
setMin(triangleVerts[2]);
371 aabbMax.
setMax(triangleVerts[2]);
374 connectivityProcessor.
m_partIdA = partId;
398 nearestPoint = line0;
402 btScalar delta = (point-line0).
dot(lineDelta) / (lineDelta).
dot(lineDelta);
407 else if ( delta > 1 )
410 nearestPoint = line0 + lineDelta*delta;
426 if (correctedEdgeAngle<0)
428 if (curAngle < correctedEdgeAngle)
430 btScalar diffAngle = correctedEdgeAngle-curAngle;
432 clampedLocalNormal =
btMatrix3x3(rotation)*localContactNormalOnB;
437 if (correctedEdgeAngle>=0)
439 if (curAngle > correctedEdgeAngle)
441 btScalar diffAngle = correctedEdgeAngle-curAngle;
443 clampedLocalNormal =
btMatrix3x3(rotation)*localContactNormalOnB;
467 if (!triangleInfoMapPtr)
487 btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
496 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 497 const btTransform& tr = colObj0->getWorldTransform();
499 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 503 bool isNearEdge =
false;
505 int numConcaveEdgeHits = 0;
506 int numConvexEdgeHits = 0;
522 if( len < disttobestedge )
535 if( len < disttobestedge )
548 if( len < disttobestedge )
555 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 557 btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red );
561 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 565 if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
568 btVector3 edge(v0-v1);
573 numConcaveEdgeHits++;
579 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 580 btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
581 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 583 btVector3 nA = swapFactor * tri_normal;
586 btVector3 computedNormalB =
quatRotate(orn,tri_normal);
589 btVector3 nB = swapFactor*computedNormalB;
591 btScalar NdotA = localContactNormalOnB.
dot(nA);
592 btScalar NdotB = localContactNormalOnB.
dot(nB);
593 bool backFacingNormal = (NdotA< triangleInfoMapPtr->
m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
595 #ifdef DEBUG_INTERNAL_EDGE 600 #endif //DEBUG_INTERNAL_EDGE 603 if (backFacingNormal)
605 numConcaveEdgeHits++;
610 btVector3 clampedLocalNormal;
631 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 633 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 635 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 636 btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green );
641 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 643 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 648 if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
652 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 653 btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
654 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 656 btVector3 edge(v1-v2);
662 numConcaveEdgeHits++;
667 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 668 btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
669 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 671 btVector3 nA = swapFactor * tri_normal;
674 btVector3 computedNormalB =
quatRotate(orn,tri_normal);
677 btVector3 nB = swapFactor*computedNormalB;
679 #ifdef DEBUG_INTERNAL_EDGE 683 #endif //DEBUG_INTERNAL_EDGE 686 btScalar NdotA = localContactNormalOnB.
dot(nA);
687 btScalar NdotB = localContactNormalOnB.
dot(nB);
688 bool backFacingNormal = (NdotA< triangleInfoMapPtr->
m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
690 if (backFacingNormal)
692 numConcaveEdgeHits++;
698 btVector3 clampedLocalNormal;
718 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 720 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 721 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 722 btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue );
728 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 730 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 733 if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
737 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 738 btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
739 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 741 btVector3 edge(v2-v0);
745 numConcaveEdgeHits++;
751 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 752 btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
753 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW 755 btVector3 nA = swapFactor * tri_normal;
757 btVector3 computedNormalB =
quatRotate(orn,tri_normal);
760 btVector3 nB = swapFactor*computedNormalB;
762 #ifdef DEBUG_INTERNAL_EDGE 766 #endif //DEBUG_INTERNAL_EDGE 768 btScalar NdotA = localContactNormalOnB.
dot(nA);
769 btScalar NdotB = localContactNormalOnB.
dot(nB);
770 bool backFacingNormal = (NdotA< triangleInfoMapPtr->
m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
772 if (backFacingNormal)
774 numConcaveEdgeHits++;
783 btVector3 clampedLocalNormal;
804 #ifdef DEBUG_INTERNAL_EDGE 806 btVector3 color(0,1,1);
809 #endif //DEBUG_INTERNAL_EDGE 814 if (numConcaveEdgeHits>0)
819 if (tri_normal.
dot(localContactNormalOnB) < 0)
826 btVector3 newNormal = tri_normal *frontFacing;
828 btScalar d = newNormal.
dot(localContactNormalOnB) ;
void btNearestPointInLineSegment(const btVector3 &point, const btVector3 &line0, const btVector3 &line1, btVector3 &nearestPoint)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
const btTriangleInfoMap * getTriangleInfoMap() const
#define TRI_INFO_V0V1_SWAP_NORMALB
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
#define TRI_INFO_V1V2_CONVEX
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
virtual void processAllTriangles(btTriangleCallback *callback, const btVector3 &aabbMin, const btVector3 &aabbMax) const
btScalar m_maxEdgeAngleThreshold
used to determine edge contacts: if the closest distance between a contact point and an edge is small...
const btVector3 & getScaling() const
static int btGetHash(int partId, int triangleIndex)
ManifoldContactPoint collects and maintains persistent contactpoints.
btScalar m_equalVertexThreshold
used to determine if a triangle edge is planar with zero angle
virtual int getNumSubParts() const =0
getNumSubParts returns the number of seperate subparts each subpart has a continuous array of vertice...
#define TRI_INFO_V1V2_SWAP_NORMALB
btScalar dot(const btVector3 &v) const
Return the dot product.
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
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
void calcNormal(btVector3 &normal) const
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
const btScalar & getZ() const
Return the z value.
virtual void getVertex(int index, btVector3 &vert) const
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
btTriangleInfoMap * m_triangleInfoMap
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
btScalar btAtan2(btScalar x, btScalar y)
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape *trimeshShape, btTriangleInfoMap *triangleInfoMap)
Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo'.
const btScalar & getY() const
Return the y value.
const btTransform & getWorldTransform() const
The btTriangleInfo structure stores information to adjust collision normals to avoid collisions again...
const btScalar & getX() const
Return the x value.
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 ...
const btCollisionShape * getCollisionShape() const
const btVector3 & getPositionWorldOnB() const
btVector3 can be used to represent 3D points and vectors.
const Value * find(const Key &key) const
#define TRI_INFO_V0V1_CONVEX
for btTriangleInfo m_flags
btScalar length2() const
Return the length of the vector squared.
#define TRI_INFO_V2V0_SWAP_NORMALB
The btStridingMeshInterface is the interface class for high performance generic access to triangle me...
btMatrix3x3 transpose() const
Return the transpose of the matrix.
static btScalar btGetAngle(const btVector3 &edgeA, const btVector3 &normalA, const btVector3 &normalB)
#define TRI_INFO_V2V0_CONVEX
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...
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
const btCollisionShape * getCollisionShape() const
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...
PHY_ScalarType
PHY_ScalarType enumerates possible scalar types.
const btCollisionObject * getCollisionObject() const
btScalar btFabs(btScalar x)