btInternalEdgeUtility.cpp

Go to the documentation of this file.

#include "btInternalEdgeUtility.h"

#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
#include "LinearMath/btIDebugDraw.h"


//#define DEBUG_INTERNAL_EDGE


#ifdef DEBUG_INTERNAL_EDGE
#include <stdio.h>
#endif //DEBUG_INTERNAL_EDGE


#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
static btIDebugDraw* gDebugDrawer = 0;

void    btSetDebugDrawer(btIDebugDraw* debugDrawer)
{
    gDebugDrawer = debugDrawer;
}

static void    btDebugDrawLine(const btVector3& from,const btVector3& to, const btVector3& color)
{
    if (gDebugDrawer)
        gDebugDrawer->drawLine(from,to,color);
}
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW


static int  btGetHash(int partId, int triangleIndex)
{
    int hash = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
    return hash;
}



static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA,const btVector3& normalB)
{
    const btVector3 refAxis0  = edgeA;
    const btVector3 refAxis1  = normalA;
    const btVector3 swingAxis = normalB;
    btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
    return  angle;
}


struct btConnectivityProcessor : public btTriangleCallback
{
    int             m_partIdA;
    int             m_triangleIndexA;
    btVector3*      m_triangleVerticesA;
    btTriangleInfoMap*  m_triangleInfoMap;


    virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
    {
        //skip self-collisions
        if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
            return;

        //skip duplicates (disabled for now)
        //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
        //  return;

        //search for shared vertices and edges
        int numshared = 0;
        int sharedVertsA[3]={-1,-1,-1};
        int sharedVertsB[3]={-1,-1,-1};

        btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2();
        if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)
            return;


        btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2();
        if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold)
            return;

#if 0
        printf("triangle A[0]   =   (%f,%f,%f)\ntriangle A[1]   =   (%f,%f,%f)\ntriangle A[2]   =   (%f,%f,%f)\n",
            m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(),
            m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(),
            m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ());

        printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex);
        printf("triangle B[0]   =   (%f,%f,%f)\ntriangle B[1]   =   (%f,%f,%f)\ntriangle B[2]   =   (%f,%f,%f)\n",
            triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(),
            triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(),
            triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());
#endif

        for (int i=0;i<3;i++)
        {
            for (int j=0;j<3;j++)
            {
                if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold)
                {
                    sharedVertsA[numshared] = i;
                    sharedVertsB[numshared] = j;
                    numshared++;
                    if(numshared >= 3)
                        return;
                }
            }
            if(numshared >= 3)
                return;
        }
        switch (numshared)
        {
        case 0:
            {
                break;
            }
        case 1:
            {
                //shared vertex
                break;
            }
        case 2:
            {
                //shared edge
                //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
                if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
                {
                    sharedVertsA[0] = 2;
                    sharedVertsA[1] = 0;
                    int tmp = sharedVertsB[1];
                    sharedVertsB[1] = sharedVertsB[0];
                    sharedVertsB[0] = tmp;
                }

                int hash = btGetHash(m_partIdA,m_triangleIndexA);

                btTriangleInfo* info = m_triangleInfoMap->find(hash);
                if (!info)
                {
                    btTriangleInfo tmp;
                    m_triangleInfoMap->insert(hash,tmp);
                    info = m_triangleInfoMap->find(hash);
                }

                int sumvertsA = sharedVertsA[0]+sharedVertsA[1];
                int otherIndexA = 3-sumvertsA;

                
                btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]);

                btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]);
                int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]);

                btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]);
                //btTriangleShape tB(triangle[0],triangle[1],triangle[2]);

                btVector3 normalA;
                btVector3 normalB;
                tA.calcNormal(normalA);
                tB.calcNormal(normalB);
                edge.normalize();
                btVector3 edgeCrossA = edge.cross(normalA).normalize();

                {
                    btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]];
                    if (edgeCrossA.dot(tmp) < 0)
                    {
                        edgeCrossA*=-1;
                    }
                }

                btVector3 edgeCrossB = edge.cross(normalB).normalize();

                {
                    btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]];
                    if (edgeCrossB.dot(tmp) < 0)
                    {
                        edgeCrossB*=-1;
                    }
                }

                btScalar    angle2 = 0;
                btScalar    ang4 = 0.f;


                btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB);
                btScalar len2 = calculatedEdge.length2();

                btScalar correctedAngle(0);
                btVector3 calculatedNormalB = normalA;
                bool isConvex = false;

                if (len2<m_triangleInfoMap->m_planarEpsilon)
                {
                    angle2 = 0.f;
                    ang4 = 0.f;
                } else
                {

                    calculatedEdge.normalize();
                    btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA);
                    calculatedNormalA.normalize();
                    angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB);
                    ang4 = SIMD_PI-angle2;
                    btScalar dotA = normalA.dot(edgeCrossB);
                    isConvex = (dotA<0.);

                    correctedAngle = isConvex ? ang4 : -ang4;
                    btQuaternion orn2(calculatedEdge,-correctedAngle);
                    calculatedNormalB = btMatrix3x3(orn2)*normalA;


                }

                

                
                            
                //alternatively use 
                //btVector3 calculatedNormalB2 = quatRotate(orn,normalA);


                switch (sumvertsA)
                {
                case 1:
                    {
                        btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1];
                        btQuaternion orn(edge,-correctedAngle);
                        btVector3 computedNormalB = quatRotate(orn,normalA);
                        btScalar bla = computedNormalB.dot(normalB);
                        if (bla<0)
                        {
                            computedNormalB*=-1;
                            info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;
                        }
#ifdef DEBUG_INTERNAL_EDGE
                        if ((computedNormalB-normalB).length()>0.0001)
                        {
                            printf("warning: normals not identical\n");
                        }
#endif//DEBUG_INTERNAL_EDGE

                        info->m_edgeV0V1Angle = -correctedAngle;

                        if (isConvex)
                            info->m_flags |= TRI_INFO_V0V1_CONVEX;
                        break;
                    }
                case 2:
                    {
                        btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0];
                        btQuaternion orn(edge,-correctedAngle);
                        btVector3 computedNormalB = quatRotate(orn,normalA);
                        if (computedNormalB.dot(normalB)<0)
                        {
                            computedNormalB*=-1;
                            info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB;
                        }

