sergeypdev/gutter_runner
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

My own rigid body phyiscs that works, wat

Browse Source
This commit is contained in:
sergeypdev 2025-01-04 21:49:34 +04:00
parent bb77e8e821
commit 5c0dd2f9f5
5 changed files with 144 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
game/game.odin
View File

@ -46,7 +46,7 @@ Car :: struct {
SOLVER_CONFIG :: physics.Solver_Config {
timestep = 1.0 / 120,
gravity = rl.Vector3{0, 0, 0},
gravity = rl.Vector3{0, -9.8, 0},
}
Game_Memory :: struct {
@ -363,7 +363,7 @@ update :: proc() {
physics.Body_Config {
initial_pos = {0, 1, 0},
initial_rot = linalg.QUATERNIONF32_IDENTITY,
initial_ang_vel = {0, 1, 0},
initial_ang_vel = {0, 0, 0},
mass = 100,
inertia_tensor = physics.inertia_tensor_box(car_bounds.max - car_bounds.min),
},
@ -374,12 +374,14 @@ update :: proc() {
g_mem.camera.projection = .PERSPECTIVE
g_mem.camera.target = physics.get_body(&get_world().physics_scene, g_mem.car_handle).x
if g_mem.camera.position == {} {
g_mem.camera.position = g_mem.camera.target - rl.Vector3{0, 0, 10}
g_mem.camera.position = g_mem.camera.target - rl.Vector3{10, 0, 10}
}
// 1.6 is a good value
wheel_extent_x := f32(2)
rest := f32(1)
wheel_extent_x := f32(2.0)
rest := f32(0.9)
suspension_stiffness := f32(10000)
compliance := 1.0 / suspension_stiffness
physics.immediate_suspension_constraint(
&get_world().physics_scene,
@ -389,6 +391,7 @@ update :: proc() {
rel_pos = {-wheel_extent_x, 0, 2.5},
rel_dir = {0, -1, 0},
rest = rest,
compliance = compliance,
body = g_mem.car_handle,
},
)
@ -400,6 +403,7 @@ update :: proc() {
rel_pos = {wheel_extent_x, 0, 2.5},
rel_dir = {0, -1, 0},
rest = rest,
compliance = compliance,
body = g_mem.car_handle,
},
)
@ -411,6 +415,7 @@ update :: proc() {
rel_pos = {-wheel_extent_x, 0, -3},
rel_dir = {0, -1, 0},
rest = rest,
compliance = compliance,
body = g_mem.car_handle,
},
)
@ -422,6 +427,7 @@ update :: proc() {
rel_pos = {wheel_extent_x, 0, -3},
rel_dir = {0, -1, 0},
rest = rest,
compliance = compliance,
body = g_mem.car_handle,
},
)
@ -484,6 +490,8 @@ draw :: proc() {
rl.GetScreenToWorldRay(rl.GetMousePosition(), camera),
)
car_model := assets.get_model(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
{
rl.BeginMode3D(camera)
defer rl.EndMode3D()
@ -504,7 +512,6 @@ draw :: proc() {
rl.DrawGrid(100, 1)
if !g_mem.editor {
car_model := assets.get_model(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
car_body := physics.get_body(&get_world().physics_scene, g_mem.car_handle)
car_matrix := rl.QuaternionToMatrix(car_body.q)
@ -596,7 +603,13 @@ draw :: proc() {
}
} else {
car_pos := physics.get_body(&get_world().physics_scene, g_mem.car_handle).x
rl.DrawText(fmt.ctprintf("Car Pos: %v", car_pos), 5, 32, 8, rl.ORANGE)
rl.DrawText(
fmt.ctprintf("Car Pos: %v. Mesh count: %v", car_pos, car_model.meshCount),
5,
32,
8,
rl.ORANGE,
)
}
}

11
game/physics/helpers.odin
View File

@ -1,5 +1,6 @@
package physics
import lg "core:math/linalg"
import rl "vendor:raylib"
inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: rl.Vector3) {
@ -13,3 +14,13 @@ inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: rl.Vector3) {
return
}
body_local_to_world :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
return body.x + lg.quaternion_mul_vector3(body.q, pos)
}
body_world_to_local :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
// TODO: maybe store that
inv_q := lg.quaternion_inverse(body.q)
return lg.quaternion_mul_vector3(inv_q, pos - body.x)
}

4
game/physics/immediate.odin
View File

@ -27,12 +27,12 @@ initialize_body_from_config :: proc(body: ^Body, config: Body_Config) {
body.v = config.initial_vel
body.w = config.initial_ang_vel
body.inv_mass = 1.0 / config.mass
body.inv_intertia_tensor = 1.0 / (config.inertia_tensor * config.mass)
body.inv_inertia_tensor = 1.0 / (config.inertia_tensor * config.mass)
}
update_body_from_config :: proc(body: Body_Ptr, config: Body_Config) {
body.inv_mass = 1.0 / config.mass
body.inv_intertia_tensor = 1.0 / (config.inertia_tensor * config.mass)
body.inv_inertia_tensor = 1.0 / (config.inertia_tensor * config.mass)
}
update_suspension_constraint_from_config :: proc(

28
game/physics/scene.odin
View File

@ -7,6 +7,10 @@ Scene :: struct {
suspension_constraints: #soa[dynamic]Suspension_Constraint,
first_free_body_plus_one: i32,
first_free_suspension_constraint_plus_one: i32,
// Slices. When you call get_body or get_suspension_constraint you will get a pointer to an element in this slice
bodies_slice: #soa[]Body,
suspension_constraints_slice: #soa[]Suspension_Constraint,
}
Body :: struct {
@ -23,7 +27,7 @@ Body :: struct {
// Mass
inv_mass: f32,
// Moment of inertia
inv_intertia_tensor: rl.Vector3,
inv_inertia_tensor: rl.Vector3,
//
next_plus_one: i32,
}
@ -55,6 +59,9 @@ Suspension_Constraint :: struct {
Body_Handle :: distinct i32
Suspension_Constraint_Handle :: distinct i32
INVALID_BODY :: Body_Handle(0)
INVALID_SUSPENSION_CONSTRAINT :: Suspension_Constraint_Handle(0)
is_body_handle_valid :: proc(handle: Body_Handle) -> bool {
return i32(handle) > 0
}
@ -70,18 +77,19 @@ Body_Ptr :: #soa^#soa[]Body
Suspension_Constraint_Ptr :: #soa^#soa[]Suspension_Constraint
_invalid_body: #soa[1]Body
_invalid_body_slice: #soa[]Body
_invalid_suspension_constraint: #soa[1]Suspension_Constraint
_invalid_suspension_constraint_slice := _invalid_suspension_constraint[:]
/// Returns pointer to soa slice. NEVER STORE IT
get_body :: proc(scene: ^Scene, handle: Body_Handle) -> Body_Ptr {
index := int(handle) - 1
if index < 0 {
slice := _invalid_body[:]
return &slice[0]
return &_invalid_body_slice[0]
}
bodies_slice := scene.bodies[:]
return &bodies_slice[index]
return &scene.bodies_slice[index]
}
add_body :: proc(scene: ^Scene, body: Body) -> Body_Handle {
@ -101,6 +109,9 @@ add_body :: proc(scene: ^Scene, body: Body) -> Body_Handle {
append_soa(&scene.bodies, body_copy)
index := len(scene.bodies)
scene.bodies_slice = scene.bodies[:]
return Body_Handle(index)
}
@ -120,13 +131,11 @@ get_suspension_constraint :: proc(
handle: Suspension_Constraint_Handle,
) -> Suspension_Constraint_Ptr {
if !is_handle_valid(handle) {
slice := _invalid_suspension_constraint[:]
return &slice[0]
return &_invalid_suspension_constraint_slice[0]
}
index := int(handle) - 1
slice := scene.suspension_constraints[:]
return &slice[index]
return &scene.suspension_constraints_slice[index]
}
add_suspension_constraint :: proc(
@ -148,6 +157,7 @@ add_suspension_constraint :: proc(
}
append_soa(&scene.suspension_constraints, copy)
scene.suspension_constraints_slice = scene.suspension_constraints[:]
index := len(scene.suspension_constraints)
return Suspension_Constraint_Handle(index)
}

87
game/physics/simulation.odin
View File

@ -1,9 +1,12 @@
package physics
import "collision"
import "core:math"
import lg "core:math/linalg"
import rl "vendor:raylib"
_ :: math
Solver_Config :: struct {
// Will automatically do fixed timestep
timestep: f32,
@ -93,14 +96,29 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
body := get_body(scene, v.body)
q := body.q
pos := body.x
pos += lg.quaternion_mul_vector3(q, v.rel_pos)
pos := body_local_to_world(body, v.rel_pos)
dir := lg.quaternion_mul_vector3(q, 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 := v.hit_point - pos
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,
)
}
}
}
@ -118,3 +136,68 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
}
}
}
apply_constraint_correction_unilateral :: proc(
dt: f32,
body: Body_Ptr,
compliance: f32,
error: f32,
error_gradient: rl.Vector3,
pos: rl.Vector3,
other_combined_inv_mass: f32 = 0,
) {
if error == 0 {
return
}
w := get_body_inverse_mass(body, error_gradient, pos)
w += other_combined_inv_mass
if w == 0 {
return
}
alpha := compliance / dt / dt
lambda := -error / (w + alpha)
delta_pos := error_gradient * -lambda
apply_correction(body, delta_pos, pos)
}
apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) {
body.x += corr * body.inv_mass
q := body.q
inv_q := lg.quaternion_inverse(q)
delta_omega := pos - body.x
delta_omega = lg.cross(delta_omega, corr)
delta_omega = lg.quaternion_mul_vector3(inv_q, delta_omega)
delta_omega *= body.inv_inertia_tensor
delta_omega = lg.quaternion_mul_vector3(q, delta_omega)
delta_rot := quaternion(x = delta_omega.x, y = delta_omega.y, z = delta_omega.z, w = 0)
delta_rot *= q
q.x += 0.5 * delta_rot.x
q.y += 0.5 * delta_rot.y
q.z += 0.5 * delta_rot.z
q.w += 0.5 * delta_rot.w
q = lg.normalize0(q)
body.q = q
}
get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: rl.Vector3) -> f32 {
q := body.q
inv_q := lg.quaternion_inverse(q)
rn := pos - body.x
rn = lg.cross(rn, normal)
rn = lg.quaternion_mul_vector3(inv_q, rn)
rn *= rn
w := lg.dot(rn, body.inv_inertia_tensor)
w += body.inv_mass
return w
}