diff options
| -rw-r--r-- | collision.odin | 472 | ||||
| -rw-r--r-- | main.odin | 478 |
2 files changed, 950 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 +} + diff --git a/main.odin b/main.odin new file mode 100644 index 0000000..993a580 --- /dev/null +++ b/main.odin @@ -0,0 +1,478 @@ +package physics + +import "core:fmt" +import "core:math" +import "core:math/rand" +import "core:math/linalg" +import rl "vendor:raylib" +import "vendor:raylib/rlgl" + +/* + XPBD algorithm + - integrate + - solve constraints + - correct positions +*/ + +quat :: quaternion128 + +RigidBody :: struct { + pos: [3]f32, + rot: quat, + + vel: [3]f32, + omega: [3]f32, // angular velocity + + inv_mass: f32, + inv_inertia: matrix[3,3]f32, + + static, active: bool, + col: Collider, + + // previous state + prev_pos: [3]f32, + prev_rot: quaternion128, +} + +// decent enough approximations for drag without doing real math. +// otherwise we would have to calculate forces and integrate them properly. +// fraction of velocity LOST per second (0.0 = none, higher = more damping) +LINEAR_DAMPING :: 0.2 +ANGULAR_DAMPING :: 0.01 +FRICTION :: 0.5 +GRAVITY :: [3]f32{0, -10, 0} +DT :: f32(1.0/60.0) +SUBSTEP_CNT :: 10 + +bodies : [dynamic]RigidBody +contacts : [dynamic]Contact + +body_create :: proc(col: Collider, pos: [3]f32, mass: f32, + rot := linalg.Quaternionf32(1), static := false) -> RigidBody { + static := static + + body := RigidBody { + pos = pos, + rot = rot, + prev_pos = pos, + prev_rot = rot, + col = col, + } + if mass <= 0 && !static { + fmt.println("warn:" + + "non static body with no mass was set to static") + static = true + } + if !static { + body.inv_mass = 1.0 / mass + body.inv_inertia = collider_inv_inertia(col, mass) + } + + body.static = static + return body +} + +inv_inertia_world :: proc(body: ^RigidBody) -> matrix[3,3]f32 { + rot_mat := linalg.matrix3_from_quaternion(body.rot) + return rot_mat * body.inv_inertia * linalg.transpose(rot_mat) +} + +generalized_inv_mass :: proc(body: ^RigidBody, r: [3]f32, n: [3]f32) -> f32 { + r_cross_n := linalg.cross(r, n) + inv_inertia_w := inv_inertia_world(body) + angular := linalg.dot(r_cross_n, inv_inertia_w * r_cross_n) + return body.inv_mass + angular +} + +apply_impulse :: proc(body: ^RigidBody, r: [3]f32, impulse: [3]f32) { + // Note the sign convention: callers pass +impulse to one body and + // -impulse to the other, so apply_impulse itself never negates + // Newton's third law is the caller's job. + + // linear: heavier bodies move less + body.pos += impulse * body.inv_mass + + // angular: torque = r × impulse + inv_inertia_w := inv_inertia_world(body) + angular := inv_inertia_w * linalg.cross(r, impulse) + angular_q := quaternion(x = angular.x, y = angular.y, + z = angular.z, w = 0) + spin := angular_q * body.rot + body.rot += quaternion(x = spin.x * 0.5, y = spin.y * 0.5, + z = spin.z * 0.5, w = spin.w * 0.5) + body.rot = linalg.normalize(body.rot) +} + +solve_positional :: proc(a, b: ^RigidBody, r1, r2, correction: [3]f32, + compliance, dt: f32) -> f32 { + c := linalg.length(correction) + + if c == 0 { + return 0 + } + + n := correction / c + w1 := generalized_inv_mass(a, r1, n) + w2 := generalized_inv_mass(b, r2, n) + w_sum := w1 + w2 + + if w_sum == 0 { + return 0 + } + + alpha := compliance / (dt * dt) + delta_lambda := c / (w_sum + alpha) + impulse := delta_lambda * n + + apply_impulse(a, r1, impulse) + apply_impulse(b, r2, -impulse) + + return delta_lambda +} + +solve_attachment :: proc(body: ^RigidBody, local_point, world_point: [3]f32, + compliance, dt: f32) { + r := linalg.quaternion_mul_vector3(body.rot, local_point) + correction := world_point - (body.pos + r) + + magnitude := linalg.length(correction) + if magnitude == 0 { + return + } + n := correction / magnitude + w := generalized_inv_mass(body, r, n) + + if w == 0 { + return + } + + alpha := compliance / (dt * dt) + delta_lambda := magnitude / (w + alpha) + apply_impulse(body, r, delta_lambda * n) +} + +friction_solve :: proc(contact: ^Contact, dt: f32) { + a := contact.a + b := contact.b + n := contact.normal + + r1 := linalg.quaternion_mul_vector3(a.rot, contact.r1_local) + r2 := linalg.quaternion_mul_vector3(b.rot, contact.r2_local) + + p1_cur := a.pos + r1 + p2_cur := b.pos + r2 + p1_prev := a.prev_pos + + linalg.quaternion_mul_vector3(a.prev_rot, contact.r1_local) + p2_prev := b.prev_pos + + linalg.quaternion_mul_vector3(b.prev_rot, contact.r2_local) + + delta := (p1_cur - p1_prev) - (p2_cur - p2_prev) + delta_t := delta - linalg.dot(delta, n) * n + slide := linalg.length(delta_t) + + if slide < 1e-8 { + return + } + tangent_dir := delta_t / slide + + correction_mag := min(slide, FRICTION * contact.normal_lambda) + correction := -correction_mag * tangent_dir + + solve_positional(a, b, r1, r2, correction, 0, dt) + + // fmt.printfln("slide=%v clamp=%v applied=%v normal_lambda=%v", + // slide, FRICTION * contact.normal_lambda, + // min(slide, FRICTION * contact.normal_lambda), + // contact.normal_lambda) +} + +integrate :: proc(body: ^RigidBody, dt: f32) { + body.prev_pos = body.pos + body.prev_rot = body.rot + + // linear motion + if body.inv_mass > 0 { + body.vel += GRAVITY * dt + } + body.pos += body.vel * dt + + // angular motion + omega_q := quaternion( + x = body.omega.x, + y = body.omega.y, + z = body.omega.z, + w = 0) + spin := omega_q * body.rot + scale := 0.5 * dt + delta := quaternion( + x = spin.x * scale, + y = spin.y * scale, + z = spin.z * scale, + w = spin.w * scale, + ) + body.rot += delta + body.rot = linalg.normalize(body.rot) +} + +recover_velocity :: proc(body: ^RigidBody, dt: f32) { + body.vel = (body.pos - body.prev_pos) / dt + + rdelta := body.rot * linalg.quaternion_inverse(body.prev_rot) + body.omega = 2 * [3]f32{rdelta.x, rdelta.y, rdelta.z} / dt + + if rdelta.w < 0 { + body.omega = -body.omega + } +} + +/* + physics steps: + - integrate + - solve collisions/contacts + - solve constraints + - recover velocity +*/ + +physics_simulate :: proc() { + dt := DT / f32(SUBSTEP_CNT) + + // raised to pow of dt to remove frame rate dependency + linear_damp_factor := math.pow(1 - LINEAR_DAMPING, dt) + angular_damp_factor := math.pow(1 - ANGULAR_DAMPING, dt) + + for substep in 0..<SUBSTEP_CNT { + for &body in bodies { + integrate(&body, dt) + } + + clear(&contacts) + for i in 0..<len(bodies) { + for j in i+1..<len(bodies) { + collide(&bodies[i], &bodies[j], &contacts) + } + } + + for &contact in contacts { + contact_solve(&contact, dt) + friction_solve(&contact, dt) + } + + for &body in bodies { + recover_velocity(&body, dt) + body.vel *= linear_damp_factor + body.omega *= angular_damp_factor + } + } +} + +// BOX DEBUG HELPERS +box_corners :: proc(body: ^RigidBody) -> [8][3]f32 { + half := body.col.(Box).sz / 2 + axes := box_axes(body) + corners: [8][3]f32 + idx := 0 + for sx in ([2]f32{-1, 1}) { + for sy in ([2]f32{-1, 1}) { + for sz in ([2]f32{-1, 1}) { + local := axes[0]*half.x*sx + axes[1]*half.y*sy + axes[2]*half.z*sz + corners[idx] = body.pos + local + idx += 1 + }}} + return corners +} + +// returns the axis index (0,1,2) and sign (+1/-1) of the face +// whose outward normal best matches `dir` +box_best_face :: proc(axes: [3][3]f32, dir: [3]f32) -> (axis: int, sign: f32) { + best_dot := f32(-1e30) + for i in 0..<3 { + d := linalg.dot(axes[i], dir) + if d > best_dot { + best_dot = d + axis = i + sign = 1 + } + if -d > best_dot { + best_dot = -d + axis = i + sign = -1 + } + } + return +} + +// the 4 world-space corners of the face identified by (axis, sign) +box_face_corners :: proc(body: ^RigidBody, axes: [3][3]f32, + axis: int, sign: f32) -> [4][3]f32 { + half := body.col.(Box).sz / 2 + // the two axes spanning the face (the ones that aren't `axis`) + u := (axis + 1) % 3 + v := (axis + 2) % 3 + + // center of the face: box center pushed out to the face along `axis` + face_center := body.pos + axes[axis] * (half[axis] * sign) + + hu := half[u] + hv := half[v] + return { + face_center + axes[u]*hu + axes[v]*hv, + face_center + axes[u]*hu - axes[v]*hv, + face_center - axes[u]*hu - axes[v]*hv, + face_center - axes[u]*hu + axes[v]*hv, + } +} + +// for a confirmed box-box contact with normal `n` (points a->b), +// returns the reference and incident face corner sets. +// also returns which body owns the reference (for later). +box_contact_faces :: proc(a, b: ^RigidBody, n: [3]f32) -> + (ref_corners, inc_corners: [4][3]f32, ref_is_a: bool) { + + axes_a := box_axes(a) + axes_b := box_axes(b) + + // how aligned is each box's best face with the normal? + // reference face = most aligned with the contact normal. + // for body a, the separating direction pushing toward b is +n. + // for body b, it's -n. we compare alignment. + a_axis, a_sign := box_best_face(axes_a, n) + b_axis, b_sign := box_best_face(axes_b, -n) + + a_align := abs(linalg.dot(axes_a[a_axis] * a_sign, n)) + b_align := abs(linalg.dot(axes_b[b_axis] * b_sign, -n)) + + if a_align >= b_align { + // a owns the reference face; b owns the incident + ref_corners = box_face_corners(a, axes_a, a_axis, a_sign) + // incident face on b: most anti-aligned with n (points back toward a) + i_axis, i_sign := box_best_face(axes_b, -n) + inc_corners = box_face_corners(b, axes_b, i_axis, i_sign) + ref_is_a = true + } else { + ref_corners = box_face_corners(b, axes_b, b_axis, b_sign) + i_axis, i_sign := box_best_face(axes_a, n) + inc_corners = box_face_corners(a, axes_a, i_axis, i_sign) + ref_is_a = false + } + return +} + +// END BOX DEBUG HELPERS + +body_draw :: proc(body: ^RigidBody) { + angle, axis := linalg.angle_axis_from_quaternion(body.rot) + rlgl.PushMatrix() + rlgl.Translatef(body.pos.x, body.pos.y, body.pos.z) + rlgl.Rotatef(angle * 180 / math.PI, axis.x, axis.y, axis.z) + + switch shape in body.col { + case Sphere: + rl.DrawSphere({0,0,0}, shape.radius, rl.Fade(rl.BLUE, 0.5)) + rl.DrawSphereWires({0,0,0}, shape.radius + 0.01, 8, 16, rl.BLACK) + + case Box: + rl.DrawCubeV({0,0,0}, shape.sz, rl.GRAY) + rl.DrawCubeWiresV({0,0,0}, shape.sz + 0.005, rl.BLACK) + } + + rlgl.PopMatrix() +} + +start_sim :: proc() { + clear(&bodies) + + // one cube near the edge, given a shove toward it + // cube := body_create(Box{sz={1,1,1}}, {4, 2, 0}, 10) + // cube.vel = {3, 0, 0} // push it toward the +x edge + // append(&bodies, cube) + + // floor + append(&bodies, body_create(Box{sz={50,1,50}}, {0,-0.55,0}, 0, static = true)) + + // 4 walls + // append(&bodies, body_create(Box{sz={50,2,1}}, {0,1.05,24.5}, 0, static = true)) + // append(&bodies, body_create(Box{sz={50,2,1}}, {0,1.05,-24.5}, 0, static = true)) + // append(&bodies, body_create(Box{sz={1,2,47.95}}, {24.5,1.05,0}, 0, static = true)) + // append(&bodies, body_create(Box{sz={1,2,47.95}}, {-24.5,1.05,0}, 0, static = true)) + + // larger box floating in the air + // append(&bodies, body_create(Box{sz={2,2,2}}, {0, 1, 0}, 0, static = true)) + + // N := 3 + // for x in 0..<N { + // for y in 0..<N { + // for z in 0..<N { + // spacing := f32(2.5) + // hw := (spacing) * f32(N) / f32(2) + // pos := [3]f32 { + // spacing * f32(x) - hw, + // spacing * f32(y) + 10, + // spacing * f32(z) - hw, + // } + // // rot := linalg.quaternion_from_pitch_yaw_roll_f32(10, 50, 23) + // rot := quaternion128(1) + // // append(&bodies, body_create(Sphere{radius = 0.5}, pos, 10, rot)) + // append(&bodies, body_create(Box{sz={1,1,1}}, pos, 10, rot)) + // }}} + + append(&bodies, body_create(Box{sz={1,1,1}}, {0, 1, 0}, 1)) + append(&bodies, body_create(Box{sz={1,1,1}}, {0, 2.2, 0}, 1)) +} + +main :: proc() { + rl.SetTraceLogLevel(.WARNING) + rl.SetConfigFlags({.MSAA_4X_HINT}) + rl.InitWindow(1920, 1080, "Physics") + rl.SetWindowState({.WINDOW_RESIZABLE, .FULLSCREEN_MODE}) + rl.SetTargetFPS(60) + rl.SetExitKey(.KEY_NULL) + rl.DisableCursor() + + camera := rl.Camera3D { + position = {5, 5, 5}, + target = {0, 1, 0}, + up = {0, 1, 0}, + fovy = 60, + projection = .PERSPECTIVE, + } + + start_sim() + + cursor_locked := true + for !rl.WindowShouldClose() { + if rl.IsKeyPressed(.ESCAPE) { + cursor_locked = !cursor_locked + if cursor_locked { + rl.DisableCursor() + } else { + rl.EnableCursor() + } + } + + if rl.IsKeyPressed(.R) { + start_sim() + } + + if cursor_locked { + rl.UpdateCamera(&camera, .THIRD_PERSON) + } + + physics_simulate() + + rl.BeginDrawing() + rl.ClearBackground(rl.RAYWHITE) + + rl.BeginMode3D(camera) + // rl.DrawGrid(50, 1) + + for &body in bodies { + body_draw(&body) + } + + rl.EndMode3D() + + rl.DrawFPS(10, 10) + rl.EndDrawing() + } +} + |