CavesOfSwift/Sources/Test/Collision.swift

import Foundation
import JolkEngine


class Collision
{
	struct Edge { let p: Vec2f, n: Vec2f, w: Float }

	enum Edge3D
	{
		case triangle(normal: Vec3f, origin: Vec3f, tri: Triangle)
		case aabbFloor(normal: Vec3f, origin: Vec3f, width: Float, depth: Float)
		case flatQuad(quad: Quad, normal: Vec3f)
		case quad(quad: Quad, wind: Winding)
	}
	var edge3d = [Edge3D]()

	struct Triangle { let a: Vec3f, b: Vec3f, c: Vec3f }
	struct Quad { let a: Vec3f, b: Vec3f, c: Vec3f, d: Vec3f }

	fileprivate static let epsilon: Float = 0.00001 //.ulpOfOne

	private static func isRectangle(_ positions: [Vec3f]) -> Bool
	{
		var winding: Winding = .none
		var xdir: Float = 0.0, zdir: Float = 0.0
		let first = positions[0]
		var previous = first
		for p in positions[1...]
		{
			let (xdelta, zdelta) = (p.x - previous.x, p.z - previous.z)
			let (xzero, zzero) = (abs(xdelta) <= epsilon, abs(zdelta) <= epsilon)
			if !xzero && zzero
			{
				if xdir != 0.0 && xdelta.sign != xdir.sign { return false }
				if zdir != 0.0
				{
					switch winding
					{
					case .none: winding = xdelta.sign == zdir.sign ? .ccw : .cw
					case .cw:  if xdelta.sign == zdir.sign { return false }
					case .ccw: if xdelta.sign != zdir.sign { return false }
					}
				}
				(xdir, zdir) = (xdelta, 0.0)
			}
			else if xzero && !zzero
			{
				if zdir != 0.0 && zdelta.sign != zdir.sign { return false }
				if xdir != 0.0
				{
					switch winding
					{
					case .none: winding = zdelta.sign == xdir.sign ? .cw : .ccw
					case .cw:  if zdelta.sign != xdir.sign { return false }
					case .ccw: if zdelta.sign == xdir.sign { return false }
					}
				}
				(xdir, zdir) = (0.0, zdelta)
			}
			else if !xzero && !zzero { return false }
			previous = p
		}
		return abs(first.x - previous.x) <= epsilon || abs(first.z - previous.z) <= epsilon
	}

	enum Winding { case none, cw, ccw }

	func build(obj: ObjModel, collision: ObjModel.Object)
	{
		for face in collision.faces
		{
			switch face
			{
			case .triangle(let v1, let v2, let v3):
				let t = Triangle(obj.positions[v1.p], obj.positions[v2.p], obj.positions[v3.p])
				let n = t.normal
				if abs(n.y) < 0.25 { continue }
				edge3d.append(.triangle(normal: n, origin: (t.a + t.b + t.c) / 3.0, tri: t))
			case .quad(let v1, let v2, let v3, let v4):
				let q = Quad(obj.positions[v1.p], obj.positions[v2.p], obj.positions[v3.p], obj.positions[v4.p])
				let n = q.normals
				if !q.isFlat
				{
					if abs((n.0 + n.1 + n.2 + n.3).normalised.y) < 0.25 { continue }
					edge3d.append(.quad(quad: q, wind: q.winding))
				}
				else if q.isSimple && q.isRectangle
				{
					if abs(n.0.y) < 0.25 { continue }
					let left    = min(q.a.x, q.b.x, q.c.x, q.d.x)
					let right   = max(q.a.x, q.b.x, q.c.x, q.d.x)
					let back    = min(q.a.z, q.b.z, q.c.z, q.d.z)
					let forward = max(q.a.z, q.b.z, q.c.z, q.d.z)
					edge3d.append(.aabbFloor(normal: n.0,
						origin: (q.a + q.b + q.c + q.d) / 4.0,
						width: (right - left) / 2.0,
						depth: (forward - back) / 2.0))
				}
				else
				{
					if abs(n.0.y) < 0.25 { continue }
					edge3d.append(.flatQuad(quad: q, normal: n.0))
				}
			case .ngon(let v):
				let p = v.map { obj.positions[$0.p] }
				let n = v.reduce(.zero) { $0 + obj.normals[$1.n] }.normalised
				if abs(n.y) < 0.25 { continue }
				if Self.isRectangle(p)
				{
					let left = p.map { $0.x }.min()!, right = p.map { $0.x }.max()!
					let bottom = p.map { $0.y }.min()!, top = p.map { $0.y }.max()!
					let back = p.map { $0.z }.min()!, forward = p.map { $0.z }.max()!
					let position = Vec3f(left + right, bottom + top, back + forward) / 2.0
					edge3d.append(.aabbFloor(normal: n, origin: position,
						width: (right - left) / 2.0,
						depth: (forward - back) / 2.0))
				}
				else
				{
					let p0 = p[0]
					for i in 1..<(v.count-1)
					{
						let p1 = p[i], p2 = p[i + 1]
						edge3d.append(.triangle(normal: n, origin: (p0 + p1 + p2) / 3.0, tri: Triangle(p0, p1, p2)))
					}
				}
			default:
				continue
			}
		}
	}

