public bool ClosestPtPointTriangle(ref IndexedVector3 p, ref IndexedVector3 a, ref IndexedVector3 b, ref IndexedVector3 c, ref SubSimplexClosestResult result) { result.m_usedVertices.SetAll(false); // Check if P in vertex region outside A IndexedVector3 ab,ac,ap; IndexedVector3.Subtract(out ab,ref b,ref a); IndexedVector3.Subtract(out ac, ref c, ref a); IndexedVector3.Subtract(out ap, ref p, ref a); float d1 = IndexedVector3.Dot(ref ab, ref ap); float d2 = IndexedVector3.Dot(ref ac, ref ap); if (d1 <= 0f && d2 <= 0f) { result.m_closestPointOnSimplex = a; result.m_usedVertices.Set(0, true); result.SetBarycentricCoordinates(1, 0, 0, 0); return true;// a; // barycentric coordinates (1,0,0) } // Check if P in vertex region outside B IndexedVector3 bp; IndexedVector3.Subtract(out bp, ref p, ref b); float d3 = IndexedVector3.Dot(ref ab, ref bp); float d4 = IndexedVector3.Dot(ref ac, ref bp); if (d3 >= 0f && d4 <= d3) { result.m_closestPointOnSimplex = b; result.m_usedVertices.Set(1, true); result.SetBarycentricCoordinates(0, 1, 0, 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 float vc = d1 * d4 - d3 * d2; if (vc <= 0f && d1 >= 0f && d3 <= 0f) { float v = d1 / (d1 - d3); result.m_closestPointOnSimplex = a + v * ab; result.m_usedVertices.Set(0, true); result.m_usedVertices.Set(1, true); result.SetBarycentricCoordinates(1 - v, v, 0, 0); return true; //return a + v * ab; // barycentric coordinates (1-v,v,0) } // Check if P in vertex region outside C IndexedVector3 cp; IndexedVector3.Subtract(out cp, ref p, ref c); float d5 = IndexedVector3.Dot(ref ab, ref cp); float d6 = IndexedVector3.Dot(ref ac, ref cp); if (d6 >= 0f && d5 <= d6) { result.m_closestPointOnSimplex = c; result.m_usedVertices.Set(2, true); result.SetBarycentricCoordinates(0, 0, 1, 0); return true;//c; // barycentric coordinates (0,0,1) } // Check if P in edge region of AC, if so return projection of P onto AC float vb = d5 * d2 - d1 * d6; if (vb <= 0f && d2 >= 0f && d6 <= 0f) { float w = d2 / (d2 - d6); result.m_closestPointOnSimplex = a + w * ac; result.m_usedVertices.Set(0, true); result.m_usedVertices.Set(2, true); result.SetBarycentricCoordinates(1 - w, 0, w, 0); 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 float va = d3 * d6 - d5 * d4; if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f) { float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); result.m_closestPointOnSimplex = b + w * (c - b); result.m_usedVertices.Set(1, true); result.m_usedVertices.Set(2, true); result.SetBarycentricCoordinates(0, 1 - w, w, 0); 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) float denom = 1f / (va + vb + vc); float v1 = vb * denom; float w1 = vc * denom; result.m_closestPointOnSimplex = a + ab * v1 + ac * w1; result.m_usedVertices.Set(0, true); result.m_usedVertices.Set(1, true); result.m_usedVertices.Set(2, true); result.SetBarycentricCoordinates(1 - v1 - w1, v1, w1, 0); return true; // return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = float(1.0) - v - w }
public bool ClosestPtPointTriangle(ref IndexedVector3 p, ref IndexedVector3 a, ref IndexedVector3 b, ref IndexedVector3 c, ref SubSimplexClosestResult result) { result.m_usedVertices.SetAll(false); // Check if P in vertex region outside A IndexedVector3 ab, ac, ap; IndexedVector3.Subtract(out ab, ref b, ref a); IndexedVector3.Subtract(out ac, ref c, ref a); IndexedVector3.Subtract(out ap, ref p, ref a); float d1 = IndexedVector3.Dot(ref ab, ref ap); float d2 = IndexedVector3.Dot(ref ac, ref ap); if (d1 <= 0f && d2 <= 0f) { result.m_closestPointOnSimplex = a; result.m_usedVertices.Set(0, true); result.SetBarycentricCoordinates(1, 0, 0, 0); return(true);// a; // barycentric coordinates (1,0,0) } // Check if P in vertex region outside B IndexedVector3 bp; IndexedVector3.Subtract(out bp, ref p, ref b); float d3 = IndexedVector3.Dot(ref ab, ref bp); float d4 = IndexedVector3.Dot(ref ac, ref bp); if (d3 >= 0f && d4 <= d3) { result.m_closestPointOnSimplex = b; result.m_usedVertices.Set(1, true); result.SetBarycentricCoordinates(0, 1, 0, 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 float vc = d1 * d4 - d3 * d2; if (vc <= 0f && d1 >= 0f && d3 <= 0f) { float v = d1 / (d1 - d3); result.m_closestPointOnSimplex = a + v * ab; result.m_usedVertices.Set(0, true); result.m_usedVertices.Set(1, true); result.SetBarycentricCoordinates(1 - v, v, 0, 0); return(true); //return a + v * ab; // barycentric coordinates (1-v,v,0) } // Check if P in vertex region outside C IndexedVector3 cp; IndexedVector3.Subtract(out cp, ref p, ref c); float d5 = IndexedVector3.Dot(ref ab, ref cp); float d6 = IndexedVector3.Dot(ref ac, ref cp); if (d6 >= 0f && d5 <= d6) { result.m_closestPointOnSimplex = c; result.m_usedVertices.Set(2, true); result.SetBarycentricCoordinates(0, 0, 1, 0); return(true);//c; // barycentric coordinates (0,0,1) } // Check if P in edge region of AC, if so return projection of P onto AC float vb = d5 * d2 - d1 * d6; if (vb <= 0f && d2 >= 0f && d6 <= 0f) { float w = d2 / (d2 - d6); result.m_closestPointOnSimplex = a + w * ac; result.m_usedVertices.Set(0, true); result.m_usedVertices.Set(2, true); result.SetBarycentricCoordinates(1 - w, 0, w, 0); 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 float va = d3 * d6 - d5 * d4; if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f) { float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); result.m_closestPointOnSimplex = b + w * (c - b); result.m_usedVertices.Set(1, true); result.m_usedVertices.Set(2, true); result.SetBarycentricCoordinates(0, 1 - w, w, 0); 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) float denom = 1f / (va + vb + vc); float v1 = vb * denom; float w1 = vc * denom; result.m_closestPointOnSimplex = a + ab * v1 + ac * w1; result.m_usedVertices.Set(0, true); result.m_usedVertices.Set(1, true); result.m_usedVertices.Set(2, true); result.SetBarycentricCoordinates(1 - v1 - w1, v1, w1, 0); return(true); // return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = float(1.0) - v - w }
public bool ClosestPtPointTetrahedron(ref IndexedVector3 p, ref IndexedVector3 a, ref IndexedVector3 b, ref IndexedVector3 c, ref IndexedVector3 d, ref SubSimplexClosestResult finalResult) { // Start ref assuming point inside all halfspaces, so closest to itself finalResult.m_closestPointOnSimplex = p; finalResult.m_usedVertices.SetAll(true); 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; } SubSimplexClosestResult tempResult = BulletGlobals.SubSimplexClosestResultPool.Get(); tempResult.Reset(); float bestSqDist = float.MaxValue; // If point outside face abc then compute closest point on abc if (pointOutsideABC != 0) { ClosestPtPointTriangle(ref p, ref a, ref b, ref c, ref tempResult); IndexedVector3 q = tempResult.m_closestPointOnSimplex; float sqDist = (q - p).LengthSquared(); // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; //convert result bitmask! finalResult.m_usedVertices.SetAll(false); finalResult.m_usedVertices.Set(0, tempResult.m_usedVertices.Get(0)); finalResult.m_usedVertices.Set(1, tempResult.m_usedVertices.Get(1)); finalResult.m_usedVertices.Set(2, tempResult.m_usedVertices.Get(2)); finalResult.SetBarycentricCoordinates( tempResult.m_barycentricCoords.X, tempResult.m_barycentricCoords.Y, tempResult.m_barycentricCoords.Z, 0 ); } } // Repeat test for face acd if (pointOutsideACD != 0) { ClosestPtPointTriangle(ref p, ref a, ref c, ref d, ref tempResult); IndexedVector3 q = tempResult.m_closestPointOnSimplex; float sqDist = (q - p).LengthSquared(); // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; //convert result bitmask! finalResult.m_usedVertices.SetAll(false); finalResult.m_usedVertices.Set(0, tempResult.m_usedVertices.Get(0)); finalResult.m_usedVertices.Set(2, tempResult.m_usedVertices.Get(1)); finalResult.m_usedVertices.Set(3, tempResult.m_usedVertices.Get(2)); finalResult.SetBarycentricCoordinates( tempResult.m_barycentricCoords.X, 0, tempResult.m_barycentricCoords.Y, tempResult.m_barycentricCoords.Z); } } // Repeat test for face adb if (pointOutsideADB != 0) { ClosestPtPointTriangle(ref p, ref a, ref d, ref b, ref tempResult); IndexedVector3 q = tempResult.m_closestPointOnSimplex; float sqDist = (q - p).LengthSquared(); // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; //convert result bitmask! finalResult.m_usedVertices.SetAll(false); finalResult.m_usedVertices.Set(0, tempResult.m_usedVertices.Get(0)); finalResult.m_usedVertices.Set(1, tempResult.m_usedVertices.Get(2)); finalResult.m_usedVertices.Set(3, tempResult.m_usedVertices.Get(1)); finalResult.SetBarycentricCoordinates( tempResult.m_barycentricCoords.X, tempResult.m_barycentricCoords.Z, 0, tempResult.m_barycentricCoords.Y); } } // Repeat test for face bdc if (pointOutsideBDC != 0) { ClosestPtPointTriangle(ref p, ref b, ref d, ref c, ref tempResult); IndexedVector3 q = tempResult.m_closestPointOnSimplex; float sqDist = (q - p).LengthSquared(); // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; //convert result bitmask! finalResult.m_usedVertices.SetAll(false); finalResult.m_usedVertices.Set(1, tempResult.m_usedVertices.Get(0)); finalResult.m_usedVertices.Set(2, tempResult.m_usedVertices.Get(2)); finalResult.m_usedVertices.Set(3, tempResult.m_usedVertices.Get(1)); finalResult.SetBarycentricCoordinates( 0, tempResult.m_barycentricCoords.X, tempResult.m_barycentricCoords.Z, tempResult.m_barycentricCoords.Y); } } BulletGlobals.SubSimplexClosestResultPool.Free(tempResult); //help! we ended up full ! if (finalResult.m_usedVertices.Get(0) && finalResult.m_usedVertices.Get(1) && finalResult.m_usedVertices.Get(2) && finalResult.m_usedVertices.Get(3)) { return true; } return true; }
public bool ClosestPtPointTetrahedron(ref IndexedVector3 p, ref IndexedVector3 a, ref IndexedVector3 b, ref IndexedVector3 c, ref IndexedVector3 d, ref SubSimplexClosestResult finalResult) { // Start ref assuming point inside all halfspaces, so closest to itself finalResult.m_closestPointOnSimplex = p; finalResult.m_usedVertices.SetAll(true); 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); } SubSimplexClosestResult tempResult = BulletGlobals.SubSimplexClosestResultPool.Get(); tempResult.Reset(); float bestSqDist = float.MaxValue; // If point outside face abc then compute closest point on abc if (pointOutsideABC != 0) { ClosestPtPointTriangle(ref p, ref a, ref b, ref c, ref tempResult); IndexedVector3 q = tempResult.m_closestPointOnSimplex; float sqDist = (q - p).LengthSquared(); // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; //convert result bitmask! finalResult.m_usedVertices.SetAll(false); finalResult.m_usedVertices.Set(0, tempResult.m_usedVertices.Get(0)); finalResult.m_usedVertices.Set(1, tempResult.m_usedVertices.Get(1)); finalResult.m_usedVertices.Set(2, tempResult.m_usedVertices.Get(2)); finalResult.SetBarycentricCoordinates( tempResult.m_barycentricCoords.X, tempResult.m_barycentricCoords.Y, tempResult.m_barycentricCoords.Z, 0 ); } } // Repeat test for face acd if (pointOutsideACD != 0) { ClosestPtPointTriangle(ref p, ref a, ref c, ref d, ref tempResult); IndexedVector3 q = tempResult.m_closestPointOnSimplex; float sqDist = (q - p).LengthSquared(); // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; //convert result bitmask! finalResult.m_usedVertices.SetAll(false); finalResult.m_usedVertices.Set(0, tempResult.m_usedVertices.Get(0)); finalResult.m_usedVertices.Set(2, tempResult.m_usedVertices.Get(1)); finalResult.m_usedVertices.Set(3, tempResult.m_usedVertices.Get(2)); finalResult.SetBarycentricCoordinates( tempResult.m_barycentricCoords.X, 0, tempResult.m_barycentricCoords.Y, tempResult.m_barycentricCoords.Z); } } // Repeat test for face adb if (pointOutsideADB != 0) { ClosestPtPointTriangle(ref p, ref a, ref d, ref b, ref tempResult); IndexedVector3 q = tempResult.m_closestPointOnSimplex; float sqDist = (q - p).LengthSquared(); // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; //convert result bitmask! finalResult.m_usedVertices.SetAll(false); finalResult.m_usedVertices.Set(0, tempResult.m_usedVertices.Get(0)); finalResult.m_usedVertices.Set(1, tempResult.m_usedVertices.Get(2)); finalResult.m_usedVertices.Set(3, tempResult.m_usedVertices.Get(1)); finalResult.SetBarycentricCoordinates( tempResult.m_barycentricCoords.X, tempResult.m_barycentricCoords.Z, 0, tempResult.m_barycentricCoords.Y); } } // Repeat test for face bdc if (pointOutsideBDC != 0) { ClosestPtPointTriangle(ref p, ref b, ref d, ref c, ref tempResult); IndexedVector3 q = tempResult.m_closestPointOnSimplex; float sqDist = (q - p).LengthSquared(); // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; //convert result bitmask! finalResult.m_usedVertices.SetAll(false); finalResult.m_usedVertices.Set(1, tempResult.m_usedVertices.Get(0)); finalResult.m_usedVertices.Set(2, tempResult.m_usedVertices.Get(2)); finalResult.m_usedVertices.Set(3, tempResult.m_usedVertices.Get(1)); finalResult.SetBarycentricCoordinates( 0, tempResult.m_barycentricCoords.X, tempResult.m_barycentricCoords.Z, tempResult.m_barycentricCoords.Y); } } BulletGlobals.SubSimplexClosestResultPool.Free(tempResult); //help! we ended up full ! if (finalResult.m_usedVertices.Get(0) && finalResult.m_usedVertices.Get(1) && finalResult.m_usedVertices.Get(2) && finalResult.m_usedVertices.Get(3)) { return(true); } return(true); }