Ejemplo n.º 1
0
        /// Changes a btManifoldPoint collision normal to the normal from the mesh.
        public static void AdjustInternalEdgeContacts(ManifoldPoint cp, CollisionObject colObj0, CollisionObject colObj1, int partId0, int index0, InternalEdgeAdjustFlags normalAdjustFlags)
        {
            //btAssert(colObj0.GetCollisionShape().GetShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
            if (colObj0.GetCollisionShape().GetShapeType() != BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
            {
                return;
            }

            BvhTriangleMeshShape trimesh = null;

            if (colObj0.GetRootCollisionShape().GetShapeType() == BroadphaseNativeTypes.SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
            {
                //trimesh = ((ScaledBvhTriangleMeshShape)colObj0.GetRootCollisionShape()).GetChildShape();
            }
            else
            {
                trimesh = (BvhTriangleMeshShape)colObj0.GetRootCollisionShape();
            }

            TriangleInfoMap triangleInfoMapPtr = (TriangleInfoMap)trimesh.GetTriangleInfoMap();

            if (triangleInfoMapPtr == null)
            {
                return;
            }

            int hash = GetHash(partId0, index0);

            TriangleInfo info;

            if (!triangleInfoMapPtr.TryGetValue(hash, out info))
            {
                return;
            }


            float frontFacing = (normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.0f : -1.0f;

            TriangleShape  tri_shape = colObj0.GetCollisionShape() as TriangleShape;
            IndexedVector3 v0, v1, v2;

            tri_shape.GetVertex(0, out v0);
            tri_shape.GetVertex(1, out v1);
            tri_shape.GetVertex(2, out v2);

            IndexedVector3 center = (v0 + v1 + v2) * (1.0f / 3.0f);

            IndexedVector3 red = new IndexedVector3(1, 0, 0), green = new IndexedVector3(0, 1, 0), blue = new IndexedVector3(0, 0, 1), white = new IndexedVector3(1, 1, 1), black = new IndexedVector3(0, 0, 0);
            IndexedVector3 tri_normal;

            tri_shape.CalcNormal(out tri_normal);

            //float dot = tri_normal.dot(cp.m_normalWorldOnB);
            IndexedVector3 nearest;

            NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);

            IndexedVector3 contact = cp.m_localPointB;

#if BT_INTERNAL_EDGE_DEBUG_DRAW
            IndexedMatrix tr = colObj0.GetWorldTransform();
            DebugDrawLine(tr * nearest, tr * cp.m_localPointB, red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW



            bool isNearEdge = false;

            int numConcaveEdgeHits = 0;
            int numConvexEdgeHits  = 0;

            IndexedVector3 localContactNormalOnB = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
            localContactNormalOnB.Normalize();//is this necessary?

            // Get closest edge
            int   bestedge       = -1;
            float disttobestedge = MathUtil.BT_LARGE_FLOAT;
            //
            // Edge 0 . 1
            if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
                //IndexedVector3 nearest;
                NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);
                float len = (contact - nearest).Length();
                //
                if (len < disttobestedge)
                {
                    bestedge       = 0;
                    disttobestedge = len;
                }
            }
            // Edge 1 . 2
            if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
                //IndexedVector3 nearest;
                NearestPointInLineSegment(ref cp.m_localPointB, ref v1, ref v2, out nearest);
                float len = (contact - nearest).Length();
                //
                if (len < disttobestedge)
                {
                    bestedge       = 1;
                    disttobestedge = len;
                }
            }
            // Edge 2 . 0
            if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
                //IndexedVector3 nearest;
                NearestPointInLineSegment(ref cp.m_localPointB, ref v2, ref v0, out nearest);
                float len = (contact - nearest).Length();
                //
                if (len < disttobestedge)
                {
                    bestedge       = 2;
                    disttobestedge = len;
                }
            }

#if BT_INTERNAL_EDGE_DEBUG_DRAW
            IndexedVector3 upfix = tri_normal * new IndexedVector3(0.1f, 0.1f, 0.1f);
            DebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
