commonMain.ru.casperix.math.intersection.CurveIntersectionSolver.kt Maven / Gradle / Ivy

Go to download
package ru.casperix.math.intersection

import ru.casperix.math.geometry.float32.Geometry2Float
import ru.casperix.math.angle.float32.RadianFloat
import ru.casperix.math.curve.*
import ru.casperix.math.curve.float32.*
import ru.casperix.math.geometry.*
import ru.casperix.math.intersection.float32.Intersection2Float
import ru.casperix.math.vector.float32.Vector2f
import kotlin.math.pow
import kotlin.math.sqrt

object CurveIntersectionSolver {
    private val approximatelySteps = 30

    data class Config(val tMin: Float = 0f, val tMax: Float = 1f) {
        fun isInside(tA: Float, tB: Float): Boolean {
            return (tA in tMin..tMax && tB in tMin..tMax)
        }
    }

    private fun List.reverseInfo(): List {
        return map { it.reversed() }
    }

    fun intersectionCurveWithCurve(first: ParametricCurve2f, second: ParametricCurve2f, config: Config = Config()): List {
        if (first is LineCurve2f && second is LineCurve2f) {
            return listOfNotNull(intersectionSegmentWithSegment(first, second, config))
        }
        if (first is Arc2f && second is Arc2f) {
            return Intersection2Float.getArcWithArc(first, second).map {
                val tA = first.getProjection(it)
                val tB = second.getProjection(it)
                CurveIntersection(CurvePositionEntry(first, tA, it), CurvePositionEntry(second, tB, it))
            }
        }

        if (first is LineCurve2f && second is Circle2f) {
            return intersectionLineWithCircle(first, second, config)
        }
        if (first is Circle2f && second is LineCurve2f) {
            return intersectionLineWithCircle(second, first, config).reverseInfo()
        }

        if (first is LineCurve2f && second is Arc2f) {
            return intersectionLineWithArc(first, second, config)
        }
        if (first is Arc2f && second is LineCurve2f) {
            return intersectionLineWithArc(second, first, config).reverseInfo()
        }

        if (first is LineCurve2f && second is BezierQuadratic2f) {
            return intersectionLineWithBezierQuadratic(first, second, config)
        }

        if (first is BezierQuadratic2f && second is LineCurve2f) {
            return intersectionLineWithBezierQuadratic(second, first, config).reverseInfo()
        }

        if (first is BezierQuadratic2f && second is BezierQuadratic2f) {
            return intersectionBezierQuadraticWithBezierQuadratic(first, second, config)
        }

        if (first is BezierCubic2f && second is BezierCubic2f) {
            return intersectionBezierCubicWithBezierCubic(first, second, config)
        }

        return intersectionCurveWithCurveApproximately(first, second, approximatelySteps, config)
    }

    private fun intersectionLineWithArc(a: LineCurve2f, b: Arc2f, config: Config): List {

        val circle = Circle2f(b.center, b.range)
        val intersectionList = intersectionLineWithCircle(a, circle, Config())
        if (intersectionList.isEmpty()) return emptyList()

        return intersectionList.filter { intersection ->
            b.isAngleInside(RadianFloat.byDirection(intersection.position - b.center))
        }.map { intersection ->
            CurveIntersection(
                intersection.first,
                CurvePositionEntry(b, b.getProjection(intersection.position), intersection.position)
            )
        }
    }

    private fun intersectionLineWithCircle(a: LineCurve2f, b: Circle2f, config: Config): List {
        a.line.apply {
            b.apply {
                val d1 = v0.distTo(center)
                val d2 = v1.distTo(center)
                if (d1 < range && d2 < range) {
                    return emptyList()
                }

                val delta = delta()

                val H = v0 + Geometry2Float.projectionByDirection( center - v0, delta)
                val h = (H - center).length()
                if (h > range) {
                    return emptyList()
                }

                val x = sqrt(range.pow(2) - h.pow(2))

                val offset = delta.normalize() * x
                val result = mutableListOf()

                val candidate1 = H - offset
                val candidate2 = H + offset

                if (Intersection2Float.hasPointWithSegment(candidate1, a.line)) result += candidate1
                if (Intersection2Float.hasPointWithSegment(candidate2, a.line)) result += candidate2

                return result.map {
                    CurveIntersection(
                        CurvePositionEntry(a, a.getProjection(it), it),
                        CurvePositionEntry(b, b.getParameterByPoint(it), it),
                    )
                }
            }
        }
    }

