public bool DoesBoxOverlap(BoundingBox box) { /// We are using separating axis theorem /// We have to test 4 planes from tetrahedron, 3 planes from BB (trivial) and 6 * 3 planes from cross product of edges /// /// First early out on box planes - trivial world space bounding box /// if (!BoundingBox.Intersects(m_BoundingBox, box)) { return(false); } Vector3[] corners = box.GetCorners(); Vector4 baryCentricBoundsMin = new Vector4(float.MaxValue, float.MaxValue, float.MaxValue, float.MaxValue); Vector4 baryCentricBoundsMax = new Vector4(float.MinValue, float.MinValue, float.MinValue, float.MinValue); /// Second check tetrahedron planes, easy due to barycentric matrices foreach (var vert in corners) { float b0, b1, b2, b3; CalculateBarycentricCoordinates(vert, out b0, out b1, out b2, out b3); baryCentricBoundsMin = Vector4.Minimize(baryCentricBoundsMin, new Vector4(b0, b1, b2, b3)); baryCentricBoundsMax = Vector4.Maximize(baryCentricBoundsMax, new Vector4(b0, b1, b2, b3)); } if (baryCentricBoundsMin.X > 1.0f || baryCentricBoundsMin.Y > 1.0f || baryCentricBoundsMin.Z > 1.0f || baryCentricBoundsMin.W > 1.0f) { return(false); } if (baryCentricBoundsMax.X < 0.0f || baryCentricBoundsMax.Y < 0.0f || baryCentricBoundsMax.Z < 0.0f || baryCentricBoundsMax.W < 0.0f) { return(false); } // Finally, more difficult part - planes between the edges of both Vector3[] boxVectors = { new Vector3(1, 0, 0), new Vector3(0, 1, 0), new Vector3(0, 0, 1) }; Vector3[] tetEdgeVectors = new Vector3[6]; // Note: not necessary to normalize, we are interested in axis limits overlap // And those vectors cannot be 0 (degenerated tetrahedron) tetEdgeVectors[0] = m_Vertices[1] - m_Vertices[0]; tetEdgeVectors[1] = m_Vertices[2] - m_Vertices[0]; tetEdgeVectors[2] = m_Vertices[3] - m_Vertices[0]; tetEdgeVectors[3] = m_Vertices[2] - m_Vertices[1]; tetEdgeVectors[4] = m_Vertices[3] - m_Vertices[2]; tetEdgeVectors[5] = m_Vertices[1] - m_Vertices[3]; foreach (var tetEdge in tetEdgeVectors) { foreach (var boxEdge in boxVectors) { // Normal of plane constructed by those 2 edges var planeNormal = Vector3.Cross(tetEdge, boxEdge); if (planeNormal.LengthSquared() > 0.0001f) { Vector2 tetLimits = GeometricsAlgorithms.GetAxisProjectionLimits(m_Vertices, planeNormal); Vector2 boxLimits = GeometricsAlgorithms.GetAxisProjectionLimits(corners, planeNormal); if (tetLimits.X > boxLimits.Y || tetLimits.Y < boxLimits.X) { return(false); } } } } return(true); }
public static Vector4 FindTwoTetrahedronsSplittingPlane(Tetrahedron first, Tetrahedron second) { List <Vector3> potentialSplitPlanes = new List <Vector3>(); potentialSplitPlanes.Add(new Vector3(first.m_TetPlanes[0].X, first.m_TetPlanes[0].Y, first.m_TetPlanes[0].Z)); potentialSplitPlanes.Add(new Vector3(first.m_TetPlanes[1].X, first.m_TetPlanes[1].Y, first.m_TetPlanes[1].Z)); potentialSplitPlanes.Add(new Vector3(first.m_TetPlanes[2].X, first.m_TetPlanes[2].Y, first.m_TetPlanes[2].Z)); potentialSplitPlanes.Add(new Vector3(first.m_TetPlanes[3].X, first.m_TetPlanes[3].Y, first.m_TetPlanes[3].Z)); potentialSplitPlanes.Add(new Vector3(second.m_TetPlanes[0].X, second.m_TetPlanes[0].Y, second.m_TetPlanes[0].Z)); potentialSplitPlanes.Add(new Vector3(second.m_TetPlanes[1].X, second.m_TetPlanes[1].Y, second.m_TetPlanes[1].Z)); potentialSplitPlanes.Add(new Vector3(second.m_TetPlanes[2].X, second.m_TetPlanes[2].Y, second.m_TetPlanes[2].Z)); potentialSplitPlanes.Add(new Vector3(second.m_TetPlanes[3].X, second.m_TetPlanes[3].Y, second.m_TetPlanes[3].Z)); Vector3[] tetEdgeVectorsFirst = new Vector3[6]; Vector3[] tetEdgeVectorsSecond = new Vector3[6]; tetEdgeVectorsFirst[0] = first.m_Vertices[1] - first.m_Vertices[0]; tetEdgeVectorsFirst[1] = first.m_Vertices[2] - first.m_Vertices[0]; tetEdgeVectorsFirst[2] = first.m_Vertices[3] - first.m_Vertices[0]; tetEdgeVectorsFirst[3] = first.m_Vertices[2] - first.m_Vertices[1]; tetEdgeVectorsFirst[4] = first.m_Vertices[3] - first.m_Vertices[2]; tetEdgeVectorsFirst[5] = first.m_Vertices[1] - first.m_Vertices[3]; tetEdgeVectorsSecond[0] = second.m_Vertices[1] - second.m_Vertices[0]; tetEdgeVectorsSecond[1] = second.m_Vertices[2] - second.m_Vertices[0]; tetEdgeVectorsSecond[2] = second.m_Vertices[3] - second.m_Vertices[0]; tetEdgeVectorsSecond[3] = second.m_Vertices[2] - second.m_Vertices[1]; tetEdgeVectorsSecond[4] = second.m_Vertices[3] - second.m_Vertices[2]; tetEdgeVectorsSecond[5] = second.m_Vertices[1] - second.m_Vertices[3]; foreach (var tetEdgeFirst in tetEdgeVectorsFirst) { foreach (var tetEdgeSecond in tetEdgeVectorsSecond) { // Normal of plane constructed by those 2 edges var planeNormal = Vector3.Cross(tetEdgeFirst, tetEdgeSecond); if (planeNormal.LengthSquared() > 0.001f) { planeNormal.Normalize(); potentialSplitPlanes.Add(planeNormal); } } } const float epsilon = 0.001f;// fixed 1mm foreach (var splittingPlaneNormal in potentialSplitPlanes) { Vector2 tetOneLimits = GeometricsAlgorithms.GetAxisProjectionLimits(first.m_Vertices, splittingPlaneNormal); Vector2 tetTwoLimits = GeometricsAlgorithms.GetAxisProjectionLimits(second.m_Vertices, splittingPlaneNormal); if ((tetOneLimits.X + epsilon) > tetTwoLimits.Y) { return(new Vector4(splittingPlaneNormal, -(tetOneLimits.X + tetTwoLimits.Y) / 2.0f)); } if (tetOneLimits.Y < (tetTwoLimits.X + epsilon)) { return(new Vector4(splittingPlaneNormal, -(tetOneLimits.Y + tetTwoLimits.X) / 2.0f)); } } throw new Exception("no splitting plane found"); }