	func draw(_ render: Renderer, position: Vec3f)
	{
		var lines = [Line](
			repeating: .init(from: .init(), to: .init(), colour: .zero),
			count: edge3d.count * 6)
		var i: Int = 0
		for edge in edge3d
		{
			var o: Vec3f = .zero, n: Vec3f = .zero
			switch edge
			{
			case .triangle(let norm, let pos, let tri):
				o = pos
				n = norm

				let v0 = tri.b - tri.a;
				let v1 = tri.c - tri.a;
				let v2 = position - tri.a;
				let iden = 1.0 / (v0.x * v1.z - v1.x * v0.z);
				let v = (v2.x * v1.z - v1.x * v2.z) * iden;
				let w = (v0.x * v2.z - v2.x * v0.z) * iden;
				let colour = if v >= 0.0 && w >= 0.0 && v + w <= 1.0
				{ XnaColour.Red } else { XnaColour.GreenYellow }
				//let p = triv
				//let det = (p.1.x - p.0.x) * (p.2.z - p.0.z) - (p.1.z - p.0.z) * (p.2.x - p.0.x)
				//let colour: Colour = if
				//	det * (p.1.x - p.0.x) * (position.z - p.0.z) - (p.1.z - p.0.z) * (position.x - p.0.x) >= 0,
				//	det * (p.2.x - p.1.x) * (position.z - p.1.z) - (p.2.z - p.1.z) * (position.x - p.1.x) >= 0,
				//	det * (p.0.x - p.2.x) * (position.z - p.2.z) - (p.0.z - p.2.z) * (position.x - p.2.x) >= 0
				/*
				let side = { (v1: Vec3f, v2: Vec3f, p: Vec3f) in
					(v2.z - v1.z) * (p.x - v1.x) + (v2.x + v1.x) * (position.z - v1.z) }
				let colour = if
					side(triv.0, triv.1, position) >= 0,
					side(triv.1, triv.2, position) >= 0,
					side(triv.2, triv.0, position) >= 0
				*/

				lines[i + 0] = Line(from: tri.a, to: tri.b, colour: colour)
				lines[i + 1] = Line(from: tri.b, to: tri.c, colour: colour)
				lines[i + 2] = Line(from: tri.c, to: tri.a, colour: colour)
				i += 3
			case .aabbFloor(let floorn, let floorp, let floorw, let floord):
				o = floorp
				n = floorn

				let n2 = Vec2f(n.x, n.z) / n.y
				let z0 = Vec2f(-floorw,  floord).dot(n2)
				let z1 = Vec2f(-floorw, -floord).dot(n2)
				let z2 = Vec2f( floorw, -floord).dot(n2)
				let z3 = Vec2f( floorw,  floord).dot(n2)

				let c0 = floorp + Vec3f( floorw, z0, -floord)
				let c1 = floorp + Vec3f( floorw, z1,  floord)
				let c2 = floorp + Vec3f(-floorw, z2,  floord)
				let c3 = floorp + Vec3f(-floorw, z3, -floord)

				lines[i + 0] = Line(from: c0, to: c1, colour: XnaColour.GreenYellow)
				lines[i + 1] = Line(from: c1, to: c2, colour: XnaColour.GreenYellow)
				lines[i + 2] = Line(from: c2, to: c3, colour: XnaColour.GreenYellow)
				lines[i + 3] = Line(from: c3, to: c0, colour: XnaColour.GreenYellow)
				i += 4
			case .flatQuad(let quad, let normal):
				o = (quad.a + quad.b + quad.c + quad.d) / 4.0
				n = normal
				let colour = quad.inside(point: position) ? XnaColour.Crimson : XnaColour.GreenYellow
				lines[i + 0] = Line(from: quad.a, to: quad.b, colour: colour)
				lines[i + 1] = Line(from: quad.b, to: quad.c, colour: colour)
				lines[i + 2] = Line(from: quad.c, to: quad.d, colour: colour)
				lines[i + 3] = Line(from: quad.d, to: quad.a, colour: colour)
				i += 4
			case .quad(let quad, let winding):
				/*
				let p = (
					verts.0 + (verts.1 - verts.0) * position.x,
					verts.3 + (verts.2 - verts.3) * position.x,
					verts.0 + (verts.3 - verts.0) * position.z,
					verts.1 + (verts.2 - verts.1) * position.z)

				let xdiff = Vec2f(p.0.x - p.1.x, p.2.x - p.3.x)
				let ydiff = Vec2f(p.0.z - p.1.z, p.2.z - p.3.z)
				let div = xdiff.cross(ydiff)
				guard div != 0.0 else { break }

				let d = Vec2f(
					Vec2f(p.0.x, p.0.z).cross(Vec2f(p.1.x, p.1.z)),
					Vec2f(p.2.x, p.2.z).cross(Vec2f(p.3.x, p.3.z)))
				let pos = Vec2f(d.cross(xdiff), d.cross(ydiff)) / div

				o = Vec3f(pos.x, 0.0, pos.y)
				n = .up
				*/

				o = quad.a.lerp(quad.d, 0.5).lerp(
					quad.c.lerp(quad.b, 0.5), 0.5)

				var colour = XnaColour.GreenYellow
				let vn = quad.normals
				n = (vn.0 + vn.1 + vn.2 + vn.3).normalised

				if quad.inside(point: position)
				{
					let p = winding == .ccw ? quad.reverse : quad
					let uv = p.project(point: position)
					guard uv.x.isFinite && uv.y.isFinite else
					{
						print(p)
						break
					}
					
					var pp = p.a
					pp += (p.b - p.a) * uv.x
					pp += (p.d - p.a) * uv.y
					pp += (p.a - p.b + p.c - p.d) * uv.x * uv.y
					lines[i] = Line(from: o, to: pp, colour: XnaColour.Aquamarine)
					i += 1
					colour = XnaColour.BurlyWood
					o = pp
					n = vn.0.lerp(vn.1, uv.x).lerp(
						vn.3.lerp(vn.2, uv.x), winding == .cw ? uv.y : 1.0 - uv.y).normalised
					//let p = verts.0.lerp(verts.1, uv.x).lerp(
					//		verts.3.lerp(verts.2, uv.x), winding == .cw ? uv.y : 1.0 - uv.y)
				}

				lines[i + 0] = Line(from: quad.a, to: quad.b, colour: colour)
				lines[i + 1] = Line(from: quad.b, to: quad.c, colour: colour)
				lines[i + 2] = Line(from: quad.c, to: quad.d, colour: colour)
				lines[i + 3] = Line(from: quad.d, to: quad.a, colour: colour)
				i += 4
			}
			lines[i + 0] = Line(from: o, to: o + n * 0.2, colour: XnaColour.Magenta)
			i += 1
			
		}
		render.drawGizmos(lines: lines)
	}
}