#endif
            if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif
                float len = (contact - nearest).Length();
                if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
                {
                    if (bestedge == 0)
                    {
                        IndexedVector3 edge = (v0 - v1);
                        isNearEdge = true;

                        if (info.m_edgeV0V1Angle == 0.0f)
                        {
                            numConcaveEdgeHits++;
                        }
                        else
                        {
                            bool  isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_CONVEX) != 0;
                            float swapFactor   = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                            DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                            IndexedVector3 nA = swapFactor * tri_normal;

                            IndexedQuaternion orn             = new IndexedQuaternion(edge, info.m_edgeV0V1Angle);
                            IndexedVector3    computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
                            if ((info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB) != 0)
                            {
                                computedNormalB *= -1;
                            }
                            IndexedVector3 nB = swapFactor * computedNormalB;

                            float NdotA            = localContactNormalOnB.Dot(ref nA);
                            float NdotB            = localContactNormalOnB.Dot(ref nB);
                            bool  backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);

#if DEBUG_INTERNAL_EDGE
                            {
                                DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
                            }
#endif //DEBUG_INTERNAL_EDGE


                            if (backFacingNormal)
                            {
                                numConcaveEdgeHits++;
                            }
                            else
                            {
                                numConvexEdgeHits++;
                                IndexedVector3 clampedLocalNormal;
                                bool           isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info.m_edgeV0V1Angle, out clampedLocalNormal);
                                if (isClamped)
                                {
                                    if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
                                    {
                                        IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *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);
                                    }
                                }
                            }
                        }
                    }
                }
            }

            NearestPointInLineSegment(ref contact, ref v1, ref v2, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
            DebugDrawLine(tr * nearest, tr * cp.m_localPointB, green);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

#if BT_INTERNAL_EDGE_DEBUG_DRAW
            DebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
#endif

            if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW



                float len = (contact - nearest).Length();
                if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
                {
                    if (bestedge == 1)
                    {
                        isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                        DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                        IndexedVector3 edge = (v1 - v2);

                        isNearEdge = true;

                        if (info.m_edgeV1V2Angle == 0f)
                        {
                            numConcaveEdgeHits++;
                        }
                        else
                        {
                            bool  isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_CONVEX) != 0;
                            float swapFactor   = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                            DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                            IndexedVector3 nA = swapFactor * tri_normal;

                            IndexedQuaternion orn             = new IndexedQuaternion(edge, info.m_edgeV1V2Angle);
                            IndexedVector3    computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
                            if ((info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB) != 0)
                            {
                                computedNormalB *= -1;
                            }
                            IndexedVector3 nB = swapFactor * computedNormalB;

#if DEBUG_INTERNAL_EDGE
                            {
                                DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
                            }
#endif //DEBUG_INTERNAL_EDGE


                            float NdotA            = localContactNormalOnB.Dot(ref nA);
                            float NdotB            = localContactNormalOnB.Dot(ref nB);
                            bool  backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);

                            if (backFacingNormal)
                            {
                                numConcaveEdgeHits++;
                            }
                            else
                            {
                                numConvexEdgeHits++;
                                IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
                                IndexedVector3 clampedLocalNormal;
                                bool           isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV1V2Angle, out clampedLocalNormal);
                                if (isClamped)
                                {
                                    if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
                                    {
                                        IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *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);
                                    }
                                }
                            }
                        }
                    }
                }
            }

            NearestPointInLineSegment(ref contact, ref v2, ref v0, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
            DebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#if BT_INTERNAL_EDGE_DEBUG_DRAW
            DebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
#endif

            if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                float len = (contact - nearest).Length();
                if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
                {
                    if (bestedge == 2)
                    {
                        isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                        DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                        IndexedVector3 edge = (v2 - v0);

                        if (info.m_edgeV2V0Angle == 0f)
                        {
                            numConcaveEdgeHits++;
                        }
                        else
                        {
                            bool  isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_CONVEX) != 0;
                            float swapFactor   = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                            DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                            IndexedVector3    nA              = swapFactor * tri_normal;
                            IndexedQuaternion orn             = new IndexedQuaternion(edge, info.m_edgeV2V0Angle);
                            IndexedVector3    computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
                            if ((info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB) != 0)
                            {
                                computedNormalB *= -1;
                            }
                            IndexedVector3 nB = swapFactor * computedNormalB;

#if DEBUG_INTERNAL_EDGE
                            {
                                DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
                            }
#endif //DEBUG_INTERNAL_EDGE

                            float NdotA            = localContactNormalOnB.Dot(ref nA);
                            float NdotB            = localContactNormalOnB.Dot(ref nB);
                            bool  backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);

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


                                IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
                                IndexedVector3 clampedLocalNormal;
                                bool           isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV2V0Angle, out clampedLocalNormal);
                                if (isClamped)
                                {
                                    if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
                                    {
                                        IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *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);
                                    }
                                }
                            }
                        }
                    }
                }
            }

#if DEBUG_INTERNAL_EDGE
            {
                IndexedVector3 color = new IndexedVector3(0, 1, 1);
                DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + cp.m_normalWorldOnB * 10, color);
            }
