diff --git a/game/assets/assets.odin b/game/assets/assets.odin index ad25d27..67e6d0d 100644 --- a/game/assets/assets.odin +++ b/game/assets/assets.odin @@ -6,11 +6,13 @@ import "core:math" import lg "core:math/linalg" import "core:os/os2" import "core:strconv" +import "game:debug" import "game:halfedge" import "game:physics/bvh" import "game:physics/collision" import "libs:tracy" import rl "vendor:raylib" +import "vendor:raylib/rlgl" Loaded_Texture :: struct { texture: rl.Texture2D, @@ -31,7 +33,8 @@ Loaded_BVH :: struct { } Loaded_Convex :: struct { - mesh: collision.Convex, + mesh: collision.Convex, + center_of_mass: rl.Vector3, } destroy_loaded_bvh :: proc(loaded_bvh: Loaded_BVH) { @@ -295,26 +298,37 @@ get_convex :: proc(assetman: ^Asset_Manager, path: cstring) -> (result: Loaded_C i1, i2, i3 := indices[0], indices[1], indices[2] v1, v2, v3 := vertices[i1].pos, vertices[i2].pos, vertices[i3].pos - collision.plane_from_point_normal( + plane = collision.plane_from_point_normal( v1, lg.normalize0(lg.cross(v2 - v1, v3 - v1)), ) } face.normal = plane.normal - for index in indices[3:] { - assert( - abs(collision.signed_distance_plane(vertices[index].pos, plane)) < 0.00001, - "mesh has non planar faces", - ) - } + // for index in indices[3:] { + // assert( + // abs(collision.signed_distance_plane(vertices[index].pos, plane)) < 0.01, + // "mesh has non planar faces", + // ) + // } + + // first_vert_pos := vertices[indices[0]].pos for i in 0 ..< len(indices) { edge_idx := halfedge.Edge_Index(first_edge_idx + i) prev_edge_relative := i == 0 ? len(indices) - 1 : i - 1 next_edge_relative := (i + 1) % len(indices) i1, i2 := indices[i], indices[next_edge_relative] - v1 := &vertices[i1] + v1, _ := &vertices[i1], &vertices[i2] + + // assert( + // lg.dot( + // lg.cross(v1.pos - first_vert_pos, v2.pos - first_vert_pos), + // plane.normal, + // ) >= + // 0, + // "non convex face or non ccw winding", + // ) if v1.edge == -1 { v1.edge = edge_idx @@ -348,7 +362,78 @@ get_convex :: proc(assetman: ^Asset_Manager, path: cstring) -> (result: Loaded_C center /= f32(len(vertices)) - return {mesh = {vertices = vertices[:], edges = edges[:], faces = faces[:], center = center}} + center_of_mass: rl.Vector3 + + mesh := halfedge.Half_Edge_Mesh { + vertices = vertices[:], + edges = edges[:], + faces = faces[:], + center = center, + } + + // Center of mass calculation + total_volume := f32(0.0) + { + rlgl.Begin(rlgl.TRIANGLES) + rlgl.End() + + rlgl.EnableWireMode() + defer rlgl.DisableWireMode() + + tri_idx := 0 + for face_idx in 0 ..< len(faces) { + face := faces[face_idx] + // for all triangles + it := halfedge.iterator_face_edges(mesh, halfedge.Face_Index(face_idx)) + i := 0 + tri: [3]rl.Vector3 + for edge in halfedge.iterate_next_edge(&it) { + switch i { + case 0 ..< 3: + tri[i] = mesh.vertices[edge.origin].pos + case: + tri[1] = tri[2] + tri[2] = mesh.vertices[edge.origin].pos + } + + if i >= 2 { + plane := collision.plane_from_point_normal(tri[0], -face.normal) + + h := max(0, collision.signed_distance_plane(center, plane)) + tri_area := + lg.dot(lg.cross(tri[1] - tri[0], tri[2] - tri[0]), face.normal) * 0.5 + tetra_volume := 1.0 / 3.0 * tri_area * h + total_volume += tetra_volume + + tetra_centroid := (tri[0] + tri[1] + tri[2] + center) * 0.25 + center_of_mass += tetra_volume * tetra_centroid + + tri_idx += 1 + } + + i += 1 + } + } + } + + assert(total_volume > 0, "degenerate convex hull") + center_of_mass /= total_volume + + return {mesh = mesh, center_of_mass = center_of_mass} +} + +debug_draw_tetrahedron_wires :: proc(tri: [3]rl.Vector3, p: rl.Vector3, color: rl.Color) { + rlgl.Begin(rlgl.LINES) + defer rlgl.End() + + debug.rlgl_color(color) + + debug.rlgl_vertex3v2(tri[0], tri[1]) + debug.rlgl_vertex3v2(tri[1], tri[2]) + debug.rlgl_vertex3v2(tri[2], tri[0]) + debug.rlgl_vertex3v2(tri[0], p) + debug.rlgl_vertex3v2(tri[1], p) + debug.rlgl_vertex3v2(tri[2], p) } shutdown :: proc(assetman: ^Asset_Manager) { diff --git a/game/debug/helpers.odin b/game/debug/helpers.odin index 93b475a..69b7b84 100644 --- a/game/debug/helpers.odin +++ b/game/debug/helpers.odin @@ -35,6 +35,11 @@ rlgl_vertex3v :: proc(v: rl.Vector3) { rlgl.Vertex3f(v.x, v.y, v.z) } +rlgl_vertex3v2 :: proc(v1, v2: rl.Vector3) { + rlgl_vertex3v(v1) + rlgl_vertex3v(v2) +} + @(deferred_none = rlgl_transform_scope_end) rlgl_transform_scope :: proc(mat: rl.Matrix) { mat := mat diff --git a/game/game.odin b/game/game.odin index ba84f29..ba9ab70 100644 --- a/game/game.odin +++ b/game/game.odin @@ -555,6 +555,8 @@ draw :: proc() { car_convex := assets.get_convex(&g_mem.assetman, "assets/car_convex.obj") halfedge.debug_draw_mesh_wires(car_convex.mesh, rl.RED) + rl.DrawSphereWires(car_convex.mesh.center, 0.5, 8, 8, rl.BLUE) + rl.DrawSphereWires(car_convex.center_of_mass, 0.5, 8, 8, rl.RED) box1_convex := collision.box_to_convex(box1, context.temp_allocator) box2_convex := collision.box_to_convex(box2, context.temp_allocator) diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index 81bc19f..fb8f736 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -5,6 +5,7 @@ import "core:fmt" import "core:math" import lg "core:math/linalg" import "game:halfedge" +import "libs:tracy" import rl "vendor:raylib" _ :: math @@ -83,6 +84,8 @@ Contact_Pair :: struct { } simulate_step :: proc(scene: ^Scene, config: Solver_Config) { + tracy.Zone() + body_states := make([]Body_Sim_State, len(scene.bodies), context.temp_allocator) scene.contact_pairs_len = 0 @@ -124,126 +127,141 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { } handled_pairs := make_map(map[Body_Pair]bool, context.temp_allocator) - for _, i in scene.bodies { - body := &scene.bodies_slice[i] - if body.alive { - for _, j in scene.bodies { - body2 := &scene.bodies_slice[j] + { + tracy.ZoneN("simulate_step::collisions") - if i != j && body2.alive && !handled_pairs[{a = min(i, j), b = max(i, j)}] { - s1, s2 := body.shape.(Shape_Box), body2.shape.(Shape_Box) + for _, i in scene.bodies { + body := &scene.bodies_slice[i] + if body.alive { + for _, j in scene.bodies { + body2 := &scene.bodies_slice[j] - box1 := collision.box_to_convex( - collision.Box{rad = s1.size * 0.5}, - context.temp_allocator, - ) - box2 := collision.box_to_convex( - collision.Box{rad = s2.size * 0.5}, - context.temp_allocator, - ) + if i != j && + body2.alive && + !handled_pairs[{a = min(i, j), b = max(i, j)}] { + s1, s2 := body.shape.(Shape_Box), body2.shape.(Shape_Box) - mat1 := lg.matrix4_translate(body.x) * lg.matrix4_from_quaternion(body.q) - mat2 := lg.matrix4_translate(body2.x) * lg.matrix4_from_quaternion(body2.q) + box1 := collision.box_to_convex( + collision.Box{rad = s1.size * 0.5}, + context.temp_allocator, + ) + box2 := collision.box_to_convex( + collision.Box{rad = s2.size * 0.5}, + context.temp_allocator, + ) - halfedge.transform_mesh(&box1, mat1) - halfedge.transform_mesh(&box2, mat2) + mat1 := + lg.matrix4_translate(body.x) * lg.matrix4_from_quaternion(body.q) + mat2 := + lg.matrix4_translate(body2.x) * lg.matrix4_from_quaternion(body2.q) - // Raw manifold has contact points in world space - raw_manifold, collision := collision.convex_vs_convex_sat(box1, box2) + halfedge.transform_mesh(&box1, mat1) + halfedge.transform_mesh(&box2, mat2) - if collision { - contact_pair := &scene.contact_pairs[scene.contact_pairs_len] - contact_pair^ = Contact_Pair { - a = Body_Handle(i + 1), - b = Body_Handle(j + 1), - manifold = raw_manifold, - } - scene.contact_pairs_len += 1 - manifold := &contact_pair.manifold + // Raw manifold has contact points in world space + raw_manifold, collision := collision.convex_vs_convex_sat(box1, box2) - // Convert manifold contact from world to local space - for point_idx in 0 ..< manifold.points_len { - manifold.points_a[point_idx] = body_world_to_local( - body, - manifold.points_a[point_idx], - ) - manifold.points_b[point_idx] = body_world_to_local( - body2, - manifold.points_b[point_idx], - ) - } - for point_idx in 0 ..< manifold.points_len { - p1, p2 := - manifold.points_a[point_idx], manifold.points_b[point_idx] - p1, p2 = - body_local_to_world(body, p1), body_local_to_world(body2, p2) - - separation := min(lg.dot(p2 - p1, manifold.normal), 0) - - handled_pairs[{a = min(i, j), b = max(i, j)}] = true - - lambda_norm, corr1, corr2, ok := calculate_constraint_params2( - dt, - body, - body2, - 0, - separation, - -manifold.normal, - p1, - p2, - ) - contact_pair.lambda_normal[point_idx] = lambda_norm - - if ok { - apply_correction(body, corr1, p1) - apply_correction(body2, corr2, p2) + if collision { + contact_pair := &scene.contact_pairs[scene.contact_pairs_len] + contact_pair^ = Contact_Pair { + a = Body_Handle(i + 1), + b = Body_Handle(j + 1), + manifold = raw_manifold, } + scene.contact_pairs_len += 1 + manifold := &contact_pair.manifold - // Static friction - if ok { + // Convert manifold contact from world to local space + for point_idx in 0 ..< manifold.points_len { + manifold.points_a[point_idx] = body_world_to_local( + body, + manifold.points_a[point_idx], + ) + manifold.points_b[point_idx] = body_world_to_local( + body2, + manifold.points_b[point_idx], + ) + } + for point_idx in 0 ..< manifold.points_len { + p1, p2 := + manifold.points_a[point_idx], manifold.points_b[point_idx] p1, p2 = - body_local_to_world(body, manifold.points_a[point_idx]), - body_local_to_world(body2, manifold.points_b[point_idx]) - prev_p1 := - body_states[i].prev_x + - lg.quaternion_mul_vector3( - body_states[i].prev_q, - manifold.points_a[point_idx], - ) - prev_p2 := - body_states[j].prev_x + - lg.quaternion_mul_vector3( - body_states[j].prev_q, - manifold.points_b[point_idx], - ) + body_local_to_world(body, p1), + body_local_to_world(body2, p2) - delta_p := (p2 - prev_p2) - (p1 - prev_p1) - delta_p_tangent_norm := - delta_p - - lg.dot(manifold.normal, delta_p) * manifold.normal - delta_p_tangent_len := lg.length(delta_p_tangent_norm) + separation := min(lg.dot(p2 - p1, manifold.normal), 0) - if delta_p_tangent_len > 0 { - delta_p_tangent_norm /= delta_p_tangent_len + handled_pairs[{a = min(i, j), b = max(i, j)}] = true - lambda_tangent, corr1_tangent, corr2_tangent, ok_tangent := - calculate_constraint_params2( - dt, + lambda_norm, corr1, corr2, ok := calculate_constraint_params2( + dt, + body, + body2, + 0, + separation, + -manifold.normal, + p1, + p2, + ) + contact_pair.lambda_normal[point_idx] = lambda_norm + + if ok { + apply_correction(body, corr1, p1) + apply_correction(body2, corr2, p2) + } + + // Static friction + if ok { + p1, p2 = + body_local_to_world( body, + manifold.points_a[point_idx], + ), + body_local_to_world( body2, - 0, - -delta_p_tangent_len, - delta_p_tangent_norm, - p1, - p2, + manifold.points_b[point_idx], + ) + prev_p1 := + body_states[i].prev_x + + lg.quaternion_mul_vector3( + body_states[i].prev_q, + manifold.points_a[point_idx], + ) + prev_p2 := + body_states[j].prev_x + + lg.quaternion_mul_vector3( + body_states[j].prev_q, + manifold.points_b[point_idx], ) - contact_pair.lambda_tangent[point_idx] = lambda_tangent - STATIC_FRICTION :: 0.3 - if ok_tangent && - lambda_tangent < lambda_norm * STATIC_FRICTION { - apply_correction(body, corr1_tangent, p1) - apply_correction(body2, corr2_tangent, p2) + delta_p := (p2 - prev_p2) - (p1 - prev_p1) + delta_p_tangent_norm := + delta_p - + lg.dot(manifold.normal, delta_p) * manifold.normal + delta_p_tangent_len := lg.length(delta_p_tangent_norm) + + if delta_p_tangent_len > 0 { + delta_p_tangent_norm /= delta_p_tangent_len + + lambda_tangent, corr1_tangent, corr2_tangent, ok_tangent := + calculate_constraint_params2( + dt, + body, + body2, + 0, + -delta_p_tangent_len, + delta_p_tangent_norm, + p1, + p2, + ) + contact_pair.lambda_tangent[point_idx] = lambda_tangent + + STATIC_FRICTION :: 0.3 + if ok_tangent && + lambda_tangent < lambda_norm * STATIC_FRICTION { + apply_correction(body, corr1_tangent, p1) + apply_correction(body2, corr2_tangent, p2) + } } } } @@ -254,79 +272,89 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { } } - for &v in scene.suspension_constraints { - if v.alive { - body := get_body(scene, v.body) + { + tracy.ZoneN("simulate_step::suspension_constraints") - pos := body_local_to_world(body, v.rel_pos) - dir := body_local_to_world_vec(body, v.rel_dir) - pos2 := pos + dir * v.rest - v.hit_t, v.hit_point, v.hit = collision.intersect_segment_plane( - {pos, pos2}, - collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}), - ) - if v.hit { - corr := pos - v.hit_point - distance := lg.length(corr) - corr = corr / distance if distance > 0 else 0 + for &v in scene.suspension_constraints { + if v.alive { + body := get_body(scene, v.body) - _ = apply_constraint_correction_unilateral( - dt, - body, - v.compliance, - error = distance - v.rest, - error_gradient = corr, - pos = pos, - other_combined_inv_mass = 0, + pos := body_local_to_world(body, v.rel_pos) + dir := body_local_to_world_vec(body, v.rel_dir) + pos2 := pos + dir * v.rest + v.hit_t, v.hit_point, v.hit = collision.intersect_segment_plane( + {pos, pos2}, + collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}), ) + if v.hit { + corr := pos - v.hit_point + distance := lg.length(corr) + corr = corr / distance if distance > 0 else 0 + + _ = apply_constraint_correction_unilateral( + dt, + body, + v.compliance, + error = distance - v.rest, + error_gradient = corr, + pos = pos, + other_combined_inv_mass = 0, + ) + } } } } solve_velocities(scene, body_states, inv_dt) - for &pair in scene.contact_pairs[:scene.contact_pairs_len] { - manifold := &pair.manifold - body1 := get_body(scene, pair.a) - body2 := get_body(scene, pair.b) + { + tracy.ZoneN("simulate_step::collision_velocities") - for point_idx in 0 ..< pair.manifold.points_len { - p1, p2 := - body_local_to_world(body1, manifold.points_a[point_idx]), - body_local_to_world(body2, manifold.points_b[point_idx]) - r1, r2 := p1 - body1.x, p2 - body2.x - v1 := body_velocity_at_point(body1, p1) - v2 := body_velocity_at_point(body2, p2) + for &pair in scene.contact_pairs[:scene.contact_pairs_len] { + manifold := &pair.manifold + body1 := get_body(scene, pair.a) + body2 := get_body(scene, pair.b) - v := v1 - v2 - v_normal := lg.dot(manifold.normal, v) * manifold.normal - v_tangent := v - v_normal + for point_idx in 0 ..< pair.manifold.points_len { + p1, p2 := + body_local_to_world(body1, manifold.points_a[point_idx]), + body_local_to_world(body2, manifold.points_b[point_idx]) + r1, r2 := p1 - body1.x, p2 - body2.x + v1 := body_velocity_at_point(body1, p1) + v2 := body_velocity_at_point(body2, p2) - DYNAMIC_FRICTION :: 0.1 - v_tangent_len := lg.length(v_tangent) - if v_tangent_len > 0 { - delta_v := - -(v_tangent / v_tangent_len) * - min( - dt * DYNAMIC_FRICTION * abs(pair.lambda_normal[point_idx] / (dt * dt)), - v_tangent_len, - ) + v := v1 - v2 + v_normal := lg.dot(manifold.normal, v) * manifold.normal + v_tangent := v - v_normal - delta_v_norm := lg.normalize0(delta_v) + DYNAMIC_FRICTION :: 0.1 + v_tangent_len := lg.length(v_tangent) + if v_tangent_len > 0 { + delta_v := + -(v_tangent / v_tangent_len) * + min( + dt * + DYNAMIC_FRICTION * + abs(pair.lambda_normal[point_idx] / (dt * dt)), + v_tangent_len, + ) - w1, w2 := - get_body_inverse_mass(body1, delta_v_norm, p1), - get_body_inverse_mass(body2, delta_v_norm, p2) + delta_v_norm := lg.normalize0(delta_v) - w := w1 + w2 - if w != 0 { - p := delta_v / w + w1, w2 := + get_body_inverse_mass(body1, delta_v_norm, p1), + get_body_inverse_mass(body2, delta_v_norm, p2) - body1.v += p * body1.inv_mass - body2.v -= p * body2.inv_mass + w := w1 + w2 + if w != 0 { + p := delta_v / w - body1.w += multiply_inv_intertia(body1, lg.cross(r1, p)) - body2.w -= multiply_inv_intertia(body2, lg.cross(r2, p)) + body1.v += p * body1.inv_mass + body2.v -= p * body2.inv_mass + + body1.w += multiply_inv_intertia(body1, lg.cross(r1, p)) + body2.w -= multiply_inv_intertia(body2, lg.cross(r2, p)) + } } } } diff --git a/src_assets/car_convex.blend b/src_assets/car_convex.blend index e5c90dd..3d67405 100644 Binary files a/src_assets/car_convex.blend and b/src_assets/car_convex.blend differ diff --git a/src_assets/car_convex.blend1 b/src_assets/car_convex.blend1 new file mode 100644 index 0000000..e6d5156 Binary files /dev/null and b/src_assets/car_convex.blend1 differ