#ifdef DEBUG_INTERNAL_EDGE
                        if ((computedNormalB-normalB).length()>0.0001)
                        {
                            printf("warning: normals not identical\n");
                        }
#endif //DEBUG_INTERNAL_EDGE
                        info->m_edgeV2V0Angle = -correctedAngle;
                        if (isConvex)
                            info->m_flags |= TRI_INFO_V2V0_CONVEX;
                        break;  
                    }
                case 3:
                    {
                        btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2];
                        btQuaternion orn(edge,-correctedAngle);
                        btVector3 computedNormalB = quatRotate(orn,normalA);
                        if (computedNormalB.dot(normalB)<0)
                        {
                            info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB;
                            computedNormalB*=-1;
                        }
#ifdef DEBUG_INTERNAL_EDGE
                        if ((computedNormalB-normalB).length()>0.0001)
                        {
                            printf("warning: normals not identical\n");
                        }
#endif //DEBUG_INTERNAL_EDGE
                        info->m_edgeV1V2Angle = -correctedAngle;

                        if (isConvex)
                            info->m_flags |= TRI_INFO_V1V2_CONVEX;
                        break;
                    }
                }

                break;
            }
        default:
            {
                //              printf("warning: duplicate triangle\n");
            }

        }
    }
};

void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap)
{
    //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
    if (trimeshShape->getTriangleInfoMap())
        return;

    trimeshShape->setTriangleInfoMap(triangleInfoMap);

    btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface();
    const btVector3& meshScaling = meshInterface->getScaling();

    for (int partId = 0; partId< meshInterface->getNumSubParts();partId++)
    {
        const unsigned char *vertexbase = 0;
        int numverts = 0;
        PHY_ScalarType type = PHY_INTEGER;
        int stride = 0;
        const unsigned char *indexbase = 0;
        int indexstride = 0;
        int numfaces = 0;
        PHY_ScalarType indicestype = PHY_INTEGER;
        //PHY_ScalarType indexType=0;

        btVector3 triangleVerts[3];
        meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,   type,stride,&indexbase,indexstride,numfaces,indicestype,partId);
        btVector3 aabbMin,aabbMax;

        for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)
        {
            unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride);

            for (int j=2;j>=0;j--)
            {

                int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
                if (type == PHY_FLOAT)
                {
                    float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
                    triangleVerts[j] = btVector3(
                        graphicsbase[0]*meshScaling.getX(),
                        graphicsbase[1]*meshScaling.getY(),
                        graphicsbase[2]*meshScaling.getZ());
                }
                else
                {
                    double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
                    triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
                }
            }
            aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
            aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); 
            aabbMin.setMin(triangleVerts[0]);
            aabbMax.setMax(triangleVerts[0]);
            aabbMin.setMin(triangleVerts[1]);
            aabbMax.setMax(triangleVerts[1]);
            aabbMin.setMin(triangleVerts[2]);
            aabbMax.setMax(triangleVerts[2]);

            btConnectivityProcessor connectivityProcessor;
            connectivityProcessor.m_partIdA = partId;
            connectivityProcessor.m_triangleIndexA = triangleIndex;
            connectivityProcessor.m_triangleVerticesA = &triangleVerts[0];
            connectivityProcessor.m_triangleInfoMap  = triangleInfoMap;

            trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax);
        }

    }

}