extension Collision.Triangle
{
	init(_ a: Vec3f, _ b: Vec3f, _ c: Vec3f)
	{
		self.init(a: a, b: b, c: c)
	}

	fileprivate var normals: (Vec3f, Vec3f, Vec3f)
	{(
		(b - a).cross(c - a).normalised,
		(c - b).cross(a - b).normalised,
		(a - c).cross(b - c).normalised)
	}

	fileprivate var normal: Vec3f
	{
		let n = normals
		return (n.0 + n.1 + n.2).normalised
	}

	func inside(point: Vec3f) -> Bool
	{
		let p = (b - a, c - a, point - a)
		let invDenom = 1.0 / (p.0.x * p.1.z - p.1.x * p.0.z);
		let v = invDenom * (p.2.x * p.1.z - p.1.x * p.2.z);
		let w = invDenom * (p.0.x * p.2.z - p.2.x * p.0.z);
		return v >= 0.0 && w >= 0.0 && v + w <= 1.0
		//let x = 1.0 - v - w
		//return v >= 0.0 && w >= 0.0 && 0 <= x && x <= 1.0
	}
}

extension Collision.Quad
{
	init(_ a: Vec3f, _ b: Vec3f, _ c: Vec3f, _ d: Vec3f)
	{
		self.init(a: a, b: b, c: c, d: d)
	}

	var normals: (Vec3f, Vec3f, Vec3f, Vec3f)
	{(
		(b - a).cross(d - a).normalised,
		(c - b).cross(a - b).normalised,
		(d - c).cross(b - c).normalised,
		(a - d).cross(c - d).normalised)
	}

	fileprivate var winding: Collision.Winding
	{
		var area: Float = 0.0
		area += (b.x - a.x) * ((a.z + b.z) * 0.5)
		area += (c.x - b.x) * ((b.z + c.z) * 0.5)
		area += (d.x - c.x) * ((c.z + d.z) * 0.5)
		area += (a.x - d.x) * ((d.z + a.z) * 0.5)
		return area.sign == .plus ? .ccw : .cw  // z is towards us
	}

