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

Go to download
package ru.casperix.math.intersection

import ru.casperix.math.axis_aligned.float32.Box2f
import ru.casperix.math.axis_aligned.float32.Box3f
import ru.casperix.math.axis_aligned.float64.Box2d
import ru.casperix.math.axis_aligned.float64.Box3d
import ru.casperix.math.axis_aligned.int32.Axis3i
import ru.casperix.math.axis_aligned.int32.Box2i
import ru.casperix.math.geometry.*
import ru.casperix.math.geometry.float64.Capsule3d
import ru.casperix.math.geometry.float64.Sphere3d
import ru.casperix.math.vector.float64.Vector3d
import ru.casperix.misc.clamp
import kotlin.math.absoluteValue
import kotlin.math.roundToInt


fun getPenetrationContinuousBoxWithBox(mainBox: Capsule3d, otherBox: Box3d, maxStepAmount: Int = 100): Vector3d? {
    val translationMin = mainBox.path.v0.lower(mainBox.path.v1) - Vector3d(mainBox.range)
    val translationMax = mainBox.path.v0.upper(mainBox.path.v1) + Vector3d(mainBox.range)
    val translationBox = Box3d(translationMin, translationMax)

    if (!hasIntersectionBoxWithBox(translationBox, otherBox)) return null

    val length = mainBox.path.length()
    val lastStep = (length / mainBox.range * 0.5).roundToInt().clamp(1, maxStepAmount)

    for (step in 0..lastStep) {
        val pathFactor = step.toDouble() / lastStep.toDouble()
        val itemBox = mainBox.getBox(pathFactor)

        val penetration = getPenetrationBoxWithBox(itemBox, otherBox)
        if (penetration != null) {
            return penetration
        }
    }
    return null
}


fun hasIntersectionBoxWithBox(A: Box2i, B: Box2i): Boolean {
    if (A.max.y - B.min.y < 0) return false
    if (B.max.y - A.min.y < 0) return false
    if (A.max.x - B.min.x < 0) return false
    if (B.max.x - A.min.x < 0) return false

    return true
}

fun hasIntersectionBoxWithBox(A: Box2f, B: Box2f): Boolean {
    if (A.max.y - B.min.y <= 0f) return false
    if (B.max.y - A.min.y <= 0f) return false
    if (A.max.x - B.min.x <= 0f) return false
    if (B.max.x - A.min.x <= 0f) return false

    return true
}

fun hasIntersectionBoxWithBox(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
}

fun hasIntersectionBoxWithBox(A: Box3f, B: Box3f): Boolean {
    if (A.max.z - B.min.z < 0f) return false
    if (B.max.z - A.min.z < 0f) return false
    if (A.max.y - B.min.y < 0f) return false
    if (B.max.y - A.min.y < 0f) return false
    if (A.max.x - B.min.x < 0f) return false
    if (B.max.x - A.min.x < 0f) return false

    return true
}

fun hasIntersectionBoxWithBox(A: Box3d, B: Box3d): Boolean {
    if (A.max.z - B.min.z < 0.0) return false
    if (B.max.z - A.min.z < 0.0) return false

    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
}

fun hasPenetrationBoxWithBox(A: Box3d, B: Box3d): Boolean {
    if (A.max.z - B.min.z <= 0.0) return false
    if (B.max.z - A.min.z <= 0.0) return false

    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
}


/**
 * 	В случае пересечения вернет вектор проникновения A in B
 */
