commonMain.ru.casperix.math.intersection.float64.Intersection2Double.kt Maven / Gradle / Ivy

Go to download
package ru.casperix.math.intersection.float64

import ru.casperix.math.axis_aligned.float64.Box2d
import ru.casperix.math.geometry.float64.Geometry2Double.calculateDeterminant
import ru.casperix.math.geometry.float64.Geometry2Double.distPointToLine
import ru.casperix.math.geometry.float64.Geometry2Double.distPointToSegment
import ru.casperix.math.geometry.*
import ru.casperix.math.intersection.ConvexHullIntersection
import ru.casperix.math.intersection.IntersectionApi
import ru.casperix.math.intersection.float32.Intersection2Float
import ru.casperix.math.vector.float64.Vector2d
import kotlin.math.absoluteValue

object Intersection2Double : IntersectionApi {
    val EPSILON = 0.000001

    fun hasBoxWithBox(A: Box2d, B: Box2d): Boolean {
        if (A.max.y - B.min.y <= 0.0) return false
        if (B.max.y - A.min.y <= 0.0) return false
        if (A.max.x - B.min.x <= 0.0) return false
        if (B.max.x - A.min.x <= 0.0) return false

        return true
    }

    override fun hasPointWithTriangle(P: Vector2d, triangle: Triangle2d): Boolean {
// Compute vectors
        val v0 = triangle.v2 - triangle.v0
        val v1 = triangle.v1 - triangle.v0
        val v2 = P - triangle.v0

// Compute dot products
        val dot00 = v0.dot(v0)
        val dot01 = v0.dot(v1)
        val dot02 = v0.dot(v2)
        val dot11 = v1.dot(v1)
        val dot12 = v1.dot(v2)

// Compute barycentric coordinates
        val invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01)
        val u = (dot11 * dot02 - dot01 * dot12) * invDenom
        val v = (dot00 * dot12 - dot01 * dot02) * invDenom

// Check if point is in triangle
        return (u >= 0) && (v >= 0) && (u + v <= 1.0)
    }

    override fun hasPointWithPolygon(P: Vector2d, polygon: Polygon): Boolean {
        val vertices = polygon.getVertices()

        if (vertices.isEmpty()) return false
        if (vertices.size == 1) return P == vertices[0]
        if (vertices.size == 2) return Intersection2Double.hasPointWithLine(P, vertices[0], vertices[1])

        repeat(polygon.getTriangleAmount()) { index ->
            if (hasPointWithTriangle(P, polygon.getTriangle(index))) return true
        }
        return false
    }

    override fun hasTriangleWithTriangle(a: Triangle2d, b: Triangle2d): Boolean {
        val listA = doubleArrayOf(a.v0.x, a.v0.y, a.v1.x, a.v1.y, a.v2.x, a.v2.y)
        val listB = doubleArrayOf(b.v0.x, b.v0.y, b.v1.x, b.v1.y, b.v2.x, b.v2.y)

        return ConvexHullIntersection.hasIntersection(listA, listA.size, listB, listB.size)
    }

    override fun hasQuadWithQuad(a: Quad2d, b: Quad2d): Boolean {
        val listA = doubleArrayOf(a.v0.x, a.v0.y, a.v1.x, a.v1.y, a.v2.x, a.v2.y, a.v3.x, a.v3.y)
        val listB = doubleArrayOf(b.v0.x, b.v0.y, b.v1.x, b.v1.y, b.v2.x, b.v2.y, b.v3.x, b.v3.y)

        return ConvexHullIntersection.hasIntersection(listA, listA.size, listB, listB.size)
    }

    override fun hasQuadWithTriangle(a: Quad2d, b: Triangle2d): Boolean {
        val listA = doubleArrayOf(b.v0.x, b.v0.y, b.v1.x, b.v1.y, b.v2.x, b.v2.y)
        val listB = doubleArrayOf(a.v0.x, a.v0.y, a.v1.x, a.v1.y, a.v2.x, a.v2.y, a.v3.x, a.v3.y)

        return ConvexHullIntersection.hasIntersection(listA, listA.size, listB, listB.size)
    }

    override fun getSegmentWithSegment(S: Line2d, T: Line2d): Vector2d? {
        return intersectSegments(S.v0.x, S.v0.y, S.v1.x, S.v1.y, T.v0.x, T.v0.y, T.v1.x, T.v1.y)
    }

    override fun getLineWithLine(S: Line2d, T: Line2d): Vector2d? {
        return intersectInfinityLines(S.v0.x, S.v0.y, S.v1.x, S.v1.y, T.v0.x, T.v0.y, T.v1.x, T.v1.y)
    }

    override fun hasPointWithLine(P: Vector2d, T: Line2d): Boolean {
        return distPointToLine(
            P,
            T
        ) < EPSILON && (P.distTo(T.v0) + P.distTo(T.v1) - T.v0.distTo(T.v1)).absoluteValue < EPSILON
    }

    override fun hasPointWithSegment(P: Vector2d, T: Line2d): Boolean {
        return distPointToSegment(P, T) < EPSILON
    }

    /**
     * Calculate intersect for two line segments.
     * Ignore collinear segments
     */
    internal fun intersectSegments(
        x1: Double,
        y1: Double,
        x2: Double,
        y2: Double,
        x3: Double,
        y3: Double,
        x4: Double,
        y4: Double
    ): Vector2d? {
        val d = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1)
        if (d == 0.0) return null
        val yd = y1 - y3
        val xd = x1 - x3
        val ua = ((x4 - x3) * yd - (y4 - y3) * xd) / d
        if (ua < 0 || ua > 1) return null
        val ub = ((x2 - x1) * yd - (y2 - y1) * xd) / d
        if (ub < 0 || ub > 1) return null
        return Vector2d(x1 + (x2 - x1) * ua, y1 + (y2 - y1) * ua)
    }

    internal fun intersectInfinityLines(
        x1: Double,
        y1: Double,
        x2: Double,
        y2: Double,
        x3: Double,
        y3: Double,
        x4: Double,
        y4: Double
    ): Vector2d? {
        val d = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1)
        if (d == 0.0) return null

        val a = x1 * y2 - y1 * x2
        val b = x3 * y4 - y3 * x4

        val px = (a * (x3 - x4) - (x1 - x2) * b) / d
        val py = (a * (y3 - y4) - (y1 - y2) * b) / d

        return Vector2d(px, py)
    }

    override fun hasPointWithQuad(point: Vector2d, value: Quad2d): Boolean {
        val ccw = calculateDeterminant(value.v0, value.v1, value.v2)

        val factor = if (ccw >= 0) 1f else -1f

        val e0 = calculateDeterminant(value.v0, value.v1, point)
        if (e0 * factor < 0) return false

        val e1 = calculateDeterminant(value.v1, value.v2, point)
        if (e1 * factor < 0) return false

        val e2 = calculateDeterminant(value.v2, value.v3, point)
        if (e2 * factor < 0) return false

        val e3 = calculateDeterminant(value.v3, value.v0, point)
        if (e3 * factor < 0) return false

        return true
    }
}