// Given a point and a line segment (defined by two points), compute the closest point
// in the line.  Cap the point at the endpoints of the line segment.
void btNearestPointInLineSegment(const btVector3 &point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)
{
    btVector3 lineDelta     = line1 - line0;

    // Handle degenerate lines
    if ( lineDelta.fuzzyZero())
    {
        nearestPoint = line0;
    }
    else
    {
        btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta);

        // Clamp the point to conform to the segment's endpoints
        if ( delta < 0 )
            delta = 0;
        else if ( delta > 1 )
            delta = 1;

        nearestPoint = line0 + lineDelta*delta;
    }
}




bool    btClampNormal(const btVector3& edge,const btVector3& tri_normal_org,const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 & clampedLocalNormal)
{
    btVector3 tri_normal = tri_normal_org;
    //we only have a local triangle normal, not a local contact normal -> only normal in world space...
    //either compute the current angle all in local space, or all in world space

    btVector3 edgeCross = edge.cross(tri_normal).normalize();
    btScalar curAngle = btGetAngle(edgeCross,tri_normal,localContactNormalOnB);

    if (correctedEdgeAngle<0)
    {
        if (curAngle < correctedEdgeAngle)
        {
            btScalar diffAngle = correctedEdgeAngle-curAngle;
            btQuaternion rotation(edge,diffAngle );
            clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
            return true;
        }
    }

    if (correctedEdgeAngle>=0)
    {
        if (curAngle > correctedEdgeAngle)
        {
            btScalar diffAngle = correctedEdgeAngle-curAngle;
            btQuaternion rotation(edge,diffAngle );
            clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
            return true;
        }
    }
    return false;
}



