public bool closestPtPointTetrahedron( ref btVector3 p, ref btVector3 a, ref btVector3 b, ref btVector3 c, ref btVector3 d, btSubSimplexClosestResult finalResult ) { btSubSimplexClosestResult tempResult = new btSubSimplexClosestResult(); // Start out assuming point inside all halfspaces, so closest to itself finalResult.m_closestPointOnSimplex = p; finalResult.m_usedVertices = 0; finalResult.m_usedVertices |= btUsageBitfield.usedVertexA; finalResult.m_usedVertices |= btUsageBitfield.usedVertexB; finalResult.m_usedVertices |= btUsageBitfield.usedVertexC; finalResult.m_usedVertices |= btUsageBitfield.usedVertexD; int pointOutsideABC = pointOutsideOfPlane( ref p, ref a, ref b, ref c, ref d ); int pointOutsideACD = pointOutsideOfPlane( ref p, ref a, ref c, ref d, ref b ); int pointOutsideADB = pointOutsideOfPlane( ref p, ref a, ref d, ref b, ref c ); int pointOutsideBDC = pointOutsideOfPlane( ref p, ref b, ref d, ref c, ref a ); if( pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0 ) { finalResult.m_degenerate = true; return false; } if( pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0 ) { return false; } double bestSqDist = btScalar.BT_MAX_FLOAT; // If point outside face abc then compute closest point on abc btVector3 tmp; if( pointOutsideABC != 0 ) { closestPtPointTriangle( ref p, ref a, ref b, ref c, ref tempResult ); //btVector3 q = tempResult.m_closestPointOnSimplex; tempResult.m_closestPointOnSimplex.Sub( ref p, out tmp ); double sqDist = ( tmp ).dot( ref tmp ); // Update best closest point if (squared) distance is less than current best if( sqDist < bestSqDist ) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = tempResult.m_closestPointOnSimplex; //convert result bitmask! finalResult.m_usedVertices = 0; finalResult.m_usedVertices = tempResult.m_usedVertices & ( btUsageBitfield.usedVertexA | btUsageBitfield.usedVertexB | btUsageBitfield.usedVertexC ); finalResult.setBarycentricCoordinates( tempResult.m_barycentricCoord0, tempResult.m_barycentricCoord1, tempResult.m_barycentricCoord2, 0 ); } } // Repeat test for face acd if( pointOutsideACD != 0 ) { closestPtPointTriangle( ref p, ref a, ref c, ref d, ref tempResult ); //btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! tempResult.m_closestPointOnSimplex.Sub( ref p, out tmp ); double sqDist = ( tmp ).dot( ref tmp ); //double sqDist = ( q - p ).dot( q - p ); if( sqDist < bestSqDist ) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = tempResult.m_closestPointOnSimplex; finalResult.m_usedVertices = 0; finalResult.m_usedVertices = tempResult.m_usedVertices & ( btUsageBitfield.usedVertexA ); finalResult.m_usedVertices |= ((tempResult.m_usedVertices & btUsageBitfield.usedVertexB) != 0)? btUsageBitfield.usedVertexC : 0; finalResult.m_usedVertices |= ((tempResult.m_usedVertices & btUsageBitfield.usedVertexC) != 0)? btUsageBitfield.usedVertexD : 0; finalResult.setBarycentricCoordinates( tempResult.m_barycentricCoord0, 0, tempResult.m_barycentricCoord1, tempResult.m_barycentricCoord2 ); } } // Repeat test for face adb if( pointOutsideADB != 0 ) { closestPtPointTriangle( ref p, ref a, ref d, ref b, ref tempResult ); //btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! tempResult.m_closestPointOnSimplex.Sub( ref p, out tmp ); double sqDist = ( tmp ).dot( ref tmp ); //double sqDist = ( q - p ).dot( q - p ); if( sqDist < bestSqDist ) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = tempResult.m_closestPointOnSimplex; finalResult.m_usedVertices = 0; finalResult.m_usedVertices = tempResult.m_usedVertices & ( btUsageBitfield.usedVertexA ); finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexC ) != 0 ) ? btUsageBitfield.usedVertexB : 0; finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexB ) != 0 ) ? btUsageBitfield.usedVertexD : 0; finalResult.setBarycentricCoordinates( tempResult.m_barycentricCoord0, tempResult.m_barycentricCoord2, 0, tempResult.m_barycentricCoord3 ); } } // Repeat test for face bdc if( pointOutsideBDC != 0 ) { closestPtPointTriangle( ref p, ref b, ref d, ref c, ref tempResult ); //btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! tempResult.m_closestPointOnSimplex.Sub( ref p, out tmp ); double sqDist = ( tmp ).dot( ref tmp ); //double sqDist = ( q - p ).dot( q - p ); if( sqDist < bestSqDist ) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = tempResult.m_closestPointOnSimplex; finalResult.m_usedVertices = 0; // finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexA ) != 0 ) ? btUsageBitfield.usedVertexB : 0; finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexC ) != 0 ) ? btUsageBitfield.usedVertexC : 0; finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexB ) != 0 ) ? btUsageBitfield.usedVertexD : 0; finalResult.setBarycentricCoordinates( 0, tempResult.m_barycentricCoord0, tempResult.m_barycentricCoord2, tempResult.m_barycentricCoord2 ); } } //help! we ended up full ! if( ( finalResult.m_usedVertices & btUsageBitfield.usedAll ) == btUsageBitfield.usedAll ) { return true; } return true; }
public bool closestPtPointTriangle( ref btVector3 p, ref btVector3 a, ref btVector3 b, ref btVector3 c, ref btSubSimplexClosestResult result ) { result.m_usedVertices = 0;//.reset(); // Check if P in vertex region outside A btVector3 ab; b.Sub( ref a, out ab ); btVector3 ac; c.Sub( ref a, out ac ); btVector3 ap; p.Sub( ref a, out ap ); double d1 = ab.dot( ref ap ); double d2 = ac.dot( ref ap ); if( d1 <= (double)( 0.0 ) && d2 <= (double)( 0.0 ) ) { result.m_closestPointOnSimplex = a; result.m_usedVertices |= btUsageBitfield.usedVertexA; result.setBarycentricCoordinates( 1, 0, 0 ); return true;// a; // barycentric coordinates (1,0,0) } // Check if P in vertex region outside B btVector3 bp; p.Sub( ref b, out bp ); double d3 = ab.dot( ref bp ); double d4 = ac.dot( ref bp ); if( d3 >= (double)( 0.0 ) && d4 <= d3 ) { result.m_closestPointOnSimplex = b; result.m_usedVertices |= btUsageBitfield.usedVertexB; result.setBarycentricCoordinates( 0, 1, 0 ); return true; // b; // barycentric coordinates (0,1,0) } // Check if P in edge region of AB, if so return projection of P onto AB double vc = d1 * d4 - d3 * d2; btVector3 tmp; if( vc <= (double)( 0.0 ) && d1 >= (double)( 0.0 ) && d3 <= (double)( 0.0 ) ) { double v = d1 / ( d1 - d3 ); ab.Mult( v, out tmp ); a.Add( ref tmp, out result.m_closestPointOnSimplex ); result.m_usedVertices |= btUsageBitfield.usedVertexA; result.m_usedVertices |= btUsageBitfield.usedVertexB; result.setBarycentricCoordinates( 1 - v, v, 0 ); return true; //return a + v * ab; // barycentric coordinates (1-v,v,0) } // Check if P in vertex region outside C btVector3 cp; p.Sub( ref c, out cp ); double d5 = ab.dot( ref cp ); double d6 = ac.dot( ref cp ); if( d6 >= (double)( 0.0 ) && d5 <= d6 ) { result.m_closestPointOnSimplex = c; result.m_usedVertices |= btUsageBitfield.usedVertexC; result.setBarycentricCoordinates( 0, 0, 1 ); return true;//c; // barycentric coordinates (0,0,1) } // Check if P in edge region of AC, if so return projection of P onto AC double vb = d5 * d2 - d1 * d6; if( vb <= (double)( 0.0 ) && d2 >= (double)( 0.0 ) && d6 <= (double)( 0.0 ) ) { double w = d2 / ( d2 - d6 ); ac.Mult( w, out tmp ); a.Add( ref tmp, out result.m_closestPointOnSimplex ); result.m_usedVertices |= btUsageBitfield.usedVertexA; result.m_usedVertices |= btUsageBitfield.usedVertexC; result.setBarycentricCoordinates( 1 - w, 0, w ); return true; //return a + w * ac; // barycentric coordinates (1-w,0,w) } // Check if P in edge region of BC, if so return projection of P onto BC double va = d3 * d6 - d5 * d4; if( va <= (double)( 0.0 ) && ( d4 - d3 ) >= (double)( 0.0 ) && ( d5 - d6 ) >= (double)( 0.0 ) ) { double w = ( d4 - d3 ) / ( ( d4 - d3 ) + ( d5 - d6 ) ); btVector3 tmp2; c.Sub( ref b, out tmp2 ); tmp2.Mult( w, out tmp ); b.Add( ref tmp, out result.m_closestPointOnSimplex ); //result.m_closestPointOnSimplex = b + w * ( c - b ); result.m_usedVertices |= btUsageBitfield.usedVertexB; result.m_usedVertices |= btUsageBitfield.usedVertexC; result.setBarycentricCoordinates( 0, 1 - w, w ); return true; // return b + w * (c - b); // barycentric coordinates (0,1-w,w) } // P inside face region. Compute Q through its barycentric coordinates (u,v,w) { double denom = (double)( 1.0 ) / ( va + vb + vc ); double v = vb * denom; double w = vc * denom; ab.Mult( v, out ab ); ac.Mult( w, out ac ); a.Add( ref ab, out tmp ); tmp.Add( ref ac, out result.m_closestPointOnSimplex ); //result.m_closestPointOnSimplex = a + ab * v + ac * w; result.m_usedVertices |= btUsageBitfield.usedVertexA; result.m_usedVertices |= btUsageBitfield.usedVertexB; result.m_usedVertices |= btUsageBitfield.usedVertexC; result.setBarycentricCoordinates( 1 - v - w, v, w ); } return true; // return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = (double)(1.0) - v - w }