a_dinosaur/CavesOfSwift
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

init dump

Browse Source
This commit is contained in:
a dinosaur
2024-05-05 17:01:56 +10:00
commit 608cf45822
53 changed files with 19224 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

497
Sources/Test/Collision.swift Normal file
View File

@ -0,0 +1,497 @@
import Foundation
import JolkEngine
class Collision
{
struct Edge { let p: Vec2f, n: Vec2f, w: Float }
enum Edge3D
{
case triangle(n: Vec3f, p: Vec3f, v: (Vec3f, Vec3f, Vec3f))
case aabbFloor(n: Vec3f, p: Vec3f, w: Float, d: Float)
case quad(p: (Vec3f, Vec3f, Vec3f, Vec3f), w: Winding)
}
var edge3d = [Edge3D]()
/*
0,-2 1,-2
*--*
| |
0,-1 * * 1,-1
| |
*--*
0, 0 1, 0
*/
init()
{
assert(Self.isRectangle([
Vec3f(0.0, 0.0, 0.0),
Vec3f(0.0, 0.0, -1.0),
Vec3f(1.0, 0.0, -1.0),
Vec3f(1.0, 0.0, 0.0),
]))
// CW
assert(Self.isRectangle([
Vec3f(0.0, 0.0, 0.0),
Vec3f(0.0, 0.0, -1.0),
Vec3f(0.0, 0.0, -2.0),
Vec3f(1.0, 0.0, -2.0),
Vec3f(1.0, 0.0, -1.0),
Vec3f(1.0, 0.0, 0.0),
]))
assert(Self.isRectangle([
Vec3f(0.0, 0.0, -1.0),
Vec3f(0.0, 0.0, -2.0),
Vec3f(1.0, 0.0, -2.0),
Vec3f(1.0, 0.0, -1.0),
Vec3f(1.0, 0.0, 0.0),
Vec3f(0.0, 0.0, 0.0),
]))
assert(Self.isRectangle([
Vec3f(0.0, 0.0, -2.0),
Vec3f(1.0, 0.0, -2.0),
Vec3f(1.0, 0.0, -1.0),
Vec3f(1.0, 0.0, 0.0),
Vec3f(0.0, 0.0, 0.0),
Vec3f(0.0, 0.0, -1.0),
]))
assert(Self.isRectangle([
Vec3f(1.0, 0.0, -2.0),
Vec3f(1.0, 0.0, -1.0),
Vec3f(1.0, 0.0, 0.0),
Vec3f(0.0, 0.0, 0.0),
Vec3f(0.0, 0.0, -1.0),
Vec3f(0.0, 0.0, -2.0),
]))
assert(Self.isRectangle([
Vec3f(1.0, 0.0, -1.0),
Vec3f(1.0, 0.0, 0.0),
Vec3f(0.0, 0.0, 0.0),
Vec3f(0.0, 0.0, -1.0),
Vec3f(0.0, 0.0, -2.0),
Vec3f(1.0, 0.0, -2.0),
]))
assert(Self.isRectangle([
Vec3f(1.0, 0.0, 0.0),
Vec3f(0.0, 0.0, 0.0),
Vec3f(0.0, 0.0, -1.0),
Vec3f(0.0, 0.0, -2.0),
Vec3f(1.0, 0.0, -2.0),
Vec3f(1.0, 0.0, -1.0),
]))
// CCW
assert(Self.isRectangle([
Vec3f( 0.0, 0.0, 0.0),
Vec3f( 0.0, 0.0, -1.0),
Vec3f( 0.0, 0.0, -2.0),
Vec3f(-1.0, 0.0, -2.0),
Vec3f(-1.0, 0.0, -1.0),
Vec3f(-1.0, 0.0, 0.0),
]))
assert(Self.isRectangle([
Vec3f( 0.0, 0.0, -1.0),
Vec3f( 0.0, 0.0, -2.0),
Vec3f(-1.0, 0.0, -2.0),
Vec3f(-1.0, 0.0, -1.0),
Vec3f(-1.0, 0.0, 0.0),
Vec3f( 0.0, 0.0, 0.0),
]))
assert(Self.isRectangle([
Vec3f( 0.0, 0.0, -2.0),
Vec3f(-1.0, 0.0, -2.0),
Vec3f(-1.0, 0.0, -1.0),
Vec3f(-1.0, 0.0, 0.0),
Vec3f( 0.0, 0.0, 0.0),
Vec3f( 0.0, 0.0, -1.0),
]))
assert(Self.isRectangle([
Vec3f(-1.0, 0.0, -2.0),
Vec3f(-1.0, 0.0, -1.0),
Vec3f(-1.0, 0.0, 0.0),
Vec3f( 0.0, 0.0, 0.0),
Vec3f( 0.0, 0.0, -1.0),
Vec3f( 0.0, 0.0, -2.0),
]))
assert(Self.isRectangle([
Vec3f(-1.0, 0.0, -1.0),
Vec3f(-1.0, 0.0, 0.0),
Vec3f( 0.0, 0.0, 0.0),
Vec3f( 0.0, 0.0, -1.0),
Vec3f( 0.0, 0.0, -2.0),
Vec3f(-1.0, 0.0, -2.0),
]))
assert(Self.isRectangle([
Vec3f(-1.0, 0.0, 0.0),
Vec3f( 0.0, 0.0, 0.0),
Vec3f( 0.0, 0.0, -1.0),
Vec3f( 0.0, 0.0, -2.0),
Vec3f(-1.0, 0.0, -2.0),
Vec3f(-1.0, 0.0, -1.0),
]))
}
private static let epsilon: Float = 0.00001 //.ulpOfOne
private static func isSimpleQuad(_ p: (Vec3f, Vec3f, Vec3f, Vec3f)) -> Bool
{
if abs(p.0.y - p.1.y) <= epsilon { return abs(p.2.y - p.3.y) <= epsilon }
if abs(p.1.y - p.2.y) <= epsilon { return abs(p.3.y - p.0.y) <= epsilon }
return false
}
private static func isRectangle(_ p: (Vec3f, Vec3f, Vec3f, Vec3f)) -> Bool
{
if abs(p.0.x - p.1.x) <= epsilon &&
abs(p.1.z - p.2.z) <= epsilon &&
abs(p.2.x - p.3.x) <= epsilon &&
abs(p.3.z - p.0.z) <= epsilon { return true }
if abs(p.0.z - p.1.z) <= epsilon &&
abs(p.1.x - p.2.x) <= epsilon &&
abs(p.2.z - p.3.z) <= epsilon &&
abs(p.3.x - p.0.x) <= epsilon { return true }
return false
}
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
}
private static func getQuadWinding(_ p: (Vec3f, Vec3f, Vec3f, Vec3f)) -> Winding
{
var area: Float = 0.0
area += (p.1.x - p.0.x) * ((p.0.z + p.1.z) * 0.5)
area += (p.2.x - p.1.x) * ((p.1.z + p.2.z) * 0.5)
area += (p.3.x - p.2.x) * ((p.2.z + p.3.z) * 0.5)
area += (p.0.x - p.3.x) * ((p.3.z + p.0.z) * 0.5)
return area.sign == .plus ? .ccw : .cw // z is towards us
}
static func quadSpaceFromCartesian(quad: (Vec3f, Vec3f, Vec3f, Vec3f), position: Vec3f) -> Vec2f
{
let p = (
Vec2d(Double(quad.0.x), Double(quad.0.z)),
Vec2d(Double(quad.1.x), Double(quad.1.z)),
Vec2d(Double(quad.2.x), Double(quad.2.z)),
Vec2d(Double(quad.3.x), Double(quad.3.z)))
let xz = Vec2d(Double(position.x), Double(position.z))
/*
let old = {
let a = xz.x - p.0.x
let b = p.1.x - p.0.x
let c = p.3.x - p.0.x
let d = p.0.x - p.1.x + p.2.x - p.3.x
let f = xz.y - p.0.y
let g = p.1.y - p.0.y
let h = p.3.y - p.0.y
let j = p.0.y - p.1.y + p.2.y - p.3.y
let v2 = -c * j - (-d * h)
let v1 = a * j - c * g - (d * f - b * h)
let v0 = a * g - b * f
let vq = (-v1 + sqrt(v1 * v1 - 4.0 * v2 * v0)) / (2.0 * v2)
let uq = (a - c * vq) / (b + d * vq)
return Vec2f(Float(uq), Float(vq))
}
*/
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))
//let oldUv = old()
//if !oldUv.x.isNaN || !uv.x.isNaN { assert (oldUv.x == uv.x) }
//if !oldUv.y.isNaN || !uv.y.isNaN { assert (oldUv.y == uv.y) }
//return uv
}
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 p = (obj.positions[v1.p], obj.positions[v2.p], obj.positions[v3.p])
let n = (obj.normals[v1.n] + obj.normals[v2.n] + obj.normals[v3.n]).normalised
if abs(n.y) < 0.25 { continue }
edge3d.append(.triangle(n: n, p: (p.0 + p.1 + p.2) / 3.0, v: p))
case .quad(let v1, let v2, let v3, let v4):
let p = (obj.positions[v1.p], obj.positions[v2.p], obj.positions[v3.p], obj.positions[v4.p])
let n = (obj.normals[v1.n] + obj.normals[v2.n] + obj.normals[v3.n] + obj.normals[v4.n]).normalised
if Self.isSimpleQuad(p) && Self.isRectangle(p)
{
if abs(n.y) < 0.25 { continue }
let left = min(p.0.x, p.1.x, p.2.x, p.3.x)
let right = max(p.0.x, p.1.x, p.2.x, p.3.x)
let back = min(p.0.z, p.1.z, p.2.z, p.3.z)
let forward = max(p.0.z, p.1.z, p.2.z, p.3.z)
edge3d.append(.aabbFloor(n: n,
p: (p.0 + p.1 + p.2 + p.3) / 4.0,
w: (right - left) / 2.0,
d: (forward - back) / 2.0))
}
else
{
if abs(n.y) < 0.25 { continue }
edge3d.append(.quad(p: p, w: Self.getQuadWinding(p)))
/*
edge3d.append(.triangle(n: n, p: (p.0 + p.1 + p.2) / 3.0, v: (p.0, p.1, p.2)))
edge3d.append(.triangle(n: n, p: (p.2 + p.3 + p.0) / 3.0, v: (p.2, p.3, p.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(n: n, p: position,
w: (right - left) / 2.0,
d: (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(n: n, p: (p0 + p1 + p2) / 3.0, v: (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 trin, let trip, let triv):
o = trip
n = trin
let v0 = triv.1 - triv.0;
let v1 = triv.2 - triv.0;
let v2 = position - triv.0;
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: triv.0, to: triv.1, colour: colour)
lines[i + 1] = Line(from: triv.1, to: triv.2, colour: colour)
lines[i + 2] = Line(from: triv.2, to: triv.0, 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 .quad(let verts, 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
*/
//n = winding == .ccw ? .up : .down
o = verts.0.lerp(verts.3, 0.5).lerp(
verts.2.lerp(verts.1, 0.5), 0.5)
/*
let xy = Vec2d(Double(position.x), Double(position.z))
let a = xy.x - p.0.x
let b = p.1.x - p.0.x
let c = p.3.x - p.0.x
let d = p.0.x - p.1.x + p.2.x - p.3.x
let f = xy.y - p.0.y
let g = p.1.y - p.0.y
let h = p.3.y - p.0.y
let j = p.0.y - p.1.y + p.2.y - p.3.y
let v2 = -c * j - (-d * h)
let v1 = a * j - c * g - (d * f - b * h)
let v0 = a * g - b * f
let vq = (-v1 + sqrt(v1 * v1 - 4.0 * v2 * v0)) / (2.0 * v2)
let uq = (a - c * vq) / (b + d * vq)
let uv = Vec2d(uq, vq)
*/
var colour = XnaColour.GreenYellow
/*
p1 p2
*------*
| |
| |
*------*
p0 p3
*/
let p = winding == .ccw ? (verts.3, verts.2, verts.1, verts.0) : (verts.0, verts.1, verts.2, verts.3)
let vn = (
(verts.1 - verts.0).cross(verts.3 - verts.0),
(verts.2 - verts.1).cross(verts.0 - verts.1),
(verts.3 - verts.2).cross(verts.1 - verts.2),
(verts.0 - verts.3).cross(verts.2 - verts.3)
)
n = (vn.0 + vn.1 + vn.2 + vn.3).normalised
let uv = Self.quadSpaceFromCartesian(quad: p, position: position)
if uv.x >= 0.0 && uv.x <= 1.0 && uv.y >= 0.0 && uv.y <= 1.0
//if uv.x >= -1.0 && uv.x <= 1.0 && uv.y >= -1.0 && uv.y <= 1.0
{
var pp = p.0
pp += (p.1 - p.0) * uv.x
pp += (p.3 - p.0) * uv.y
pp += (p.0 - p.1 + p.2 - p.3) * 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.3, uv.x).lerp(
vn.2.lerp(vn.1, uv.x), winding == .cw ? uv.y : 1.0 - uv.y).normalised
}
lines[i + 0] = Line(from: verts.0, to: verts.1, colour: colour)
lines[i + 1] = Line(from: verts.1, to: verts.2, colour: colour)
lines[i + 2] = Line(from: verts.2, to: verts.3, colour: colour)
lines[i + 3] = Line(from: verts.3, to: verts.0, colour: colour)
i += 4
}
let p = position
let a = p.dot(n) - o.dot(n)
let pissy = p - n * a
//let d = colin.position - edge.p
//let pissy = edge.p + d.cross(edge.n)
/*
if a > -1.0 && a <= 0.0
{
lines[i + 0] = Line(from: o, to: o + n * max(0, a), colour: XnaColour.Magenta)
lines[i + 1] = Line(from: pissy + .up, to: o, colour: XnaColour.Red)
i += 2
}
*/
lines[i + 0] = Line(from: o, to: o + n * 0.2, colour: XnaColour.Magenta)
i += 1
//lines[i + 4] = Line(from: pissy + Vec3f(-1, 0, 0) * 0.1, to: pissy + Vec3f(1, 0, 0) * 0.1, colour: XnaColour.Red)
//lines[i + 5] = Line(from: pissy + Vec3f( 0,-1, 0) * 0.1, to: pissy + Vec3f(0, 1, 0) * 0.1, colour: XnaColour.Red)
//lines[i + 6] = Line(from: pissy + Vec3f( 0, 0,-1) * 0.1, to: pissy + Vec3f(0, 0, 1) * 0.1, colour: XnaColour.Red)
}
render.drawGizmos(lines: lines)
}
}