    fun intersectionSegmentWithSegment(a: LineCurve2f, b: LineCurve2f, config: Config): CurveIntersection? {
        val result = Intersection2Float.getSegmentWithSegment(a.line, b.line) ?: return null

        val tA = getTForLine(a.line, result)
        val tB = getTForLine(b.line, result)
        if (tA < config.tMin || tA > config.tMax || tB < config.tMin || tB > config.tMax) {
            return null
        }
        return CurveIntersection(CurvePositionEntry(a, tA, result), CurvePositionEntry(b, tB, result))
    }

    private fun getTForLine(line: Line2f, intersection: Vector2f): Float {
        return line.v0.distTo(intersection) / line.v0.distTo(line.v1)

    }

    fun intersectionLineWithBezierQuadratic(
        a: LineCurve2f,
        b: BezierQuadratic2f,
        config: Config,
    ): List {
        //If covers triangles no intersect... Spline exactly not intersect
        if (!Intersection2Float.hasTriangleWithTriangle(
                Triangle2f(a.line.v0, a.line.v1, a.line.v1),
                Triangle2f(b.p0, b.p1, b.p2),
            )
        ) return emptyList()
        return intersectionLineCurveWithCurveApproximately(a, b, approximatelySteps, config)
    }

    fun intersectionBezierQuadraticWithBezierQuadratic(
        a: BezierQuadratic2f,
        b: BezierQuadratic2f,
        config: Config,
    ): List {
        //If covers triangles no intersect... Spline exactly not intersect
        if (!Intersection2Float.hasTriangleWithTriangle(
                Triangle2f(a.p0, a.p1, a.p2),
                Triangle2f(b.p0, b.p1, b.p2),
            )
        ) return emptyList()

        return intersectionCurveWithCurveApproximately(a, b, approximatelySteps, config)
    }

    fun intersectionBezierCubicWithBezierCubic(
        a: BezierCubic2f,
        b: BezierCubic2f,
        config: Config
    ): List {
        //If covers quads no intersect... Spline exactly not intersect
        if (!Intersection2Float.hasQuadWithQuad(
                Quad2f(a.p0, a.p1, a.p2, a.p3),
                Quad2f(b.p0, b.p1, b.p2, b.p3),
            )
        ) return emptyList()

        return intersectionCurveWithCurveApproximately(a, b, approximatelySteps, config)
    }

    fun intersectionCurveWithCurveApproximately(
        a: ParametricCurve2f,
        b: ParametricCurve2f,
        segments: Int,
        config: Config,
    ): List {
        val aPoints = getPositionList(a, segments + 1)
        val bPoints = getPositionList(b, segments + 1)

        val output = mutableListOf()
        (0 until segments).forEach { partA ->
            (0 until segments).forEach { partB ->
                val lineA = Line2f(aPoints[partA], aPoints[partA + 1])
                val lineB = Line2f(bPoints[partB], bPoints[partB + 1])


                intersectionSegmentWithSegment(LineCurve2f(lineA), LineCurve2f(lineB), config)?.also {
                    val tA = (partA + it.first.t) / segments.toFloat()
                    val tB = (partB + it.second.t) / segments.toFloat()

                    output += CurveIntersection(
                        CurvePositionEntry(a, tA, it.position),
                        CurvePositionEntry(b, tB, it.position)
                    )
                }
            }
        }
        return output
    }

    fun intersectionLineCurveWithCurveApproximately(
        a: LineCurve2f,
        b: ParametricCurve2f,
        segments: Int,
        config: Config,
    ): List {
        val lineA = a.line
        val bPoints = getPositionList(b, segments + 1)

        val output = mutableListOf()
        (0 until segments).forEach { partB ->
            val lineB = Line2f(bPoints[partB], bPoints[partB + 1])


            intersectionSegmentWithSegment(LineCurve2f(lineA), LineCurve2f(lineB), config)?.also {
                val tA = getTForLine(lineA, it.position)
                val tB = (partB + it.second.t) / segments.toFloat()

                output += CurveIntersection(
                    CurvePositionEntry(a, tA, it.position),
                    CurvePositionEntry(b, tB, it.position)
                )
            }

        }
        return output
    }

    private fun getPositionList(curve: ParametricCurve2f, points: Int): List {
        if (points <= 1) {
            throw Exception("Expected that point amount 2 or more. Actual is $points")
        }
        return (0 until points).map {
            curve.getPosition(it.toFloat() / (points - 1))
        }
    }
}