fun getPenetrationBoxWithBox(A: Box3d, B: Box3d): Vector3d? {
    val z1 = A.max.z - B.min.z
    if (z1 < 0.0) return null

    val z2 = B.max.z - A.min.z
    if (z2 < 0.0) return null

    val y1 = A.max.y - B.min.y
    if (y1 < 0.0) return null

    val y2 = B.max.y - A.min.y
    if (y2 < 0.0) return null

    val x1 = A.max.x - B.min.x
    if (x1 < 0.0) return null

    val x2 = B.max.x - A.min.x
    if (x2 < 0.0) return null

    //	if B > A, value is negative
    val z = if (z1 < z2) z1 else -z2
    val x = if (x1 < x2) x1 else -x2
    val y = if (y1 < y2) y1 else -y2

    val zAbs = z.absoluteValue
    val yAbs = y.absoluteValue
    val xAbs = x.absoluteValue

    if (zAbs <= xAbs && zAbs <= yAbs) {
        return Vector3d(0.0, 0.0, z)
    }

    if (yAbs <= xAbs && yAbs <= zAbs) {
        return Vector3d(0.0, y, 0.0)
    }

    if (xAbs <= zAbs && xAbs <= yAbs) {
        return Vector3d(x, 0.0, 0.0)
    }

    return null
}

fun intersectionOctagonWithOctagon(A: Octagon3d, B: Octagon3d): Vector3d? {
    A.getFaces().forEach { aFace ->
        val a1 = aFace.getFace(0)
        val a2 = aFace.getFace(1)
        B.getFaces().forEach { bFace ->
            val b1 = bFace.getFace(0)
            val b2 = bFace.getFace(1)
            intersectionTriangleWithTriangle(a1, b1)?.let { return it }
            intersectionTriangleWithTriangle(a2, b1)?.let { return it }
            intersectionTriangleWithTriangle(a1, b2)?.let { return it }
            intersectionTriangleWithTriangle(a2, b2)?.let { return it }
        }
    }
    return null
}

fun intersectionTriangleWithTriangle(A: Triangle3d, B: Triangle3d): Vector3d? {
    intersectionLineWithTriangle(Line3d(A.v0, A.v1), B)?.let { return it }
    intersectionLineWithTriangle(Line3d(A.v1, A.v2), B)?.let { return it }
    intersectionLineWithTriangle(Line3d(A.v2, A.v0), B)?.let { return it }
    intersectionLineWithTriangle(Line3d(B.v0, B.v1), A)?.let { return it }
    intersectionLineWithTriangle(Line3d(B.v1, B.v2), A)?.let { return it }
    intersectionLineWithTriangle(Line3d(B.v2, B.v0), A)?.let { return it }
    return null
}

//	todo: incorrect now
fun intersectionBoxWithSphere(box: Box3d, sphere: Sphere3d): Line3d? {
    val boxN = Box3d(box.min - sphere.center, box.max - sphere.center)

    var minDest = Double.POSITIVE_INFINITY
    var minLine: Line3d? = null

    boxN.indices.forEach {
        val dest = sphere.range - boxN.getCorner(it).length()
        if (dest >= 0.0 && dest < minDest) {
            minDest = dest
            minLine = Line3d(box.getCorner(it), (box.getCorner(it) - sphere.center).normalize() * dest)
        }
    }

    for (axis in Axis3i.values()) {
        val sphereDirection = axis.value.toVector3d() * sphere.range
        val info = intersectionLineWithBox(Line3d(sphere.center, sphere.center + sphereDirection), box)
        if (info != null) {
            val dest = info.line.length()
            if (dest < minDest) {
                minDest = dest
                minLine = info.line
            }
        }
    }
    return minLine
}


//fun sign(p1: Vector2d, p2: Vector2d, p3: Vector2d): Double {
//	return (p1.x - p3.x) * (p2.y - p3.y) - (p2.x - p3.x) * (p1.y - p3.y)
//}
//
//fun pointInsideTriangle(point: Vector2d, v1: Vector2d, v2: Vector2d, v3: Vector2d): Boolean {
//	val d1 = sign(point, v1, v2)
//	val d2 = sign(point, v2, v3)
//	val d3 = sign(point, v3, v1)
//	val has_neg = d1 < 0 || d2 < 0 || d3 < 0
//	val has_pos = d1 > 0 || d2 > 0 || d3 > 0
//	return !(has_neg && has_pos)
//}