// GetBoxTriangleIntersectionPoints // Pushes intersection points onto the back of pts. Returns the // number of points found. // Points that are close together (compared to // combinationDistance) get combined private int GetBoxTriangleIntersectionPoints(List <MyCP> pts, MyBox box, MyColDetVoxelTriangle triangle, float combinationDistance) { // first intersect each edge of the box with the triangleVertexes MyBox.Edge[] edges; box.GetEdges(out edges); Vector3[] boxPts; box.GetCornerPoints(out boxPts); float tS; float tv1, tv2; int iEdge; for (iEdge = 0; iEdge < 12; ++iEdge) { MyBox.Edge edge = edges[iEdge]; MySegment seg = new MySegment(boxPts[(int)edge.Ind0], boxPts[(int)edge.Ind1] - boxPts[(int)edge.Ind0]); if (this.SegmentTriangleIntersection(out tS, out tv1, out tv2, seg, triangle)) { float depthA = Vector3.Dot(boxPts[(int)edge.Ind0] - seg.GetPoint(tS), triangle.Normal); float depthB = Vector3.Dot(boxPts[(int)edge.Ind1] - seg.GetPoint(tS), triangle.Normal); AddPoint(pts, seg.GetPoint(tS), triangle.Normal, depthA < depthB ? depthA : depthB, combinationDistance * combinationDistance); } } Vector3 pos, n; // now each edge of the triangleVertexes with the box for (iEdge = 0; iEdge < 3; ++iEdge) { Vector3 pt0 = triangle.GetPoint(iEdge); Vector3 pt1 = triangle.GetPoint((iEdge + 1) % 3); MySegment s1 = new MySegment(pt0, pt1 - pt0); MySegment s2 = new MySegment(pt1, pt0 - pt1); if (box.SegmentIntersect(out tS, out pos, out n, s1)) { float depthA = Vector3.Dot(pt0 - pos, triangle.Normal); float depthB = Vector3.Dot(pt1 - pos, triangle.Normal); AddPoint(pts, pos, triangle.Normal, depthA < depthB ? depthA : depthB, combinationDistance * combinationDistance); } if (box.SegmentIntersect(out tS, out pos, out n, s2)) { float depthA = Vector3.Dot(pt0 - pos, triangle.Normal); float depthB = Vector3.Dot(pt1 - pos, triangle.Normal); AddPoint(pts, pos, triangle.Normal, depthA < depthB ? depthA : depthB, combinationDistance * combinationDistance); } } return(pts.Count); }
// GetBoxTriangleIntersectionPoints // Pushes intersection points onto the back of pts. Returns the // number of points found. // Points that are close together (compared to // combinationDistance) get combined private int GetBoxTriangleIntersectionPoints(List<MyCP> pts, MyBox box, MyColDetVoxelTriangle triangle, float combinationDistance) { // first intersect each edge of the box with the triangleVertexes MyBox.Edge[] edges; box.GetEdges(out edges); Vector3[] boxPts; box.GetCornerPoints(out boxPts); float tS; float tv1, tv2; int iEdge; for (iEdge = 0; iEdge < 12; ++iEdge) { MyBox.Edge edge = edges[iEdge]; MySegment seg = new MySegment(boxPts[(int)edge.Ind0], boxPts[(int)edge.Ind1] - boxPts[(int)edge.Ind0]); if (this.SegmentTriangleIntersection(out tS, out tv1, out tv2, seg, triangle)) { float depthA = Vector3.Dot(boxPts[(int)edge.Ind0] - seg.GetPoint(tS), triangle.Normal); float depthB = Vector3.Dot(boxPts[(int)edge.Ind1] - seg.GetPoint(tS), triangle.Normal); AddPoint(pts, seg.GetPoint(tS),triangle.Normal, depthA < depthB ? depthA : depthB , combinationDistance * combinationDistance); } } Vector3 pos, n; // now each edge of the triangleVertexes with the box for (iEdge = 0; iEdge < 3; ++iEdge) { Vector3 pt0 = triangle.GetPoint(iEdge); Vector3 pt1 = triangle.GetPoint((iEdge + 1) % 3); MySegment s1 = new MySegment(pt0, pt1 - pt0); MySegment s2 = new MySegment(pt1, pt0 - pt1); if (box.SegmentIntersect(out tS, out pos, out n, s1)) { float depthA = Vector3.Dot(pt0 - pos, triangle.Normal); float depthB = Vector3.Dot(pt1 - pos, triangle.Normal); AddPoint(pts, pos, triangle.Normal, depthA < depthB ? depthA : depthB, combinationDistance * combinationDistance); } if (box.SegmentIntersect(out tS, out pos, out n, s2)) { float depthA = Vector3.Dot(pt0 - pos, triangle.Normal); float depthB = Vector3.Dot(pt1 - pos, triangle.Normal); AddPoint(pts, pos, triangle.Normal, depthA < depthB ? depthA : depthB, combinationDistance * combinationDistance); } } return pts.Count; }
private bool DoOverlapBoxTriangleStaticTest(MyBox box, ref MyColDetVoxelTriangle triangle) { Matrix dirs0 = box.Orientation; #region triEdge0 Vector3 pt0; Vector3 pt1; triangle.GetPoint(0, out pt0); triangle.GetPoint(1, out pt1); Vector3 triEdge0; Vector3.Subtract(ref pt1, ref pt0, out triEdge0); if (triEdge0.LengthSquared() < MyPhysicsConfig.Epsilon) { return(false); } #endregion #region triEdge1 Vector3 pt2; triangle.GetPoint(2, out pt2); Vector3 triEdge1; Vector3.Subtract(ref pt2, ref pt1, out triEdge1); if (triEdge1.LengthSquared() < MyPhysicsConfig.Epsilon) { return(false); } #endregion #region triEdge2 Vector3 triEdge2; Vector3.Subtract(ref pt0, ref pt2, out triEdge2); if (triEdge2.LengthSquared() < MyPhysicsConfig.Epsilon) { return(false); } #endregion triEdge0.Normalize(); triEdge1.Normalize(); triEdge2.Normalize(); Vector3 triNormal = triangle.Plane.Normal; m_axes[0] = triNormal; m_axes[1] = dirs0.Right; m_axes[2] = dirs0.Up; m_axes[3] = dirs0.Backward; Vector3.Cross(ref m_axes[1], ref triEdge0, out m_axes[4]); Vector3.Cross(ref m_axes[1], ref triEdge1, out m_axes[5]); Vector3.Cross(ref m_axes[1], ref triEdge2, out m_axes[6]); Vector3.Cross(ref m_axes[2], ref triEdge0, out m_axes[7]); Vector3.Cross(ref m_axes[2], ref triEdge1, out m_axes[8]); Vector3.Cross(ref m_axes[2], ref triEdge2, out m_axes[9]); Vector3.Cross(ref m_axes[3], ref triEdge0, out m_axes[10]); Vector3.Cross(ref m_axes[3], ref triEdge1, out m_axes[11]); Vector3.Cross(ref m_axes[3], ref triEdge2, out m_axes[12]); // the overlap depths along each axis // see if the boxes are separate along any axis, and if not keep a // record of the depths along each axis int i; for (i = 0; i < numAxes; ++i) { m_overlapDepths[i] = 1.0f; if (Disjoint(out m_overlapDepths[i], m_axes[i], box, triangle, MyPhysicsConfig.CollisionEpsilon)) { return(false); } } // The box overlap, find the separation depth closest to 0. float minDepth = float.MaxValue; int minAxis = -1; for (i = 0; i < numAxes; ++i) { // If we can't normalise the axis, skip it float l2 = m_axes[i].LengthSquared(); if (l2 < MyPhysicsConfig.Epsilon) { continue; } // Normalise the separation axis and the depth float invl = 1.0f / (float)System.Math.Sqrt(l2); m_axes[i] *= invl; m_overlapDepths[i] *= invl; // If this axis is the minimum, select it if (m_overlapDepths[i] < minDepth) { minDepth = m_overlapDepths[i]; minAxis = i; } } if (minAxis == -1) { return(false); } // Make sure the axis is facing towards the 0th box. // if not, invert it Vector3 D = box.GetCentre() - triangle.Centre; Vector3 N = m_axes[minAxis]; float depth = m_overlapDepths[minAxis]; if (Vector3.Dot(D, N) < 0.0f) { N *= -1; } MyRigidBody rbo0 = GetRigidBody1(); MyRigidBody rbo1 = GetRigidBody2(); float dt = MyPhysics.physicsSystem.GetRigidBodyModule().CurrentTimeStep; Vector3 boxOldPos = rbo0.Position; Vector3 boxNewPos = rbo0.Position + rbo0.LinearVelocity * dt; Vector3 meshPos = rbo1.Position; m_CPList.Clear(); GetBoxTriangleIntersectionPoints(m_CPList, box, triangle, MyPhysicsConfig.CollisionEpsilon); // adjust the depth #region delta Vector3 delta; Vector3.Subtract(ref boxNewPos, ref boxOldPos, out delta); #endregion int numPts = m_CPList.Count; if (numPts > 0) { return(true); } else { return(false); } }
private bool DoOverlapBoxTriangleStaticTest(MyBox box, ref MyColDetVoxelTriangle triangle) { Matrix dirs0 = box.Orientation; #region triEdge0 Vector3 pt0; Vector3 pt1; triangle.GetPoint(0, out pt0); triangle.GetPoint(1, out pt1); Vector3 triEdge0; Vector3.Subtract(ref pt1, ref pt0, out triEdge0); if (triEdge0.LengthSquared() < MyPhysicsConfig.Epsilon) return false; #endregion #region triEdge1 Vector3 pt2; triangle.GetPoint(2, out pt2); Vector3 triEdge1; Vector3.Subtract(ref pt2, ref pt1, out triEdge1); if (triEdge1.LengthSquared() < MyPhysicsConfig.Epsilon) return false; #endregion #region triEdge2 Vector3 triEdge2; Vector3.Subtract(ref pt0, ref pt2, out triEdge2); if (triEdge2.LengthSquared() < MyPhysicsConfig.Epsilon) return false; #endregion triEdge0.Normalize(); triEdge1.Normalize(); triEdge2.Normalize(); Vector3 triNormal = triangle.Plane.Normal; m_axes[0] = triNormal; m_axes[1] = dirs0.Right; m_axes[2] = dirs0.Up; m_axes[3] = dirs0.Backward; Vector3.Cross(ref m_axes[1], ref triEdge0, out m_axes[4]); Vector3.Cross(ref m_axes[1], ref triEdge1, out m_axes[5]); Vector3.Cross(ref m_axes[1], ref triEdge2, out m_axes[6]); Vector3.Cross(ref m_axes[2], ref triEdge0, out m_axes[7]); Vector3.Cross(ref m_axes[2], ref triEdge1, out m_axes[8]); Vector3.Cross(ref m_axes[2], ref triEdge2, out m_axes[9]); Vector3.Cross(ref m_axes[3], ref triEdge0, out m_axes[10]); Vector3.Cross(ref m_axes[3], ref triEdge1, out m_axes[11]); Vector3.Cross(ref m_axes[3], ref triEdge2, out m_axes[12]); // the overlap depths along each axis // see if the boxes are separate along any axis, and if not keep a // record of the depths along each axis int i; for (i = 0; i < numAxes; ++i) { m_overlapDepths[i] = 1.0f; if (Disjoint(out m_overlapDepths[i], m_axes[i], box, triangle, MyPhysicsConfig.CollisionEpsilon)) return false; } // The box overlap, find the separation depth closest to 0. float minDepth = float.MaxValue; int minAxis = -1; for (i = 0; i < numAxes; ++i) { // If we can't normalise the axis, skip it float l2 = m_axes[i].LengthSquared(); if (l2 < MyPhysicsConfig.Epsilon) continue; // Normalise the separation axis and the depth float invl = 1.0f / (float)System.Math.Sqrt(l2); m_axes[i] *= invl; m_overlapDepths[i] *= invl; // If this axis is the minimum, select it if (m_overlapDepths[i] < minDepth) { minDepth = m_overlapDepths[i]; minAxis = i; } } if (minAxis == -1) return false; // Make sure the axis is facing towards the 0th box. // if not, invert it Vector3 D = box.GetCentre() - triangle.Centre; Vector3 N = m_axes[minAxis]; float depth = m_overlapDepths[minAxis]; if (Vector3.Dot(D, N) < 0.0f) N *= -1; MyRigidBody rbo0 = GetRigidBody1(); MyRigidBody rbo1 = GetRigidBody2(); float dt = MyPhysics.physicsSystem.GetRigidBodyModule().CurrentTimeStep; Vector3 boxOldPos = rbo0.Position; Vector3 boxNewPos = rbo0.Position + rbo0.LinearVelocity * dt; Vector3 meshPos = rbo1.Position; m_CPList.Clear(); GetBoxTriangleIntersectionPoints(m_CPList, box, triangle, MyPhysicsConfig.CollisionEpsilon); // adjust the depth #region delta Vector3 delta; Vector3.Subtract(ref boxNewPos, ref boxOldPos, out delta); #endregion int numPts = m_CPList.Count; if (numPts > 0) return true; else return false; }