void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* colObj0,const btCollisionObject* colObj1, int partId0, int index0, int normalAdjustFlags)
{
    //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
    if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
        return;

    btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape();
    btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
    if (!triangleInfoMapPtr)
        return;

    int hash = btGetHash(partId0,index0);


    btTriangleInfo* info = triangleInfoMapPtr->find(hash);
    if (!info)
        return;

    btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f;
    
    const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0->getCollisionShape());
    btVector3 v0,v1,v2;
    tri_shape->getVertex(0,v0);
    tri_shape->getVertex(1,v1);
    tri_shape->getVertex(2,v2);

    btVector3 center = (v0+v1+v2)*btScalar(1./3.);

    btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
    btVector3 tri_normal;
    tri_shape->calcNormal(tri_normal);

    //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
    btVector3 nearest;
    btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest);

    btVector3 contact = cp.m_localPointB;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
    const btTransform& tr = colObj0->getWorldTransform();
    btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW



    bool isNearEdge = false;

    int numConcaveEdgeHits = 0;
    int numConvexEdgeHits = 0;

    btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
    localContactNormalOnB.normalize();//is this necessary?

    if ((info->m_edgeV0V1Angle)< SIMD_2_PI)
    {
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
        btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif
        btScalar len = (contact-nearest).length();
        if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
        {
            btVector3 edge(v0-v1);
            isNearEdge = true;

            if (info->m_edgeV0V1Angle==btScalar(0))
            {
                numConcaveEdgeHits++;
            } else
            {

                bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
                btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
    #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
                btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
    #endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                btVector3 nA = swapFactor * tri_normal;

                btQuaternion orn(edge,info->m_edgeV0V1Angle);
                btVector3 computedNormalB = quatRotate(orn,tri_normal);
                if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
                    computedNormalB*=-1;
                btVector3 nB = swapFactor*computedNormalB;

                btScalar    NdotA = localContactNormalOnB.dot(nA);
                btScalar    NdotB = localContactNormalOnB.dot(nB);
                bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);

#ifdef DEBUG_INTERNAL_EDGE
                {
                    
                    btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
                }
#endif //DEBUG_INTERNAL_EDGE


                if (backFacingNormal)
                {
                    numConcaveEdgeHits++;
                }
                else
                {
                    numConvexEdgeHits++;
                    btVector3 clampedLocalNormal;
                    bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal);
                    if (isClamped)
                    {
                        if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
                        {
                            btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;
                            //                  cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
                            cp.m_normalWorldOnB = newNormal;
                            // Reproject collision point along normal. (what about cp.m_distance1?)
                            cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
                            cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
                            
                        }
                    }
                }
            }
        }
    }

    btNearestPointInLineSegment(contact,v1,v2,nearest);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
    btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

    if ((info->m_edgeV1V2Angle)< SIMD_2_PI)
    {
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
        btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW



        btScalar len = (contact-nearest).length();
        if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
        {
            isNearEdge = true;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
            btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

            btVector3 edge(v1-v2);

            isNearEdge = true;

            if (info->m_edgeV1V2Angle == btScalar(0))
            {
                numConcaveEdgeHits++;
            } else
            {
                bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0;
                btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
    #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
                btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
    #endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                btVector3 nA = swapFactor * tri_normal;
                
                btQuaternion orn(edge,info->m_edgeV1V2Angle);
                btVector3 computedNormalB = quatRotate(orn,tri_normal);
                if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)
                    computedNormalB*=-1;
                btVector3 nB = swapFactor*computedNormalB;

#ifdef DEBUG_INTERNAL_EDGE
                {
                    btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
                }
#endif //DEBUG_INTERNAL_EDGE


                btScalar    NdotA = localContactNormalOnB.dot(nA);
                btScalar    NdotB = localContactNormalOnB.dot(nB);
                bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);

                if (backFacingNormal)
                {
                    numConcaveEdgeHits++;
                }
                else
                {
                    numConvexEdgeHits++;
                    btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
                    btVector3 clampedLocalNormal;
                    bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal);
                    if (isClamped)
                    {
                        if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
                        {
                            btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;
                            //                  cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
                            cp.m_normalWorldOnB = newNormal;
                            // Reproject collision point along normal.
                            cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
                            cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
                        }
                    }
                }
            }
        }
    }

    btNearestPointInLineSegment(contact,v2,v0,nearest);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
    btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

    if ((info->m_edgeV2V0Angle)< SIMD_2_PI)
    {

#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
        btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

        btScalar len = (contact-nearest).length();
        if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
        {
            isNearEdge = true;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
            btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

            btVector3 edge(v2-v0);

            if (info->m_edgeV2V0Angle==btScalar(0))
            {
                numConcaveEdgeHits++;
            } else
            {

                bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0;
                btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
    #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
                btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
    #endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                btVector3 nA = swapFactor * tri_normal;
                btQuaternion orn(edge,info->m_edgeV2V0Angle);
                btVector3 computedNormalB = quatRotate(orn,tri_normal);
                if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)
                    computedNormalB*=-1;
                btVector3 nB = swapFactor*computedNormalB;

#ifdef DEBUG_INTERNAL_EDGE
                {
                    btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
                }
#endif //DEBUG_INTERNAL_EDGE

                btScalar    NdotA = localContactNormalOnB.dot(nA);
                btScalar    NdotB = localContactNormalOnB.dot(nB);
                bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);

                if (backFacingNormal)
                {
                    numConcaveEdgeHits++;
                }
                else
                {
                    numConvexEdgeHits++;
                    //              printf("hitting convex edge\n");


                    btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
                    btVector3 clampedLocalNormal;
                    bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal);
                    if (isClamped)
                    {
                        if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
                        {
                            btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;
                            //                  cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
                            cp.m_normalWorldOnB = newNormal;
                            // Reproject collision point along normal.
                            cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
                            cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
                        }
                    }
                } 
            }
            

        }
    }

#ifdef DEBUG_INTERNAL_EDGE
    {
        btVector3 color(0,1,1);
        btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color);
    }
#endif //DEBUG_INTERNAL_EDGE

    if (isNearEdge)
    {

        if (numConcaveEdgeHits>0)
        {
            if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0)
            {
                //fix tri_normal so it pointing the same direction as the current local contact normal
                if (tri_normal.dot(localContactNormalOnB) < 0)
                {
                    tri_normal *= -1;
                }
                cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis()*tri_normal;
            } else
            {
                //modify the normal to be the triangle normal (or backfacing normal)
                cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *(tri_normal *frontFacing);
            }
            
            
            // Reproject collision point along normal.
            cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
            cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
        }
    }
}