// return true if they interstect private bool IntersectLineOnTwistedPlane(Vector3 lineStart, Vector3 lineEnd, bool bExtendPlaneBeyondFirstVector, bool bExtendPlaneBeyoneSecondVector, out Vector3 intersectionPoint, out Vector3 intersectForwardVector, out float distanceToSecondVector) { distanceToSecondVector = 0; intersectForwardVector = forwardVector; intersectionPoint = lineEnd; Vector3 lineVector = lineEnd - lineStart; Vector3 offsetVector = lineStart - firstOrigin; Vector3 TVxFW = Vector3.Cross(twistVector, forwardVector); Vector3 FVxFW = Vector3.Cross(firstVector, forwardVector); Vector3 OFxTW = Vector3.Cross(offsetVector, twistVector); Vector3 OFxFV = Vector3.Cross(offsetVector, firstVector); double quadraticA = lineVector.x * TVxFW.x + lineVector.y * TVxFW.y + lineVector.z * TVxFW.z; double quadraticB = lineVector.x * (FVxFW.x + OFxTW.x) + lineVector.y * (FVxFW.y + OFxTW.y) + lineVector.z * (FVxFW.z + OFxTW.z); double quadraticC = lineVector.x * OFxFV.x + lineVector.y * OFxFV.y + lineVector.z * OFxFV.z; if (MathHelpers.SolveQuadratic(quadraticA, quadraticB, quadraticC, out double u1, out double u2) == false) { return(false); } double t1 = 0, t2 = 0; Vector3 FVxLV = Vector3.Cross(firstVector, lineVector); Vector3 TWxLV = Vector3.Cross(twistVector, lineVector); Vector3 OFxLV = Vector3.Cross(offsetVector, lineVector); Vector3 LVxFW = Vector3.Cross(lineVector, forwardVector); bool bIntersect1Found = false; if ((u1 < 4) && (u1 > -4) && (u1 >= 0 || bExtendPlaneBeyondFirstVector) && (u1 <= 1 || bExtendPlaneBeyoneSecondVector)) { bIntersect1Found = this.FindIntersectValues(lineVector, offsetVector, FVxLV, TWxLV, OFxLV, LVxFW, u1, out _, out t1); if (bIntersect1Found && t1 < 0 || t1 > 1) { bIntersect1Found = false; } } bool bIntersect2Found = false; if ((u2 < 4) && (u2 > -4) && (u2 >= 0 || bExtendPlaneBeyondFirstVector) && (u2 <= 1 || bExtendPlaneBeyoneSecondVector)) { bIntersect2Found = this.FindIntersectValues(lineVector, offsetVector, FVxLV, TWxLV, OFxLV, LVxFW, u2, out _, out t2); if (bIntersect2Found && t2 < 0 || t2 > 1) { bIntersect2Found = false; } } if (!bIntersect1Found && !bIntersect2Found) { return(false); } float t = 0; if (bIntersect1Found) { t = (float)t1; } if (bIntersect2Found && (!bIntersect1Found || (bIntersect1Found && t2 < t1))) { t = (float)t2; } intersectionPoint = lineStart + lineVector * t; Vector3 intersectFirst = MathHelpers.CastSegmentToSegment(lineStart, lineVector, firstOrigin, firstVector); Vector3 intersectSecond = MathHelpers.CastSegmentToSegment(lineStart, lineVector, secondOrigin, secondVector); intersectForwardVector = Vector3.Normalize(intersectSecond - intersectFirst); distanceToSecondVector = Vector3.Distance(intersectSecond, intersectionPoint); return(true); }
private bool FindIntersectValues(Vector3 lineVector, Vector3 offsetVector, Vector3 FVxLV, Vector3 TWxLV, Vector3 OFxLV, Vector3 LVxFW, double u, out double v, out double t) { double Cu = 0; v = 0; t = 0; for (MathHelpers.Axis axis = MathHelpers.Axis.X; axis <= MathHelpers.Axis.Z; axis++) { if (axis == MathHelpers.Axis.X) { Cu = FVxLV.x + TWxLV.x * u; if (!MathHelpers.ApproximatelyZero(Cu)) { v = (OFxLV.x + LVxFW.x * u) / Cu; break; } } else if (axis == MathHelpers.Axis.Y) { Cu = FVxLV.y + TWxLV.y * u; if (!MathHelpers.ApproximatelyZero(Cu)) { v = (OFxLV.y + LVxFW.y * u) / Cu; break; } } else { Cu = FVxLV.z + TWxLV.z * u; if (!MathHelpers.ApproximatelyZero(Cu)) { v = (OFxLV.z + LVxFW.z * u) / Cu; break; } } } if (MathHelpers.ApproximatelyZero(Cu)) { return(false); } if (!MathHelpers.ApproximatelyZero(lineVector.x)) { t = (-offsetVector.x + firstVector.x * v + (forwardVector.x + twistVector.x * v) * u) / lineVector.x; } else if (!MathHelpers.ApproximatelyZero(lineVector.y)) { t = (-offsetVector.y + firstVector.y * v + (forwardVector.y + twistVector.y * v) * u) / lineVector.y; } else if (!MathHelpers.ApproximatelyZero(lineVector.z)) { t = (-offsetVector.z + firstVector.z * v + (forwardVector.z + twistVector.z * v) * u) / lineVector.z; } else { return(false); } return(true); }