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

Go to download
package ru.casperix.math.intersection

import ru.casperix.math.vector.float64.Vector2d
import kotlin.math.abs
import kotlin.math.max
import kotlin.math.min
import kotlin.math.sqrt

object ConvexHullIntersection {
	class MinimalTranslationVector(var normal: Vector2d, var depth: Double)

	fun hasIntersection(listA: DoubleArray,  listB: DoubleArray): Boolean {
		return hasIntersection(listA, listA.size, listB, listB.size)
	}

	fun hasIntersection(listA: DoubleArray, countA: Int, listB: DoubleArray, countB: Int): Boolean {
		if (overlapsOnAxisOfShape(listA, 0, countA, listB, 0, countB, true, null)) {
			return overlapsOnAxisOfShape(listB, 0, countB, listA, 0, countA, true, null)
		}
		return false
	}

	fun getPenetration(listA: DoubleArray,  listB: DoubleArray): Vector2d? {
		return getPenetration(listA, listA.size, listB, listB.size)
	}

	fun getPenetration(listA: DoubleArray, countA: Int, listB: DoubleArray, countB: Int): Vector2d? {
		val mtv1 = MinimalTranslationVector(Vector2d.ZERO, Double.MAX_VALUE)
		val hasA = overlapsOnAxisOfShape(listA, 0, countA, listB, 0, countB, true, mtv1)
		if (hasA) {
			val mtv2 = MinimalTranslationVector(Vector2d.ZERO, Double.MAX_VALUE)
			if (overlapsOnAxisOfShape(listB, 0, countB, listA, 0, countA, false, mtv2)) {
				val mtv0 = if (mtv1.depth <= mtv2.depth) mtv1 else mtv2
				return mtv0.normal.normalize() * mtv0.depth
			}
		}
		return null
	}

	private fun overlapsOnAxisOfShape(verts1: DoubleArray, offset1: Int, count1: Int, verts2: DoubleArray, offset2: Int, count2: Int, shapesShifted: Boolean, mtv: MinimalTranslationVector?): Boolean {
		val endA = offset1 + count1
		val endB = offset2 + count2
		// get axis of polygon A
		var i = offset1

		mtv?.depth = Double.MAX_VALUE
		mtv?.normal = Vector2d.ZERO

		while (i < endA) {
			val x1 = verts1[i]
			val y1 = verts1[i + 1]
			val x2 = verts1[(i + 2) % count1]
			val y2 = verts1[(i + 3) % count1]

			// Get the Axis for the 2 vertices
			var axisX = y1 - y2
			var axisY = -(x1 - x2)
			val len = sqrt((axisX * axisX + axisY * axisY))
			// We got a normalized Vector
			axisX /= len
			axisY /= len
			var minA = Double.MAX_VALUE
			var maxA = -Double.MAX_VALUE
			// project shape a on axis
			run {
				var v = offset1
				while (v < endA) {
					val p = verts1[v] * axisX + verts1[v + 1] * axisY
					minA = min(minA, p)
					maxA = max(maxA, p)
					v += 2
				}
			}
			var minB = Double.MAX_VALUE
			var maxB = -Double.MAX_VALUE

			// project shape b on axis
			var v = offset2
			while (v < endB) {
				val p = verts2[v] * axisX + verts2[v + 1] * axisY
				minB = min(minB, p)
				maxB = max(maxB, p)
				v += 2
			}
			// There is a gap
			if (maxA < minB || maxB < minA) {
				return false
			} else {
				var o: Double = min(maxA, maxB) - max(minA, minB)
				val aContainsB = minA < minB && maxA > maxB
				val bContainsA = minB < minA && maxB > maxA
				// if it contains one or another
				var mins = 0.0
				var maxs = 0.0
				if (aContainsB || bContainsA) {
					mins = abs(minA - minB)
					maxs = abs(maxA - maxB)
					o += min(mins, maxs)
				}
				if (mtv != null && mtv.depth > o) {
					mtv.depth = o
					var condition: Boolean
					if (shapesShifted) {
						condition = minA < minB
						axisX = if (condition) axisX else -axisX
						axisY = if (condition) axisY else -axisY
					} else {
						condition = minA > minB
						axisX = if (condition) axisX else -axisX
						axisY = if (condition) axisY else -axisY
					}
					if (aContainsB || bContainsA) {
						condition = mins > maxs
						axisX = if (condition) axisX else -axisX
						axisY = if (condition) axisY else -axisY
					}
					mtv.normal = Vector2d(axisX, axisY)
				}
			}
			i += 2
		}

		return true
	}

}