#endif //DEBUG_INTERNAL_EDGE

            if (isNearEdge)
            {
                if (numConcaveEdgeHits > 0)
                {
                    if ((normalAdjustFlags & InternalEdgeAdjustFlags.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(ref localContactNormalOnB) < 0)
                        {
                            tri_normal *= -1;
                        }
                        cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis *tri_normal;
                    }
                    else
                    {
                        IndexedVector3 newNormal = tri_normal * frontFacing;
                        //if the tri_normal is pointing opposite direction as the current local contact normal, skip it
                        float d = newNormal.Dot(ref localContactNormalOnB);
                        if (d < 0)
                        {
                            return;
                        }
                        //modify the normal to be the triangle normal (or backfacing normal)
                        cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis *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);
                }
            }
        }
        /// Changes a btManifoldPoint collision normal to the normal from the mesh.
        public static void AdjustInternalEdgeContacts(ManifoldPoint cp, CollisionObject colObj0, CollisionObject colObj1, int partId0, int index0, InternalEdgeAdjustFlags normalAdjustFlags)
        {
            //btAssert(colObj0.GetCollisionShape().GetShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
            if (colObj0.GetCollisionShape().GetShapeType() != BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
                return;

            BvhTriangleMeshShape trimesh = null;

            if (colObj0.GetRootCollisionShape().GetShapeType() == BroadphaseNativeTypes.SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
            {
                //trimesh = ((ScaledBvhTriangleMeshShape)colObj0.GetRootCollisionShape()).GetChildShape();
            }
            else
            {
                trimesh = (BvhTriangleMeshShape)colObj0.GetRootCollisionShape();
            }

            TriangleInfoMap triangleInfoMapPtr = (TriangleInfoMap)trimesh.GetTriangleInfoMap();
            if (triangleInfoMapPtr == null)
            {
                return;
            }

            int hash = GetHash(partId0, index0);

            TriangleInfo info;
            if (!triangleInfoMapPtr.TryGetValue(hash, out info))
            {
                return;
            }


            float frontFacing = (normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.0f : -1.0f;

            TriangleShape tri_shape = colObj0.GetCollisionShape() as TriangleShape;
            IndexedVector3 v0, v1, v2;
            tri_shape.GetVertex(0, out v0);
            tri_shape.GetVertex(1, out v1);
            tri_shape.GetVertex(2, out v2);

            IndexedVector3 center = (v0 + v1 + v2) * (1.0f / 3.0f);

            IndexedVector3 red = new IndexedVector3(1, 0, 0), green = new IndexedVector3(0, 1, 0), blue = new IndexedVector3(0, 0, 1), white = new IndexedVector3(1, 1, 1), black = new IndexedVector3(0, 0, 0);
            IndexedVector3 tri_normal;
            tri_shape.CalcNormal(out tri_normal);

            //float dot = tri_normal.dot(cp.m_normalWorldOnB);
            IndexedVector3 nearest;
            NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);

            IndexedVector3 contact = cp.m_localPointB;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
            IndexedMatrix tr = colObj0.GetWorldTransform();
            DebugDrawLine(tr * nearest, tr * cp.m_localPointB, red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW



            bool isNearEdge = false;

            int numConcaveEdgeHits = 0;
            int numConvexEdgeHits = 0;

            IndexedVector3 localContactNormalOnB = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
            localContactNormalOnB.Normalize();//is this necessary?

            // Get closest edge
            int bestedge = -1;
            float disttobestedge = MathUtil.BT_LARGE_FLOAT;
            //
            // Edge 0 . 1
            if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
                //IndexedVector3 nearest;
                NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);
                float len = (contact - nearest).Length();
                //
                if (len < disttobestedge)
                {
                    bestedge = 0;
                    disttobestedge = len;
                }
            }
            // Edge 1 . 2
            if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
                //IndexedVector3 nearest;
                NearestPointInLineSegment(ref  cp.m_localPointB, ref v1, ref v2, out nearest);
                float len = (contact - nearest).Length();
                //
                if (len < disttobestedge)
                {
                    bestedge = 1;
                    disttobestedge = len;
                }
            }
            // Edge 2 . 0
            if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
                //IndexedVector3 nearest;
                NearestPointInLineSegment(ref  cp.m_localPointB, ref v2, ref v0, out nearest);
                float len = (contact - nearest).Length();
                //
                if (len < disttobestedge)
                {
                    bestedge = 2;
                    disttobestedge = len;
                }
            }

#if BT_INTERNAL_EDGE_DEBUG_DRAW
            IndexedVector3 upfix = tri_normal * new IndexedVector3(0.1f, 0.1f, 0.1f);
            DebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
#endif
            if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif
                float len = (contact - nearest).Length();
                if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
                    if (bestedge == 0)
                    {
                        IndexedVector3 edge = (v0 - v1);
                        isNearEdge = true;

                        if (info.m_edgeV0V1Angle == 0.0f)
                        {
                            numConcaveEdgeHits++;
                        }
                        else
                        {

                            bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_CONVEX) != 0;
                            float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                            DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                            IndexedVector3 nA = swapFactor * tri_normal;

                            IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV0V1Angle);
                            IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
                            if ((info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB) != 0)
                            {
                                computedNormalB *= -1;
                            }
                            IndexedVector3 nB = swapFactor * computedNormalB;

                            float NdotA = localContactNormalOnB.Dot(ref nA);
                            float NdotB = localContactNormalOnB.Dot(ref nB);
                            bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);

