diff options
Diffstat (limited to 'collision.odin')
| -rw-r--r-- | collision.odin | 472 |
1 files changed, 472 insertions, 0 deletions
diff --git a/collision.odin b/collision.odin new file mode 100644 index 0000000..1583f5e --- /dev/null +++ b/collision.odin @@ -0,0 +1,472 @@ +package physics + +import "core:fmt" +import "core:math" +import "core:math/linalg" + +Ray :: struct { + pos, nrm: [3]f32, + len: f32, +} + +Line :: struct { + start, end: [3]f32, +} + +Plane :: struct { + pos, nrm, sz: [3]f32, +} + +AABB :: struct { + min, max: [3]f32, +} + +Box :: struct { + sz: [3]f32, +} + +Sphere :: struct { + radius: f32, +} + +Capsule :: struct { + base: [3]f32, + radius, height: f32, +} + +Cylinder :: struct { + base: [3]f32, + radius, height: f32, +} + +ConvexHull :: struct { + verts: []f32, +} + +TriMesh :: struct { + verts: []f32, +} + +Frustrum :: struct { + l, r, t, b, n, f: [4]f32, +} + +Collider :: union { + Sphere, + Box, + // Capsule, + // Cylinder, + // ConvexHull, + // TriMesh, +} + +Contact :: struct { + a, b: ^RigidBody, + point, normal: [3]f32, // normal points from a to b + r1_local, r2_local: [3]f32, + depth, weight, normal_lambda: f32, +} + +collider_inv_inertia :: proc(col: Collider, mass: f32) -> matrix[3,3]f32 { + switch shape in col { + case Sphere: + inertia := 0.4 * mass * shape.radius * shape.radius + return matrix[3,3]f32 { + 1/inertia, 0, 0, + 0, 1/inertia, 0, + 0, 0, 1/inertia, + } + + case Box: + sq := shape.sz * shape.sz // full side lengths, squared + ix := mass * (sq.y + sq.z) / 12 + iy := mass * (sq.x + sq.z) / 12 + iz := mass * (sq.x + sq.y) / 12 + return matrix[3,3]f32 { + 1/ix, 0, 0, + 0, 1/iy, 0, + 0, 0, 1/iz, + } + + // case Cylinder: + // r := shape.radius + // h := shape.height + // iy := 0.5 * mass * r * r + // ix := mass * (3*r*r + h*h) / 12 + // return matrix[3,3]f32 { + // 1/ix, 0, 0, + // 0, 1/iy, 0, + // 0, 0, 1/ix, + // } + // + // case Capsule: + // r := shape.radius + // h := shape.height + // // split total mass by volume share + // cyl_vol := math.PI * r * r * h + // cap_vol := (4.0/3.0) * math.PI * r * r * r // both caps together + // cyl_mass := mass * cyl_vol / (cyl_vol + cap_vol) + // cap_mass := mass - cyl_mass + // + // iy := 0.5*cyl_mass*r*r + 0.4*cap_mass*r*r + // ix := cyl_mass*(3*r*r + h*h)/12 + + // cap_mass*(0.4*r*r + 0.25*h*h + 0.375*h*r) + // return matrix[3,3]f32 { + // 1/ix, 0, 0, + // 0, 1/iy, 0, + // 0, 0, 1/ix, + // } + + case: + panic("collider_inv_inertia: collider shape can't be nil") + } +} + +contact_solve :: proc(contact: ^Contact, dt: f32) { + r1 := contact.point - contact.a.pos + r2 := contact.point - contact.b.pos + correction := -contact.normal * contact.depth // * contact.weight + contact.normal_lambda = + solve_positional(contact.a, contact.b, r1, r2, + correction, 0, dt) +} + +collide :: proc(a, b: ^RigidBody, contacts: ^[dynamic]Contact) { + switch _ in a.col { + case Sphere: + switch _ in b.col { + case Sphere: + c: Contact + if sphere_v_sphere(a, b, &c) do append(contacts, c) + case Box: + c: Contact + if sphere_v_box(a, b, &c) do append(contacts, c) + } + case Box: + switch _ in b.col { + case Sphere: + // reverse args so sphere is `a`, then flip the normal + c: Contact + if sphere_v_box(b, a, &c) { + c.a, c.b = a, b + c.normal = -c.normal + append(contacts, c) + } + case Box: + box_v_box(a, b, contacts) + } + } +} + +// marked for deletion +sphere_v_ground :: proc(ground, body: ^RigidBody, out: ^Contact) -> bool { + radius := body.col.(Sphere).radius // TODO: fix this + depth := radius - body.pos.y + + if depth <= 0 { + return false + } + + out^ = Contact { + a = ground, + b = body, + normal = {0,1,0}, + depth = depth, + point = body.pos - [3]f32{0,radius,0}, + weight = 1, + } + + return true +} + +sphere_v_sphere :: proc(a, b: ^RigidBody, out: ^Contact) -> bool { + radius_a := a.col.(Sphere).radius + radius_b := b.col.(Sphere).radius + + delta := b.pos - a.pos + dist := linalg.length(delta) + + // guard: coincident centers have no defined normal + if dist == 0 { + return false + } + + overlap := radius_a + radius_b - dist + if overlap <= 0 { + return false // not touching + } + + normal := delta / dist + out.a = a + out.b = b + out.normal = normal + out.depth = overlap + out.point = a.pos + normal * radius_a // on a's surface, toward b + out.weight = 1 + out.r1_local = + linalg.quaternion_mul_vector3( + linalg.quaternion_inverse(a.rot), out.point - a.pos) + out.r2_local = + linalg.quaternion_mul_vector3( + linalg.quaternion_inverse(b.rot), out.point - b.pos) + + return true +} + +sphere_v_box :: proc(a, b: ^RigidBody, out: ^Contact) -> bool { + radius := a.col.(Sphere).radius + cp := box_closest_point(b, a.pos) + + delta := cp - a.pos // from sphere center toward box surface + dist := linalg.length(delta) + + if dist >= radius { + return false // closest point is outside the sphere + } + + normal: [3]f32 + if dist > 0 { + normal = delta / dist + } else { + // TODO: come back and figure this out at some point + // sphere center is *inside* the box — degenerate, pick any axis. + // (proper handling: push out along the shallowest face. punt for now.) + normal = {0, 1, 0} + } + + out.a = a + out.b = b + out.normal = normal + out.depth = radius - dist + out.point = cp + out.weight = 1 + out.r1_local = + linalg.quaternion_mul_vector3( + linalg.quaternion_inverse(a.rot), cp - a.pos) + out.r2_local = + linalg.quaternion_mul_vector3( + linalg.quaternion_inverse(b.rot), cp - b.pos) + + return true +} + +// Box helpers +// boxes use SAT + sutherland-hodgman clipping +box_v_box :: proc(a, b: ^RigidBody, contacts: ^[dynamic]Contact) -> int { + axes_a := box_axes(a) + axes_b := box_axes(b) + center_delta := b.pos - a.pos + + // track the minimum-penetration axis as we go + best_sep := f32(-1e30) // most-negative separation = deepest; we want the LEAST negative + best_axis: [3]f32 + + test_axis :: proc(L: [3]f32, a, b: ^RigidBody, + axes_a, axes_b: [3][3]f32, center_delta: [3]f32, + best_sep: ^f32, best_axis: ^[3]f32) -> bool { + len_sq := linalg.dot(L, L) + if len_sq < 1e-8 { + return true // degenerate axis (parallel edges); skip, treat as non-separating + } + + axis := L / math.sqrt(len_sq) + ra := box_radius_on_axis(a, axes_a, axis) + rb := box_radius_on_axis(b, axes_b, axis) + dist := abs(linalg.dot(center_delta, axis)) + sep := dist - (ra + rb) + + if sep > 0 { + return false // separating axis found, no overlap + } + + if sep > best_sep^ { + best_sep^ = sep + if linalg.dot(center_delta, axis) < 0 { + axis = -axis + } + best_axis^ = axis + } + return true + } + + // 6 face axes + for i in 0..<3 { + if !test_axis(axes_a[i], a, b, axes_a, axes_b, center_delta, + &best_sep, &best_axis) { + return 0 + } + } + for i in 0..<3 { + if !test_axis(axes_b[i], a, b, axes_a, axes_b, center_delta, + &best_sep, &best_axis) { + return 0 + } + } + + // 9 edge-edge cross product axes + for i in 0..<3 { + for j in 0..<3 { + L := linalg.cross(axes_a[i], axes_b[j]) + if !test_axis(L, a, b, axes_a, axes_b, center_delta, + &best_sep, &best_axis) { + return 0 + } + }} + + ref, inc, ref_is_a := box_contact_faces(a, b, best_axis) + ref_normal := ref_is_a ? best_axis : -best_axis + clipped := clip_incident(inc, ref, ref_normal) + + if !a.static && !b.static { + fmt.println(">>> DYN-DYN manifold, clipped.n =", clipped.n) + fmt.println(" ref_corners:", ref) + for i in 0..<clipped.n { + fmt.println(" pt", i, clipped.pts[i]) + } + } + + CONTACT_SLOP :: f32(0.001) // 1mm tolerance + MAX_CORRECTION :: f32(0.2) + + count := 0 + for i in 0..<clipped.n { + point := clipped.pts[i] + dist := linalg.dot(point - ref[0], ref_normal) + if dist > CONTACT_SLOP { + continue + } + append(contacts, Contact { + a = a, + b = b, + normal = best_axis, + depth = min(max(-dist, 0), MAX_CORRECTION), + point = point, + r1_local = + linalg.quaternion_mul_vector3( + linalg.quaternion_inverse(a.rot), + point - a.pos), + r2_local = + linalg.quaternion_mul_vector3( + linalg.quaternion_inverse(b.rot), + point - b.pos), + weight = 1, + }) + + if !a.static && !b.static { + // fmt.println("contact point", count, point) + } + count += 1 + + if -dist > 0.05 { // only print suspicious depths + fmt.printfln("BIG DEPTH depth=%v point=%v b.pos=%v lever=%v", + max(-dist,0), point, b.pos, point - b.pos) + } + } + + + if !a.static && !b.static && count > 0 { + // fmt.println("contacts:", count) + // fmt.println("") + } + + return count +} + +box_axes :: proc(body: ^RigidBody) -> [3][3]f32 { + rot := linalg.matrix3_from_quaternion(body.rot) + return { + {rot[0,0], rot[1,0], rot[2,0]}, // local x in world + {rot[0,1], rot[1,1], rot[2,1]}, // local y in world + {rot[0,2], rot[1,2], rot[2,2]}, // local z in world + } +} + +// half-width of the box's shadow when projected onto unit axis L +box_radius_on_axis :: proc(body: ^RigidBody, axes: [3][3]f32, + L: [3]f32) -> f32 { + half := body.col.(Box).sz / 2 + return abs(half.x * linalg.dot(axes[0], L)) + + abs(half.y * linalg.dot(axes[1], L)) + + abs(half.z * linalg.dot(axes[2], L)) +} + +// closest point on an oriented box to world-space point `p` +box_closest_point :: proc(box_body: ^RigidBody, p: [3]f32) -> [3]f32 { + box := box_body.col.(Box) + half := box.sz / 2 + + // world point -> box local space: undo translation, then undo rotation + local := p - box_body.pos + inv_rot := linalg.quaternion_inverse(box_body.rot) + local = linalg.quaternion_mul_vector3(inv_rot, local) + + // clamp per-axis to the box extents (closest point in local space) + local.x = clamp(local.x, -half.x, half.x) + local.y = clamp(local.y, -half.y, half.y) + local.z = clamp(local.z, -half.z, half.z) + + // box local space -> world: re-apply rotation, then translation + world := linalg.quaternion_mul_vector3(box_body.rot, local) + return world + box_body.pos +} + +Poly :: struct { + pts: [8][3]f32, + n: int, +} + +clip_against_plane :: proc(src: Poly, plane_pt, plane_nrm: [3]f32) -> Poly { + dst: Poly + + for i in 0..<src.n { + current := src.pts[i] + next := src.pts[(i + 1) % src.n] + + d_c := linalg.dot(current - plane_pt, plane_nrm) + d_n := linalg.dot(next - plane_pt, plane_nrm) + + if d_c >= 0 { + dst.pts[dst.n] = current + dst.n += 1 + } + + if (d_c >= 0) != (d_n >= 0) { + t := d_c / (d_c - d_n) + dst.pts[dst.n] = current + t * (next - current) + dst.n += 1 + } + } + + return dst +} + +// clip the incident polygon against all 4 side planes of the reference face. +// ref_corners in winding order; ref_normal is the reference face's outward normal. +clip_incident :: proc(inc_corners, ref_corners: [4][3]f32, + ref_normal: [3]f32) -> Poly { + poly: Poly + for c in inc_corners { + poly.pts[poly.n] = c + poly.n += 1 + } + + ref_center := (ref_corners[0] + ref_corners[1] + + ref_corners[2] + ref_corners[3]) / 4 + + fmt.println("ref_corners:", ref_corners) + fmt.println("ref_normal:", ref_normal) + + for i in 0..<4 { + edge_start := ref_corners[i] + edge_dir := ref_corners[(i + 1) % 4] - edge_start + inward := linalg.normalize(linalg.cross(ref_normal, edge_dir)) + if linalg.dot(inward, ref_center - edge_start) < 0 { + inward = -inward + } + poly = clip_against_plane(poly, edge_start, inward) + + fmt.println(" edge", i, "start:", edge_start, "inward:", inward) + } + return poly +} + |