Fix floor collisions
This commit is contained in:
@ -8,155 +8,17 @@ class Collision
|
||||
|
||||
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)
|
||||
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]()
|
||||
|
||||
/*
|
||||
0,-2 1,-2
|
||||
*--*
|
||||
| |
|
||||
0,-1 * * 1,-1
|
||||
| |
|
||||
*--*
|
||||
0, 0 1, 0
|
||||
*/
|
||||
struct Triangle { let a: Vec3f, b: Vec3f, c: Vec3f }
|
||||
struct Quad { let a: Vec3f, b: Vec3f, c: Vec3f, d: Vec3f }
|
||||
|
||||
|
||||
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
|
||||
}
|
||||
fileprivate static let epsilon: Float = 0.00001 //.ulpOfOne
|
||||
|
||||
private static func isRectangle(_ positions: [Vec3f]) -> Bool
|
||||
{
|
||||
@ -202,60 +64,6 @@ class Collision
|
||||
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)
|
||||
@ -265,33 +73,34 @@ class Collision
|
||||
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
|
||||
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(n: n, p: (p.0 + p.1 + p.2) / 3.0, v: p))
|
||||
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 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)
|
||||
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.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))
|
||||
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.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)))
|
||||
*/
|
||||
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] }
|
||||
@ -303,9 +112,9 @@ class Collision
|
||||
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))
|
||||
edge3d.append(.aabbFloor(normal: n, origin: position,
|
||||
width: (right - left) / 2.0,
|
||||
depth: (forward - back) / 2.0))
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -313,7 +122,7 @@ class Collision
|
||||
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)))
|
||||
edge3d.append(.triangle(normal: n, origin: (p0 + p1 + p2) / 3.0, tri: Triangle(p0, p1, p2)))
|
||||
}
|
||||
}
|
||||
default:
|
||||
@ -333,13 +142,13 @@ class Collision
|
||||
var o: Vec3f = .zero, n: Vec3f = .zero
|
||||
switch edge
|
||||
{
|
||||
case .triangle(let trin, let trip, let triv):
|
||||
o = trip
|
||||
n = trin
|
||||
case .triangle(let norm, let pos, let tri):
|
||||
o = pos
|
||||
n = norm
|
||||
|
||||
let v0 = triv.1 - triv.0;
|
||||
let v1 = triv.2 - triv.0;
|
||||
let v2 = position - triv.0;
|
||||
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;
|
||||
@ -360,9 +169,9 @@ class Collision
|
||||
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)
|
||||
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
|
||||
@ -384,7 +193,16 @@ class Collision
|
||||
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):
|
||||
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,
|
||||
@ -406,92 +224,214 @@ class Collision
|
||||
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)
|
||||
*/
|
||||
o = quad.a.lerp(quad.d, 0.5).lerp(
|
||||
quad.c.lerp(quad.b, 0.5), 0.5)
|
||||
|
||||
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)
|
||||
)
|
||||
let vn = quad.normals
|
||||
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
|
||||
if quad.inside(point: position)
|
||||
{
|
||||
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
|
||||
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.3, uv.x).lerp(
|
||||
vn.2.lerp(vn.1, uv.x), winding == .cw ? uv.y : 1.0 - uv.y).normalised
|
||||
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: 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)
|
||||
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
|
||||
}
|
||||
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)
|
||||
}
|
||||
}
|
||||
|
||||
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))
|
||||
}
|
||||
}
|
||||
|
||||
@ -120,40 +120,33 @@ class Colin: Actor
|
||||
|
||||
response3D(plainPos: origin, plainNorm: normal)
|
||||
|
||||
case .triangle(let normal, let origin, let verts):
|
||||
let p = (verts.1 - verts.0, verts.2 - verts.0, _pos - verts.0)
|
||||
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);
|
||||
let x = 1.0 - v - w
|
||||
//guard v >= 0.0 && w >= 0.0 && v + w <= 1.0 else { break }
|
||||
guard v >= 0.0 && w >= 0.0 && 0 <= x && x <= 1.0 else { break }
|
||||
response3D(plainPos: origin, plainNorm: normal)
|
||||
|
||||
case .quad(let verts, let winding):
|
||||
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)
|
||||
)
|
||||
|
||||
let uv = Collision.quadSpaceFromCartesian(quad:
|
||||
winding == .ccw ? (verts.3, verts.2, verts.1, verts.0) : (verts.0, verts.1, verts.2, verts.3)
|
||||
, position: _pos)
|
||||
if uv.x >= 0.0 && uv.x <= 1.0 && uv.y >= 0.0 && uv.y <= 1.0
|
||||
case .triangle(let normal, let origin, let triangle):
|
||||
if triangle.inside(point: _pos)
|
||||
{
|
||||
response3D(plainPos: origin, plainNorm: normal)
|
||||
}
|
||||
|
||||
case .flatQuad(let quad, let normal):
|
||||
if quad.inside(point: _pos)
|
||||
{
|
||||
response3D(plainPos: quad.a, plainNorm: normal)
|
||||
}
|
||||
|
||||
case .quad(let quad, let winding):
|
||||
if !quad.inside(point: _pos) { continue }
|
||||
let d = winding == .ccw ? quad.reverse : quad
|
||||
let uv = d.project(point: _pos)
|
||||
//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
|
||||
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)
|
||||
let 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
|
||||
let p = quad.a.lerp(quad.b, uv.x).lerp(
|
||||
quad.d.lerp(quad.c, uv.x), winding == .cw ? uv.y : 1.0 - uv.y)
|
||||
let vn = quad.normals
|
||||
let 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
|
||||
|
||||
response3D(plainPos: Vec3f(_pos.x, p.y, _pos.z), plainNorm: n)
|
||||
}
|
||||
/*
|
||||
let p = (
|
||||
verts.0 + (verts.1 - verts.0) * _pos.x,
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Reference in New Issue
Block a user