#if DEBUG_INTERNAL_EDGE
                            {

                                DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
                            }
#endif //DEBUG_INTERNAL_EDGE


                            if (backFacingNormal)
                            {
                                numConcaveEdgeHits++;
                            }
                            else
                            {
                                numConvexEdgeHits++;
                                IndexedVector3 clampedLocalNormal;
                                bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info.m_edgeV0V1Angle, out clampedLocalNormal);
                                if (isClamped)
                                {
                                    if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
                                    {
                                        IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis * 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);

                                    }
                                }
                            }
                        }
                    }
            }

            NearestPointInLineSegment(ref contact, ref v1, ref v2, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
            DebugDrawLine(tr * nearest, tr * cp.m_localPointB, green);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

#if BT_INTERNAL_EDGE_DEBUG_DRAW
            DebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
#endif

            if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW



                float len = (contact - nearest).Length();
                if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
                    if (bestedge == 1)
                    {
                        isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                        DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                        IndexedVector3 edge = (v1 - v2);

                        isNearEdge = true;

                        if (info.m_edgeV1V2Angle == 0f)
                        {
                            numConcaveEdgeHits++;
                        }
                        else
                        {
                            bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_CONVEX) != 0;
                            float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                            DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                            IndexedVector3 nA = swapFactor * tri_normal;

                            IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV1V2Angle);
                            IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
                            if ((info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB) != 0)
                            {
                                computedNormalB *= -1;
                            }
                            IndexedVector3 nB = swapFactor * computedNormalB;

#if DEBUG_INTERNAL_EDGE
                            {
                                DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
                            }
#endif //DEBUG_INTERNAL_EDGE


                            float NdotA = localContactNormalOnB.Dot(ref nA);
                            float NdotB = localContactNormalOnB.Dot(ref nB);
                            bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);

                            if (backFacingNormal)
                            {
                                numConcaveEdgeHits++;
                            }
                            else
                            {
                                numConvexEdgeHits++;
                                IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
                                IndexedVector3 clampedLocalNormal;
                                bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV1V2Angle, out clampedLocalNormal);
                                if (isClamped)
                                {
                                    if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
                                    {
                                        IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis * 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);
                                    }
                                }
                            }
                        }
                    }
            }

            NearestPointInLineSegment(ref contact, ref v2, ref v0, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
            DebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#if BT_INTERNAL_EDGE_DEBUG_DRAW
            DebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
#endif

            if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {

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

                float len = (contact - nearest).Length();
                if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
                    if (bestedge == 2)
                    {
                        isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                        DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                        IndexedVector3 edge = (v2 - v0);

                        if (info.m_edgeV2V0Angle == 0f)
                        {
                            numConcaveEdgeHits++;
                        }
                        else
                        {

                            bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_CONVEX) != 0;
                            float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                            DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                            IndexedVector3 nA = swapFactor * tri_normal;
                            IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV2V0Angle);
                            IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
                            if ((info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB) != 0)
                            {
                                computedNormalB *= -1;
                            }
                            IndexedVector3 nB = swapFactor * computedNormalB;

#if DEBUG_INTERNAL_EDGE
                            {
                                DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
                            }
#endif //DEBUG_INTERNAL_EDGE

                            float NdotA = localContactNormalOnB.Dot(ref nA);
                            float NdotB = localContactNormalOnB.Dot(ref nB);
                            bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);

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


                                IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
                                IndexedVector3 clampedLocalNormal;
                                bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV2V0Angle, out clampedLocalNormal);
                                if (isClamped)
                                {
                                    if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
                                    {
                                        IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis * 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);
                                    }
                                }
                            }
                        }


                    }
            }

#if DEBUG_INTERNAL_EDGE
            {
                IndexedVector3 color = new IndexedVector3(0, 1, 1);
                DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + cp.m_normalWorldOnB * 10, color);
            }
#endif //DEBUG_INTERNAL_EDGE

            if (isNearEdge)
            {

                if (numConcaveEdgeHits > 0)
                {
                    if ((normalAdjustFlags & InternalEdgeAdjustFlags.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(ref localContactNormalOnB) < 0)
                        {
                            tri_normal *= -1;
                        }
                        cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis * tri_normal;
                    }
                    else
                    {
                        IndexedVector3 newNormal = tri_normal * frontFacing;
                        //if the tri_normal is pointing opposite direction as the current local contact normal, skip it
                        float d = newNormal.Dot(ref localContactNormalOnB);
                        if (d < 0)
                        {
                            return;
                        }
                        //modify the normal to be the triangle normal (or backfacing normal)
                        cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis * 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);
                }
            }
        }