	var reverse: Self
	{
		Self(a: d, b: c, c: b, d: a)
	}

	fileprivate var isFlat: Bool
	{
		let e = Collision.epsilon
		let n = normals
		return
			abs(n.0.x - n.1.x) <= e && abs(n.0.y - n.1.y) <= e && abs(n.0.z - n.1.z) <= e &&
			abs(n.0.x - n.2.x) <= e && abs(n.0.y - n.2.y) <= e && abs(n.0.z - n.2.z) <= e &&
			abs(n.0.x - n.3.x) <= e && abs(n.0.y - n.3.y) <= e && abs(n.0.z - n.3.z) <= e
	}

	fileprivate var isSimple: Bool
	{
		let e = Collision.epsilon
		if abs(a.y - b.y) <= e { return abs(c.y - d.y) <= e }
		if abs(b.y - c.y) <= e { return abs(d.y - a.y) <= e }
		return false
	}

	fileprivate var isRectangle: Bool
	{
		let e: Float = Collision.epsilon
		if	abs(a.x - b.x) <= e &&
			abs(b.z - c.z) <= e &&
			abs(c.x - d.x) <= e &&
			abs(d.z - a.z) <= e { return true }
		if	abs(a.z - b.z) <= e &&
			abs(b.x - c.x) <= e &&
			abs(c.z - d.z) <= e &&
			abs(d.x - a.x) <= e { return true }
		return false
	}

	fileprivate var isRhombus: Bool
	{
		let e = Float.ulpOfOne
		if abs(a.x - c.x) <= e && abs(b.z - d.z) <= e
		{
			// Exclude kites
			return abs(abs(a.z - b.z) - abs(c.z - d.z)) <= e
		}
		if abs(b.x - d.x) <= e && abs(a.z - c.z) <= e
		{
			// Exclude kites
			return abs(abs(b.x - a.x) - abs(d.x - c.x)) <= e
		}
		return false
	}

	func inside(point p: Vec3f) -> Bool
	{
		return
			Collision.Triangle(a, b, c).inside(point: p) ||
			Collision.Triangle(c, d, a).inside(point: p)

		/*
		let ab = (p.z * v.0.z) * (v.1.x - v.0.x) - (p.x - v.0.x) * (v.1.z - v.0.z)
		let bc = (p.z * v.1.z) * (v.2.x - v.1.x) - (p.x - v.1.x) * (v.2.z - v.1.z)
		let ca = (p.z * v.2.z) * (v.0.x - v.2.x) - (p.x - v.2.x) * (v.0.z - v.2.z)
		return ab * bc > 0 && bc * ca > 0
		*/
		/*
		let p = Self.getQuadWinding(p) == .ccw
			? (p.3, p.2, p.1, p.0)
			: (p.0, p.1, p.2, p.3)
		let uv = Self.quadSpaceFromCartesian(quad: p, position: position)
		return uv.x >= 0.0 && uv.x <= 1.0 && uv.y >= 0.0 && uv.y <= 1.0
		*/
	}

	func project(point: Vec3f) -> Vec2f
	{
		if isRectangle
		{
			let ap = point - a, ab = b - a, ad = b - d
			return Vec2f(ap.x / ab.x, -ap.z / ad.z)
		}
		else if isRhombus
		{
			let pma = Vec2f(point.x - a.x, point.z - a.z)
			let idb = 0.5 / Vec2f(d.x - a.x, b.z - a.z)
			return Vec2f(
				pma.x * idb.x + pma.y * idb.y,
				pma.x * idb.x - pma.y * idb.y)
		}

		let p = (
			Vec2d(Double(a.x), Double(a.z)),
			Vec2d(Double(b.x), Double(b.z)),
			Vec2d(Double(c.x), Double(c.z)),
			Vec2d(Double(d.x), Double(d.z)))

		let xz = Vec2d(Double(point.x), Double(point.z))

		let a = xz - p.0
		let b = p.1 - p.0
		let c = p.3 - p.0
		let d = p.0 - p.1 + p.2 - p.3
		let v0 = a.cross(b), v2 = c.cross(-d)
		let v1 = a.x * d.y - b.y * c.x - (a.y * d.x - b.x * c.y)
		let vq = (-v1 + sqrt(v1 * v1 - 4.0 * v2 * v0)) / (2.0 * v2)
		let uq = (a.x - c.x * vq) / (b.x + d.x * vq)
		return Vec2f(Float(uq), Float(vq))
	}
}