KStarbound/src/main/kotlin/ru/dbotthepony/kbox2d/collision/Collision.kt

package ru.dbotthepony.kbox2d.collision

import ru.dbotthepony.kbox2d.api.*
import ru.dbotthepony.kstarbound.math.Vector2d
import ru.dbotthepony.kstarbound.math.times
import java.util.*
import kotlin.collections.ArrayList

/// Compute the point states given two manifolds. The states pertain to the transition from manifold1
/// to manifold2. So state1 is either persist or remove while state2 is either add or persist.
fun b2GetPointStates(state1: Array<PointState>, state2: Array<PointState>, manifold1: Manifold, manifold2: Manifold) {
	Arrays.fill(state1, PointState.NULL)
	Arrays.fill(state2, PointState.NULL)

	// Detect persists and removes.
	for (i in manifold1.points.indices) {
		val id = manifold1.points[i].id
		state1[i] = PointState.REMOVE

		for (j in manifold2.points.indices) {
			if (manifold2.points[j].id.key == id.key) {
				state1[i] = PointState.PERSIST
				break
			}
		}
	}

	// Detect persists and adds.
	for (i in manifold2.points.indices) {
		val id = manifold2.points[i].id
		state2[i] = PointState.ADD

		for (j in manifold1.points.indices) {
			if (manifold1.points[j].id.key == id.key) {
				state1[i] = PointState.PERSIST
				break
			}
		}
	}
}

/**
 * This is used to compute the current state of a contact manifold.
 * Evaluate the manifold with supplied transforms. This assumes
 * modest motion from the original state. This does not change the
 * point count, impulses, etc. The radii must come from the shapes
 * that generated the manifold.
 */
class WorldManifold(manifold: Manifold, xfA: Transform, radiusA: Double, xfB: Transform, radiusB: Double) :
	IWorldManifold {
	override var normal: Vector2d = Vector2d.ZERO
		private set

	override var points: Array<Vector2d> = Array(b2_maxManifoldPoints) { Vector2d.ZERO }
		private set

	override var separations: DoubleArray = DoubleArray(b2_maxManifoldPoints)
		private set

	init {
		if (manifold.points.isNotEmpty()) {
			when (manifold.type) {
				Manifold.Type.CIRCLES -> {
					normal = Vector2d.RIGHT

					val pointA = b2Mul(xfA, manifold.localPoint);
					val pointB = b2Mul(xfB, manifold.points[0].localPoint);

					if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) {
						normal = (pointB - pointA).normalized
					}

					val cA = pointA + radiusA * normal
					val cB = pointB - radiusB * normal

					points[0] = 0.5 * (cA + cB)
					separations[0] = b2Dot(cB - cA, normal)
				}

				Manifold.Type.FACE_A -> {
					normal = b2Mul(xfA.q, manifold.localNormal)
					val planePoint = b2Mul(xfA, manifold.localPoint)

					for (i in manifold.points.indices) {
						val clipPoint = b2Mul(xfB, manifold.points[i].localPoint)
						val cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
						val cB = clipPoint - radiusB * normal;

						points[i] = 0.5 * (cA + cB)
						separations[i] = b2Dot(cB - cA, normal)
					}
				}

				Manifold.Type.FACE_B -> {
					normal = b2Mul(xfB.q, manifold.localNormal)
					val planePoint = b2Mul(xfB, manifold.localPoint)

					for (i in manifold.points.indices) {
						val clipPoint = b2Mul(xfA, manifold.points[i].localPoint);
						val cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
						val cA = clipPoint - radiusA * normal;

						points[i] = 0.5 * (cA + cB)
						separations[i] = b2Dot(cA - cB, normal)
					}

					// Ensure normal points from A to B.
					normal = -normal
				}

				null -> throw IllegalArgumentException()
			}
		}
	}
}

// Sutherland-Hodgman clipping.
internal fun b2ClipSegmentToLine(
	vIn: Array<ClipVertex>,
	normal: Vector2d,
	offset: Double,
	vertexIndexA: Int
): Array<ClipVertex> {
	// Start with no output points
	val vOut = arrayOfNulls<ClipVertex>(2)
	var count = 0

	// Calculate the distance of end points to the line
	val distance0 = b2Dot(normal, vIn[0].v) - offset
	val distance1 = b2Dot(normal, vIn[1].v) - offset

	// If the points are behind the plane
	if (distance0 <= 0.0) vOut[count++] = vIn[0]
	if (distance1 <= 0.0) vOut[count++] = vIn[1]

	// If the points are on different sides of the plane
	if (distance0 * distance1 < 0.0) {
		// Find intersection point of edge and plane
		val interp = distance0 / (distance0 - distance1)

		val clipVertex = ClipVertex(
			v = vIn[0].v + interp * (vIn[1].v - vIn[0].v),
			id = ContactID(
				cf = ContactFeature(
					// VertexA is hitting edgeB.
					indexA = vertexIndexA,
					indexB = vIn[0].id.cf.indexB,
					typeA = ContactFeature.Type.VERTEX,
					typeB = ContactFeature.Type.FACE,
				)
			)
		)

		vOut[count] = clipVertex
		count++

		check(count == 2) { "Expected output to be 2 in size, got $count" }
	}

	if (count == 2)
		return vOut as Array<ClipVertex>
	else if (count == 1)
		return arrayOf(vOut[0]!!)
	else if (count == 0)
		return arrayOf()
	else
		throw IllegalStateException(count.toString())
}

internal fun b2TestOverlap(
	shapeA: IShape<*>,
	indexA: Int,
	shapeB: IShape<*>,
	indexB: Int,
	xfA: Transform,
	xfB: Transform,
): Boolean {
	return b2Distance(
		SimplexCache(),
		DistanceProxy(shapeA, indexA),
		DistanceProxy(shapeB, indexB),
		xfA, xfB, true
	).distance < 10.0 * b2_epsilon
}