private static bool checkRange(ArcLoftPath a, Vector3 bDir, Vector3 center, float bDirLen, Vector3 startRight, float distance, ref float bestOffsetA, ref float bestOffsetB)
{
float offsetA;
float offsetB;
if (distance > 0 && distance < 1)
{
var farPoint = startRight + bDir * distance + center;
if (a.GetOffsetToPoint(farPoint, out offsetA))
{
Debug.DrawLine(farPoint, farPoint + Vector3.left * 5, Color.green);
Debug.DrawLine(farPoint, farPoint + Vector3.forward * 5, Color.green);
offsetB = distance * bDirLen;
if (offsetA < bestOffsetA)
{
bestOffsetA = offsetA;
}
if (offsetB < bestOffsetB)
{
bestOffsetB = offsetB;
}
return(true);
}
}
return(false);
}
private static void get2DEllipse(ArcLoftPath a, out Vector3 center, out Vector3 axisX, out Vector3 axisY, out float radiusX, out float radiusY)
{
var offset = Vector3.Dot(a.StartPoint, a.Normal) * a.Normal;
center = a.Center + offset;
var direction = (a.StartPoint + a.Center - center);
var radius = direction.magnitude;
axisX = a.Normal;
axisX.y = 0;
axisX.Normalize();
axisY = new Vector3(-axisX.z, 0, axisX.x);
radiusX = Vector3.Cross(axisX, a.Normal).magnitude *radius;
radiusY = Vector3.Cross(axisY, a.Normal).magnitude *radius;
//Debug.DrawLine(center, center + axisX * radiusX, Color.red);
//Debug.DrawLine(center, center + axisY * radiusY, Color.green);
//Debug.DrawLine(a.StartPoint + a.Center, a.Center, Color.red);
//DebugUtils.DrawCircle(center, radius, a.Normal, Color.blue);
//Debug.DrawLine(center, a.Center, Color.yellow);
}
private static bool intersectsArcArc(ArcLoftPath a, float widthA, ArcLoftPath b, float widthB, IntersectionMode mode, out float offsetA, out float offsetB)
{
Vector3 aCenter, aAxisX, aAxisY;
float aRadiusX, aRadiusY;
if (a.Normal.y > 0.99999)
{
aCenter = a.Center;
aCenter.y = a.StartPoint.y;
aAxisX = Vector3.right;
aAxisY = Vector3.forward;
var ray = a.StartPoint;
ray.y = 0;
aRadiusX = aRadiusY = ray.magnitude;
}
else
{
get2DEllipse(a, out aCenter, out aAxisX, out aAxisY, out aRadiusX, out aRadiusY);
}
Vector3 bCenter, bAxisX, bAxisY;
float bRadiusX, bRadiusY;
if (b.Normal.y > 0.99999)
{
bCenter = b.Center;
bCenter.y = b.StartPoint.y;
bAxisX = Vector3.right;
bAxisY = Vector3.forward;
var ray = b.StartPoint;
ray.y = 0;
bRadiusX = bRadiusY = ray.magnitude;
Debug.DrawLine(bCenter, b.StartPoint, Color.green);
}
else
{
get2DEllipse(b, out bCenter, out bAxisX, out bAxisY, out bRadiusX, out bRadiusY);
}
var aAngle = -Mathf.Atan2(aAxisX.z, aAxisX.x);
var bAngle = -Mathf.Atan2(bAxisX.z, bAxisX.x);
var displayOffset = Vector3.right * 50;
offsetB = offsetA = float.NaN;
return(false);
}
private static bool intersectEllipseCenter(ArcLoftPath a, out float offsetA, out float offsetB, Vector3 center, float radiusX, float radiusY, float bDirLen, Vector3 bStart, Vector3 bDir)
{
float nearDistance;
float farDistance;
if (VectorMath2D.IntersectsLineEllipse(t2d(bStart), t2d(bDir), Vector3.zero, radiusX, radiusY, out nearDistance, out farDistance))
{
float bestOffsetA = float.PositiveInfinity, bestOffsetB = float.PositiveInfinity;
if (checkRange(a, bDir, center, bDirLen, bStart, nearDistance, ref bestOffsetA, ref bestOffsetB))
{
offsetA = bestOffsetA;
offsetB = bestOffsetB;
return(true);
}
if (checkRange(a, bDir, center, bDirLen, bStart, farDistance, ref bestOffsetA, ref bestOffsetB))
{
offsetA = bestOffsetA;
offsetB = bestOffsetB;
return(true);
}
}
offsetA = offsetB = float.NaN;
return(false);
}
private ILoftPath getArc(Vector3 point, Vector3 tangent, Vector3 pointH, bool reverse)
{
#if DEBUG
if (float.IsNaN(point.x))
{
throw new InvalidOperationException("point.x is not a number");
}
if (float.IsNaN(point.y))
{
throw new InvalidOperationException("point.y is not a number");
}
if (float.IsNaN(point.z))
{
throw new InvalidOperationException("point.z is not a number");
}
if (float.IsNaN(tangent.x))
{
throw new InvalidOperationException("tangent.x is not a number");
}
if (float.IsNaN(tangent.y))
{
throw new InvalidOperationException("tangent.y is not a number");
}
if (float.IsNaN(tangent.z))
{
throw new InvalidOperationException("tangent.z is not a number");
}
if (float.IsNaN(pointH.x))
{
throw new InvalidOperationException("pointH.x is not a number");
}
if (float.IsNaN(pointH.y))
{
throw new InvalidOperationException("pointH.y is not a number");
}
if (float.IsNaN(pointH.z))
{
throw new InvalidOperationException("pointH.z is not a number");
}
#endif
//Debug.DrawLine(point, point + tangent * 10);
var lHP = pointH - point;
var pHSC = (pointH + point) / 2;
var pHSn = lHP.normalized;
var pHSd = Vector3.Dot(pHSn, pHSC);
var pSn = tangent;
var pSd = Vector3.Dot(pSn, point);
//DebugUtils.DrawPlane(pHSC, pHSn, 0.05f, Color.cyan);
//DebugUtils.DrawPlane(point, pSn, 0.05f, Color.cyan);
Ray rS;
if (intersect(pSn, pSd, pHSn, pHSd, out rS))
{
var center = rS.GetPoint(Vector3.Dot(rS.direction, point - rS.origin));
//Debug.DrawLine(rS.origin, center, Color.blue);
//DebugUtils.DrawCircle(center, (center - point).magnitude, rS.direction, Color.cyan);
//Debug.DrawLine(point, centerStart, Color.magenta);
//Debug.DrawLine(pointH, centerStart, Color.magenta);
// TODO: it isn't always shortest angle
var side = Vector3.Cross(tangent, Vector3.up);
var dot = Vector3.Dot(side, point - center);
//if (Mathf.Abs(dot) < 0.0001f)
//{
// side = Vector3.Cross(tangent, Vector3.right);
// dot = Vector3.Dot(side, point - center);
//}
var ccw = dot > 0;
return(reverse ? ArcLoftPath.Create(center, point, pointH, ccw) : ArcLoftPath.Create(center, pointH, point, ccw));
}
return(reverse ? new LinearPath(point, pointH) : new LinearPath(pointH, point));
}
private static bool intersectsEllipseEdge(ArcLoftPath a, Vector3 bStart, Vector3 bDir, Vector3 bTan, Vector3 center, float bDirLen, float widthB, float radiusX, float radiusY, out float offsetA, out float offsetB)
{
var startLeft = bStart - bTan;
var startRight = bStart + bTan;
float outerRadiusX = radiusX + widthB;
float outerRadiusY = radiusY + widthB;
float innerRadiusX = radiusX - widthB;
float innerRadiusY = radiusY - widthB;
float nearDistanceInnerLeft, farDistanceInnerLeft;
float nearDistanceInnerRight, farDistanceInnerRight;
float nearDistanceOuterLeft, farDistanceOuterLeft;
float nearDistanceOuterRight, farDistanceOuterRight;
bool intersectsInnerLeft = VectorMath2D.IntersectsLineEllipse(t2d(startLeft), t2d(bDir), Vector3.zero, innerRadiusX, innerRadiusY, out nearDistanceInnerLeft, out farDistanceInnerLeft);
bool intersectsInnerRight = VectorMath2D.IntersectsLineEllipse(t2d(startRight), t2d(bDir), Vector3.zero, innerRadiusX, innerRadiusY, out nearDistanceInnerRight, out farDistanceInnerRight);
bool intersectsOuterLeft = VectorMath2D.IntersectsLineEllipse(t2d(startLeft), t2d(bDir), Vector3.zero, outerRadiusX, outerRadiusY, out nearDistanceOuterLeft, out farDistanceOuterLeft);
bool intersectsOuterRight = VectorMath2D.IntersectsLineEllipse(t2d(startRight), t2d(bDir), Vector3.zero, outerRadiusX, outerRadiusY, out nearDistanceOuterRight, out farDistanceOuterRight);
//if (intersectsOuterLeft)
// Debug.DrawLine(startLeft + bDir * nearDistanceOuterLeft, startLeft + bDir * farDistanceOuterLeft, Color.green);
//if (intersectsOuterRight)
// Debug.DrawLine(startRight + bDir * nearDistanceOuterRight, startRight + bDir * farDistanceOuterRight, Color.green);
//if (intersectsInnerLeft)
// Debug.DrawLine(startLeft + bDir * nearDistanceInnerLeft, startLeft + bDir * farDistanceInnerLeft, Color.yellow);
//if (intersectsInnerRight)
// Debug.DrawLine(startRight + bDir * nearDistanceInnerRight, startRight + bDir * farDistanceInnerRight, Color.yellow);
float bestOffsetA = float.PositiveInfinity, bestOffsetB = float.PositiveInfinity;
bool any = false;
if (intersectsOuterLeft)
{
any |= checkRange(a, bDir, center, bDirLen, startLeft, nearDistanceOuterLeft, ref bestOffsetA, ref bestOffsetB);
//any |= checkRange(a, bDir, center, bDirLen, startLeft, farDistanceOuterLeft, ref bestOffsetA, ref bestOffsetB);
}
if (intersectsOuterRight)
{
any |= checkRange(a, bDir, center, bDirLen, startRight, nearDistanceOuterRight, ref bestOffsetA, ref bestOffsetB);
//any |= checkRange(a, bDir, center, bDirLen, startRight, farDistanceOuterRight, ref bestOffsetA, ref bestOffsetB);
}
if (intersectsInnerLeft)
{
//any |= checkRange(a, bDir, center, bDirLen, startLeft, nearDistanceInnerLeft, ref bestOffsetA, ref bestOffsetB);
any |= checkRange(a, bDir, center, bDirLen, startLeft, farDistanceInnerLeft, ref bestOffsetA, ref bestOffsetB);
}
if (intersectsInnerRight)
{
//any |= checkRange(a, bDir, center, bDirLen, startRight, nearDistanceInnerRight, ref bestOffsetA, ref bestOffsetB);
any |= checkRange(a, bDir, center, bDirLen, startRight, farDistanceInnerRight, ref bestOffsetA, ref bestOffsetB);
}
if (any)
{
offsetA = bestOffsetA;
offsetB = bestOffsetB;
return(true);
}
offsetA = offsetB = float.NaN;
return(false);
}
private static bool intersectsArcLinear(ArcLoftPath a, float widthA, LinearPath b, float widthB, IntersectionMode mode, out float offsetA, out float offsetB)
{
widthA /= 2;
widthB /= 2;
if (a.Normal.y > 0.99999f)
{
// perfect circle
var bDirLen = b.Length;
var bStart = b.Start - a.Center;
var bDir = b.Dir * bDirLen;
var bTan = b.Tangent * widthB;
var center = a.Center;
center.y = a.StartPoint.y;
var radius = a.Radius;
if (mode == IntersectionMode.Edge)
{
if (intersectsEllipseEdge(a, bStart, bDir, bTan, center, bDirLen, widthB, radius, radius, out offsetA, out offsetB))
{
return(true);
}
}
else
{
if (intersectEllipseCenter(a, out offsetA, out offsetB, center, radius, radius, bDirLen, bStart, bDir))
{
return(true);
}
}
}
else
{
Vector3 center, axisX, axisY;
float radiusX, radiusY;
get2DEllipse(a, out center, out axisX, out axisY, out radiusX, out radiusY);
var bDirLen = get2DLength(b.Dir.y, b.Length);
var bStart = project(b.Start - center, axisX, axisY);
var bDir = project(b.Dir * bDirLen, axisX, axisY);
var bTan = project(b.Tangent * widthB, axisX, axisY);
var aStart = project(a.StartPoint, axisX, axisY);
//DebugUtils.DrawEllipse(Vector3.zero, radiusX + widthA, radiusY + widthA, Color.cyan);
//DebugUtils.DrawEllipse(Vector3.zero, radiusX, radiusY, Color.yellow);
//DebugUtils.DrawEllipse(Vector3.zero, radiusX - widthA, radiusY - widthA, Color.magenta);
if (mode == IntersectionMode.Edge)
{
if (intersectsEllipseEdge(a, bStart, bDir, bTan, center, bDirLen, widthB, radiusX, radiusY, out offsetA, out offsetB))
{
return(true);
}
}
else
{
if (intersectEllipseCenter(a, out offsetA, out offsetB, center, radiusX, radiusY, bDirLen, bStart, bDir))
{
return(true);
}
}
}
offsetB = offsetA = float.NaN;
return(false);
}