Wheel friction working
This commit is contained in:
parent
5c0dd2f9f5
commit
0687ff4858
153
game/editor.odin
Normal file
153
game/editor.odin
Normal file
@ -0,0 +1,153 @@
|
||||
package game
|
||||
|
||||
import lg "core:math/linalg"
|
||||
import rl "vendor:raylib"
|
||||
|
||||
update_free_look_camera :: proc(es: ^Editor_State) {
|
||||
input: rl.Vector2
|
||||
|
||||
if rl.IsKeyDown(.UP) || rl.IsKeyDown(.W) {
|
||||
input.y -= 1
|
||||
}
|
||||
if rl.IsKeyDown(.DOWN) || rl.IsKeyDown(.S) {
|
||||
input.y += 1
|
||||
}
|
||||
if rl.IsKeyDown(.LEFT) || rl.IsKeyDown(.A) {
|
||||
input.x -= 1
|
||||
}
|
||||
if rl.IsKeyDown(.RIGHT) || rl.IsKeyDown(.D) {
|
||||
input.x += 1
|
||||
}
|
||||
|
||||
should_capture_mouse := rl.IsMouseButtonDown(.RIGHT)
|
||||
if es.mouse_captured != should_capture_mouse {
|
||||
if should_capture_mouse {
|
||||
rl.DisableCursor()
|
||||
} else {
|
||||
rl.EnableCursor()
|
||||
}
|
||||
}
|
||||
es.mouse_captured = should_capture_mouse
|
||||
|
||||
if es.mouse_captured {
|
||||
get_runtime_world().camera_yaw_pitch += rl.GetMouseDelta().yx * -1 * 0.001
|
||||
}
|
||||
|
||||
get_runtime_world().camera_speed += rl.GetMouseWheelMove() * 0.01
|
||||
get_runtime_world().camera_speed = lg.clamp(get_runtime_world().camera_speed, 0.01, 10)
|
||||
|
||||
rotation_matrix := camera_rotation_matrix()
|
||||
forward := -rotation_matrix[2]
|
||||
right := lg.cross(rl.Vector3{0, 1, 0}, forward)
|
||||
|
||||
input = lg.normalize0(input)
|
||||
get_world().player_pos +=
|
||||
(input.x * right + input.y * forward) * get_runtime_world().camera_speed
|
||||
}
|
||||
|
||||
update_editor :: proc(es: ^Editor_State) {
|
||||
update_free_look_camera(es)
|
||||
|
||||
switch es.track_edit_state {
|
||||
case .Select:
|
||||
{
|
||||
if rl.IsKeyPressed(.F) {
|
||||
add_track_spline_point()
|
||||
}
|
||||
|
||||
if is_point_selected() {
|
||||
if rl.IsKeyPressed(.X) {
|
||||
|
||||
if len(es.point_selection) <= 1 {
|
||||
for i in es.point_selection {
|
||||
ordered_remove(&get_world().track.points, i)
|
||||
}
|
||||
} else {
|
||||
#reverse for _, i in get_world().track.points {
|
||||
if i in es.point_selection {
|
||||
ordered_remove(&get_world().track.points, i)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
clear(&es.point_selection)
|
||||
}
|
||||
if rl.IsKeyPressed(.G) {
|
||||
es.track_edit_state = .Move
|
||||
es.move_axis = .None
|
||||
es.total_movement_world = {}
|
||||
// es.initial_point_pos = g_mem.track.points[es.selected_track_point]
|
||||
}
|
||||
}
|
||||
}
|
||||
case .Move:
|
||||
{
|
||||
if rl.IsKeyPressed(.ESCAPE) {
|
||||
es.track_edit_state = .Select
|
||||
// g_mem.track.points[es.selected_track_point] = es.initial_point_pos
|
||||
break
|
||||
}
|
||||
|
||||
if (rl.IsMouseButtonPressed(.LEFT)) {
|
||||
es.track_edit_state = .Select
|
||||
break
|
||||
}
|
||||
|
||||
if !es.mouse_captured {
|
||||
// Blender style movement
|
||||
if rl.IsKeyDown(.LEFT_SHIFT) {
|
||||
if rl.IsKeyPressed(.X) {
|
||||
es.move_axis = .YZ
|
||||
}
|
||||
if rl.IsKeyPressed(.Y) {
|
||||
es.move_axis = .XZ
|
||||
}
|
||||
if rl.IsKeyPressed(.Z) {
|
||||
es.move_axis = .XY
|
||||
}
|
||||
} else {
|
||||
if rl.IsKeyPressed(.X) {
|
||||
es.move_axis = .X
|
||||
}
|
||||
if rl.IsKeyPressed(.Y) {
|
||||
es.move_axis = .Y
|
||||
}
|
||||
if rl.IsKeyPressed(.Z) {
|
||||
es.move_axis = .Z
|
||||
}
|
||||
}
|
||||
|
||||
// log.debugf("Move axis %v", es.move_axis)
|
||||
|
||||
camera := game_camera_3d()
|
||||
|
||||
mouse_delta := rl.GetMouseDelta() * 0.05
|
||||
|
||||
view_rotation := lg.transpose(rl.GetCameraMatrix(camera))
|
||||
view_rotation[3].xyz = 0
|
||||
view_proj := view_rotation * rl.MatrixOrtho(-1, 1, 1, -1, -1, 1)
|
||||
|
||||
axes_buf: [2]rl.Vector3
|
||||
colors_buf: [2]rl.Color
|
||||
axes, _ := get_movement_axes(es.move_axis, &axes_buf, &colors_buf)
|
||||
|
||||
movement_world: rl.Vector3
|
||||
for axis in axes {
|
||||
axis_screen := (rl.Vector4{axis.x, axis.y, axis.z, 1} * view_proj).xy
|
||||
axis_screen = lg.normalize0(axis_screen)
|
||||
|
||||
movement_screen := lg.dot(axis_screen, mouse_delta) * axis_screen
|
||||
movement_world +=
|
||||
(rl.Vector4{movement_screen.x, movement_screen.y, 0, 1} * rl.MatrixInvert(view_proj)).xyz
|
||||
}
|
||||
|
||||
for k in es.point_selection {
|
||||
get_world().track.points[k] += movement_world
|
||||
}
|
||||
|
||||
es.total_movement_world += movement_world
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
426
game/game.odin
426
game/game.odin
@ -31,34 +31,46 @@ Track :: struct {
|
||||
}
|
||||
|
||||
World :: struct {
|
||||
player_pos: rl.Vector3,
|
||||
track: Track,
|
||||
physics_scene: physics.Scene,
|
||||
}
|
||||
|
||||
destroy_world :: proc(world: ^World) {
|
||||
delete(world.track.points)
|
||||
physics.destroy_physics_scene(&world.physics_scene)
|
||||
}
|
||||
|
||||
|
||||
Runtime_World :: struct {
|
||||
world: World,
|
||||
pause: bool,
|
||||
solver_state: physics.Solver_State,
|
||||
car_com: rl.Vector3,
|
||||
car_handle: physics.Body_Handle,
|
||||
camera_yaw_pitch: rl.Vector2,
|
||||
camera_speed: f32,
|
||||
camera: rl.Camera3D,
|
||||
}
|
||||
destroy_runtime_world :: proc(runtime_world: ^Runtime_World) {
|
||||
destroy_world(&runtime_world.world)
|
||||
physics.destroy_solver_state(&runtime_world.solver_state)
|
||||
}
|
||||
|
||||
Car :: struct {
|
||||
pos: rl.Vector3,
|
||||
}
|
||||
|
||||
SOLVER_CONFIG :: physics.Solver_Config {
|
||||
timestep = 1.0 / 120,
|
||||
gravity = rl.Vector3{0, -9.8, 0},
|
||||
timestep = 1.0 / 120,
|
||||
gravity = rl.Vector3{0, -9.8, 0},
|
||||
substreps_minus_one = 8 - 1,
|
||||
}
|
||||
|
||||
Game_Memory :: struct {
|
||||
assetman: assets.Asset_Manager,
|
||||
player_pos: rl.Vector3,
|
||||
camera_yaw_pitch: rl.Vector2,
|
||||
camera_speed: f32,
|
||||
camera: rl.Camera3D,
|
||||
solver_state: physics.Solver_State,
|
||||
car_handle: physics.Body_Handle,
|
||||
es: Editor_State,
|
||||
editor: bool,
|
||||
assetman: assets.Asset_Manager,
|
||||
runtime_world: Runtime_World,
|
||||
es: Editor_State,
|
||||
editor: bool,
|
||||
}
|
||||
|
||||
Track_Edit_State :: enum {
|
||||
@ -90,19 +102,34 @@ Editor_State :: struct {
|
||||
|
||||
g_mem: ^Game_Memory
|
||||
|
||||
get_runtime_world :: proc() -> ^Runtime_World {
|
||||
return &g_mem.runtime_world
|
||||
}
|
||||
|
||||
get_world :: proc() -> ^World {
|
||||
return &g_mem.es.world
|
||||
return g_mem.editor ? &g_mem.es.world : &g_mem.runtime_world.world
|
||||
}
|
||||
|
||||
get_editor_state :: proc() -> ^Editor_State {
|
||||
return &g_mem.es
|
||||
}
|
||||
|
||||
game_camera :: proc() -> rl.Camera2D {
|
||||
w := f32(rl.GetScreenWidth())
|
||||
h := f32(rl.GetScreenHeight())
|
||||
|
||||
return {zoom = h / PIXEL_WINDOW_HEIGHT, target = g_mem.player_pos.xy, offset = {w / 2, h / 2}}
|
||||
return {
|
||||
zoom = h / PIXEL_WINDOW_HEIGHT,
|
||||
target = get_world().player_pos.xy,
|
||||
offset = {w / 2, h / 2},
|
||||
}
|
||||
}
|
||||
|
||||
camera_rotation_matrix :: proc() -> matrix[3, 3]f32 {
|
||||
return linalg.matrix3_from_euler_angles_xy(g_mem.camera_yaw_pitch.x, g_mem.camera_yaw_pitch.y)
|
||||
return linalg.matrix3_from_euler_angles_xy(
|
||||
get_runtime_world().camera_yaw_pitch.x,
|
||||
get_runtime_world().camera_yaw_pitch.y,
|
||||
)
|
||||
}
|
||||
|
||||
camera_forward_vec :: proc() -> rl.Vector3 {
|
||||
@ -113,64 +140,21 @@ camera_forward_vec :: proc() -> rl.Vector3 {
|
||||
game_camera_3d :: proc() -> rl.Camera3D {
|
||||
if g_mem.editor {
|
||||
return {
|
||||
position = g_mem.player_pos,
|
||||
position = get_world().player_pos,
|
||||
up = {0, 1, 0},
|
||||
fovy = 60,
|
||||
target = g_mem.player_pos + camera_forward_vec(),
|
||||
target = get_world().player_pos + camera_forward_vec(),
|
||||
projection = .PERSPECTIVE,
|
||||
}
|
||||
}
|
||||
|
||||
return g_mem.camera
|
||||
return get_runtime_world().camera
|
||||
}
|
||||
|
||||
ui_camera :: proc() -> rl.Camera2D {
|
||||
return {zoom = f32(rl.GetScreenHeight()) / PIXEL_WINDOW_HEIGHT}
|
||||
}
|
||||
|
||||
update_free_look_camera :: proc() {
|
||||
es := &g_mem.es
|
||||
|
||||
input: rl.Vector2
|
||||
|
||||
if rl.IsKeyDown(.UP) || rl.IsKeyDown(.W) {
|
||||
input.y -= 1
|
||||
}
|
||||
if rl.IsKeyDown(.DOWN) || rl.IsKeyDown(.S) {
|
||||
input.y += 1
|
||||
}
|
||||
if rl.IsKeyDown(.LEFT) || rl.IsKeyDown(.A) {
|
||||
input.x -= 1
|
||||
}
|
||||
if rl.IsKeyDown(.RIGHT) || rl.IsKeyDown(.D) {
|
||||
input.x += 1
|
||||
}
|
||||
|
||||
should_capture_mouse := rl.IsMouseButtonDown(.RIGHT)
|
||||
if es.mouse_captured != should_capture_mouse {
|
||||
if should_capture_mouse {
|
||||
rl.DisableCursor()
|
||||
} else {
|
||||
rl.EnableCursor()
|
||||
}
|
||||
}
|
||||
es.mouse_captured = should_capture_mouse
|
||||
|
||||
if es.mouse_captured {
|
||||
g_mem.camera_yaw_pitch += rl.GetMouseDelta().yx * -1 * 0.001
|
||||
}
|
||||
|
||||
g_mem.camera_speed += rl.GetMouseWheelMove() * 0.01
|
||||
g_mem.camera_speed = linalg.clamp(g_mem.camera_speed, 0.01, 10)
|
||||
|
||||
rotation_matrix := camera_rotation_matrix()
|
||||
forward := -rotation_matrix[2]
|
||||
right := linalg.cross(rl.Vector3{0, 1, 0}, forward)
|
||||
|
||||
input = linalg.normalize0(input)
|
||||
g_mem.player_pos += (input.x * right + input.y * forward) * g_mem.camera_speed
|
||||
}
|
||||
|
||||
select_track_point :: proc(index: int) {
|
||||
clear(&g_mem.es.point_selection)
|
||||
g_mem.es.point_selection[index] = true
|
||||
@ -183,7 +167,7 @@ is_point_selected :: proc() -> bool {
|
||||
add_track_spline_point :: proc() {
|
||||
forward := camera_rotation_matrix()[2]
|
||||
|
||||
append(&get_world().track.points, g_mem.player_pos + forward)
|
||||
append(&get_world().track.points, get_world().player_pos + forward)
|
||||
select_track_point(len(&get_world().track.points) - 1)
|
||||
}
|
||||
|
||||
@ -233,135 +217,20 @@ get_movement_axes :: proc(
|
||||
return out_axes[0:0], out_colors[0:0]
|
||||
}
|
||||
|
||||
update_editor :: proc() {
|
||||
es := &g_mem.es
|
||||
update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
world := &runtime_world.world
|
||||
|
||||
switch es.track_edit_state {
|
||||
case .Select:
|
||||
{
|
||||
if rl.IsKeyPressed(.F) {
|
||||
add_track_spline_point()
|
||||
}
|
||||
|
||||
if is_point_selected() {
|
||||
if rl.IsKeyPressed(.X) {
|
||||
|
||||
if len(es.point_selection) <= 1 {
|
||||
for i in es.point_selection {
|
||||
ordered_remove(&get_world().track.points, i)
|
||||
}
|
||||
} else {
|
||||
#reverse for _, i in get_world().track.points {
|
||||
if i in es.point_selection {
|
||||
ordered_remove(&get_world().track.points, i)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
clear(&es.point_selection)
|
||||
}
|
||||
if rl.IsKeyPressed(.G) {
|
||||
es.track_edit_state = .Move
|
||||
es.move_axis = .None
|
||||
es.total_movement_world = {}
|
||||
// es.initial_point_pos = g_mem.track.points[es.selected_track_point]
|
||||
}
|
||||
}
|
||||
}
|
||||
case .Move:
|
||||
{
|
||||
if rl.IsKeyPressed(.ESCAPE) {
|
||||
es.track_edit_state = .Select
|
||||
// g_mem.track.points[es.selected_track_point] = es.initial_point_pos
|
||||
break
|
||||
}
|
||||
|
||||
if (rl.IsMouseButtonPressed(.LEFT)) {
|
||||
es.track_edit_state = .Select
|
||||
break
|
||||
}
|
||||
|
||||
if !es.mouse_captured {
|
||||
// Blender style movement
|
||||
if rl.IsKeyDown(.LEFT_SHIFT) {
|
||||
if rl.IsKeyPressed(.X) {
|
||||
es.move_axis = .YZ
|
||||
}
|
||||
if rl.IsKeyPressed(.Y) {
|
||||
es.move_axis = .XZ
|
||||
}
|
||||
if rl.IsKeyPressed(.Z) {
|
||||
es.move_axis = .XY
|
||||
}
|
||||
} else {
|
||||
if rl.IsKeyPressed(.X) {
|
||||
es.move_axis = .X
|
||||
}
|
||||
if rl.IsKeyPressed(.Y) {
|
||||
es.move_axis = .Y
|
||||
}
|
||||
if rl.IsKeyPressed(.Z) {
|
||||
es.move_axis = .Z
|
||||
}
|
||||
}
|
||||
|
||||
// log.debugf("Move axis %v", es.move_axis)
|
||||
|
||||
camera := game_camera_3d()
|
||||
|
||||
mouse_delta := rl.GetMouseDelta() * 0.05
|
||||
|
||||
view_rotation := linalg.transpose(rl.GetCameraMatrix(camera))
|
||||
view_rotation[3].xyz = 0
|
||||
view_proj := view_rotation * rl.MatrixOrtho(-1, 1, 1, -1, -1, 1)
|
||||
|
||||
axes_buf: [2]rl.Vector3
|
||||
colors_buf: [2]rl.Color
|
||||
axes, _ := get_movement_axes(es.move_axis, &axes_buf, &colors_buf)
|
||||
|
||||
movement_world: rl.Vector3
|
||||
for axis in axes {
|
||||
axis_screen := (rl.Vector4{axis.x, axis.y, axis.z, 1} * view_proj).xy
|
||||
axis_screen = linalg.normalize0(axis_screen)
|
||||
|
||||
movement_screen := linalg.dot(axis_screen, mouse_delta) * axis_screen
|
||||
movement_world +=
|
||||
(rl.Vector4{movement_screen.x, movement_screen.y, 0, 1} * rl.MatrixInvert(view_proj)).xyz
|
||||
}
|
||||
|
||||
for k in es.point_selection {
|
||||
get_world().track.points[k] += movement_world
|
||||
}
|
||||
|
||||
es.total_movement_world += movement_world
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
update :: proc() {
|
||||
if rl.IsKeyPressed(.TAB) {
|
||||
g_mem.editor = !g_mem.editor
|
||||
|
||||
// if g_mem.editor {
|
||||
// rl.EnableCursor()
|
||||
// } else {
|
||||
// rl.DisableCursor()
|
||||
// }
|
||||
}
|
||||
|
||||
dt := rl.GetFrameTime()
|
||||
|
||||
if !g_mem.editor {
|
||||
if !runtime_world.pause {
|
||||
car_model := assets.get_model(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
|
||||
car_bounds := rl.GetModelBoundingBox(car_model)
|
||||
runtime_world.car_com = (car_bounds.min + car_bounds.max) / 2
|
||||
|
||||
g_mem.car_handle = physics.immediate_body(
|
||||
&get_world().physics_scene,
|
||||
&g_mem.solver_state,
|
||||
runtime_world.car_handle = physics.immediate_body(
|
||||
&world.physics_scene,
|
||||
&runtime_world.solver_state,
|
||||
#hash("car", "fnv32a"),
|
||||
physics.Body_Config {
|
||||
initial_pos = {0, 1, 0},
|
||||
initial_pos = {0, 2, 0},
|
||||
initial_rot = linalg.QUATERNIONF32_IDENTITY,
|
||||
initial_ang_vel = {0, 0, 0},
|
||||
mass = 100,
|
||||
@ -369,81 +238,133 @@ update :: proc() {
|
||||
},
|
||||
)
|
||||
|
||||
g_mem.camera.up = rl.Vector3{0, 1, 0}
|
||||
g_mem.camera.fovy = 60
|
||||
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{10, 0, 10}
|
||||
runtime_world.camera.up = rl.Vector3{0, 1, 0}
|
||||
runtime_world.camera.fovy = 60
|
||||
runtime_world.camera.projection = .PERSPECTIVE
|
||||
runtime_world.camera.target =
|
||||
physics.get_body(&world.physics_scene, runtime_world.car_handle).x
|
||||
if runtime_world.camera.position == {} {
|
||||
runtime_world.camera.position = runtime_world.camera.target - rl.Vector3{10, 0, 10}
|
||||
}
|
||||
|
||||
// 1.6 is a good value
|
||||
wheel_extent_x := f32(2.0)
|
||||
rest := f32(0.9)
|
||||
suspension_stiffness := f32(10000)
|
||||
wheel_extent_x := f32(2)
|
||||
wheel_y := f32(-0.5)
|
||||
rest := f32(1)
|
||||
suspension_stiffness := f32(2000)
|
||||
compliance := 1.0 / suspension_stiffness
|
||||
damping := f32(0.1)
|
||||
radius := f32(0.6)
|
||||
|
||||
physics.immediate_suspension_constraint(
|
||||
&get_world().physics_scene,
|
||||
&g_mem.solver_state,
|
||||
wheel_fl := physics.immediate_suspension_constraint(
|
||||
&world.physics_scene,
|
||||
&runtime_world.solver_state,
|
||||
#hash("FL", "fnv32a"),
|
||||
{
|
||||
rel_pos = {-wheel_extent_x, 0, 2.5},
|
||||
rel_pos = {-wheel_extent_x, wheel_y, 2.9},
|
||||
rel_dir = {0, -1, 0},
|
||||
radius = radius,
|
||||
rest = rest,
|
||||
compliance = compliance,
|
||||
body = g_mem.car_handle,
|
||||
damping = damping,
|
||||
body = runtime_world.car_handle,
|
||||
},
|
||||
)
|
||||
physics.immediate_suspension_constraint(
|
||||
&get_world().physics_scene,
|
||||
&g_mem.solver_state,
|
||||
wheel_fr := physics.immediate_suspension_constraint(
|
||||
&world.physics_scene,
|
||||
&runtime_world.solver_state,
|
||||
#hash("FR", "fnv32a"),
|
||||
{
|
||||
rel_pos = {wheel_extent_x, 0, 2.5},
|
||||
rel_pos = {wheel_extent_x, wheel_y, 2.9},
|
||||
rel_dir = {0, -1, 0},
|
||||
radius = radius,
|
||||
rest = rest,
|
||||
compliance = compliance,
|
||||
body = g_mem.car_handle,
|
||||
damping = damping,
|
||||
body = runtime_world.car_handle,
|
||||
},
|
||||
)
|
||||
physics.immediate_suspension_constraint(
|
||||
&get_world().physics_scene,
|
||||
&g_mem.solver_state,
|
||||
wheel_rl := physics.immediate_suspension_constraint(
|
||||
&world.physics_scene,
|
||||
&runtime_world.solver_state,
|
||||
#hash("RL", "fnv32a"),
|
||||
{
|
||||
rel_pos = {-wheel_extent_x, 0, -3},
|
||||
rel_pos = {-wheel_extent_x, wheel_y, -2.6},
|
||||
rel_dir = {0, -1, 0},
|
||||
radius = radius,
|
||||
rest = rest,
|
||||
compliance = compliance,
|
||||
body = g_mem.car_handle,
|
||||
damping = damping,
|
||||
body = runtime_world.car_handle,
|
||||
},
|
||||
)
|
||||
physics.immediate_suspension_constraint(
|
||||
&get_world().physics_scene,
|
||||
&g_mem.solver_state,
|
||||
wheel_rr := physics.immediate_suspension_constraint(
|
||||
&world.physics_scene,
|
||||
&runtime_world.solver_state,
|
||||
#hash("RR", "fnv32a"),
|
||||
{
|
||||
rel_pos = {wheel_extent_x, 0, -3},
|
||||
rel_pos = {wheel_extent_x, wheel_y, -2.6},
|
||||
rel_dir = {0, -1, 0},
|
||||
radius = radius,
|
||||
rest = rest,
|
||||
compliance = compliance,
|
||||
body = g_mem.car_handle,
|
||||
damping = damping,
|
||||
body = runtime_world.car_handle,
|
||||
},
|
||||
)
|
||||
|
||||
} else {
|
||||
update_free_look_camera()
|
||||
drive_wheels := []physics.Suspension_Constraint_Handle{wheel_rl, wheel_rr}
|
||||
turn_wheels := []physics.Suspension_Constraint_Handle{wheel_fl, wheel_fr}
|
||||
|
||||
update_editor()
|
||||
DRIVE_IMPULSE :: 1
|
||||
BRAKE_IMPULSE :: 2
|
||||
TURN_ANGLE :: -f32(10) * math.RAD_PER_DEG
|
||||
|
||||
for wheel_handle in drive_wheels {
|
||||
wheel := physics.get_suspension_constraint(&world.physics_scene, wheel_handle)
|
||||
|
||||
wheel.drive_impulse = 0
|
||||
wheel.brake_impulse = 0
|
||||
|
||||
if rl.IsKeyDown(.W) {
|
||||
wheel.drive_impulse = DRIVE_IMPULSE
|
||||
}
|
||||
|
||||
if rl.IsKeyDown(.S) {
|
||||
wheel.brake_impulse = BRAKE_IMPULSE
|
||||
}
|
||||
}
|
||||
|
||||
for wheel_handle in turn_wheels {
|
||||
wheel := physics.get_suspension_constraint(&world.physics_scene, wheel_handle)
|
||||
wheel.turn_angle = 0
|
||||
|
||||
if (rl.IsKeyDown(.A)) {
|
||||
wheel.turn_angle += -TURN_ANGLE
|
||||
}
|
||||
|
||||
if (rl.IsKeyDown(.D)) {
|
||||
wheel.turn_angle += TURN_ANGLE
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
physics.simulate(&world.physics_scene, &runtime_world.solver_state, SOLVER_CONFIG, dt)
|
||||
}
|
||||
}
|
||||
|
||||
update :: proc() {
|
||||
if rl.IsKeyPressed(.TAB) {
|
||||
g_mem.editor = !g_mem.editor
|
||||
}
|
||||
|
||||
physics.simulate(
|
||||
&g_mem.es.world.physics_scene,
|
||||
&g_mem.solver_state,
|
||||
SOLVER_CONFIG,
|
||||
g_mem.editor ? 0 : dt,
|
||||
)
|
||||
dt := rl.GetFrameTime()
|
||||
|
||||
if g_mem.editor {
|
||||
update_editor(get_editor_state())
|
||||
} else {
|
||||
update_runtime_world(get_runtime_world(), dt)
|
||||
}
|
||||
}
|
||||
|
||||
catmull_rom_coefs :: proc(
|
||||
@ -479,9 +400,12 @@ draw :: proc() {
|
||||
defer rl.EndDrawing()
|
||||
rl.ClearBackground(rl.BLACK)
|
||||
|
||||
runtime_world := get_runtime_world()
|
||||
world := get_world()
|
||||
|
||||
camera := game_camera_3d()
|
||||
|
||||
points := &get_world().track.points
|
||||
points := &world.track.points
|
||||
|
||||
interpolated_points := calculate_spline_interpolated_points(points[:], context.temp_allocator)
|
||||
|
||||
@ -496,43 +420,17 @@ draw :: proc() {
|
||||
rl.BeginMode3D(camera)
|
||||
defer rl.EndMode3D()
|
||||
|
||||
if collision.hit {
|
||||
// tangent, bitangent := get_point_frame(interpolated_points, segment_idx)
|
||||
|
||||
// rot_matrix: linalg.Matrix3f32
|
||||
// rot_matrix[0] = bitangent
|
||||
// rot_matrix[1] = interpolated_points[segment_idx].normal
|
||||
// rot_matrix[2] = -tangent
|
||||
|
||||
// angle, axis := linalg.angle_axis_from_quaternion(
|
||||
// linalg.quaternion_from_matrix3(rot_matrix),
|
||||
// )
|
||||
}
|
||||
|
||||
rl.DrawGrid(100, 1)
|
||||
|
||||
if !g_mem.editor {
|
||||
|
||||
car_body := physics.get_body(&get_world().physics_scene, g_mem.car_handle)
|
||||
car_body := physics.get_body(&world.physics_scene, runtime_world.car_handle)
|
||||
car_matrix := rl.QuaternionToMatrix(car_body.q)
|
||||
car_model.transform = car_matrix
|
||||
|
||||
rl.DrawModel(car_model, car_body.x, 1, rl.WHITE)
|
||||
rl.DrawModel(car_model, car_body.x - runtime_world.car_com, 1, rl.WHITE)
|
||||
}
|
||||
|
||||
physics.draw_debug_scene(&get_world().physics_scene)
|
||||
|
||||
// road: rl.Mesh
|
||||
// defer rl.UnloadMesh(road)
|
||||
// road_vertices: [dynamic]f32
|
||||
// road_normals: [dynamic]f32
|
||||
// road_uvs: [dynamic]f32
|
||||
// road_indices: [dynamic]u16
|
||||
// road_vertices.allocator = context.temp_allocator
|
||||
// road_normals.allocator = context.temp_allocator
|
||||
// road_uvs.allocator = context.temp_allocator
|
||||
// road_indices.allocator = context.temp_allocator
|
||||
|
||||
physics.draw_debug_scene(&world.physics_scene)
|
||||
|
||||
{
|
||||
// Debug draw spline road
|
||||
@ -602,14 +500,8 @@ draw :: proc() {
|
||||
)
|
||||
}
|
||||
} else {
|
||||
car_pos := physics.get_body(&get_world().physics_scene, g_mem.car_handle).x
|
||||
rl.DrawText(
|
||||
fmt.ctprintf("Car Pos: %v. Mesh count: %v", car_pos, car_model.meshCount),
|
||||
5,
|
||||
32,
|
||||
8,
|
||||
rl.ORANGE,
|
||||
)
|
||||
car_pos := physics.get_body(&world.physics_scene, runtime_world.car_handle).x
|
||||
rl.DrawText(fmt.ctprintf("Car Pos: %v", car_pos), 5, 32, 8, rl.ORANGE)
|
||||
}
|
||||
}
|
||||
|
||||
@ -637,7 +529,7 @@ draw :: proc() {
|
||||
false,
|
||||
rl.GuiIconName.ICON_TARGET_POINT,
|
||||
)) {
|
||||
inject_at(&get_world().track.points, 0, new_point_pos)
|
||||
inject_at(&world.track.points, 0, new_point_pos)
|
||||
log.debugf("add point before 0")
|
||||
}
|
||||
}
|
||||
@ -660,7 +552,7 @@ draw :: proc() {
|
||||
false,
|
||||
rl.GuiIconName.ICON_TARGET_POINT,
|
||||
)) {
|
||||
inject_at(&get_world().track.points, points_len - 1 + 1, new_point_pos)
|
||||
inject_at(&world.track.points, points_len - 1 + 1, new_point_pos)
|
||||
log.debugf("add point before 0")
|
||||
}
|
||||
}
|
||||
@ -674,12 +566,12 @@ draw :: proc() {
|
||||
middle_pos := sample_spline(points[:], t)
|
||||
|
||||
if (spline_handle(middle_pos, camera, false, rl.GuiIconName.ICON_TARGET_POINT)) {
|
||||
inject_at(&get_world().track.points, i + 1, middle_pos)
|
||||
inject_at(&world.track.points, i + 1, middle_pos)
|
||||
log.debugf("add point after %d", i)
|
||||
}
|
||||
}
|
||||
|
||||
if spline_handle(get_world().track.points[i], camera, es.point_selection[i]) {
|
||||
if spline_handle(world.track.points[i], camera, es.point_selection[i]) {
|
||||
if !rl.IsKeyDown(.LEFT_CONTROL) {
|
||||
clear(&g_mem.es.point_selection)
|
||||
}
|
||||
@ -784,9 +676,9 @@ game_init :: proc() {
|
||||
@(export)
|
||||
game_shutdown :: proc() {
|
||||
assets.shutdown(&g_mem.assetman)
|
||||
destroy_world(get_world())
|
||||
destroy_world(&g_mem.es.world)
|
||||
delete(g_mem.es.point_selection)
|
||||
physics.destroy_solver_state(&g_mem.solver_state)
|
||||
destroy_runtime_world(&g_mem.runtime_world)
|
||||
|
||||
free(g_mem)
|
||||
}
|
||||
|
||||
@ -1,10 +1,12 @@
|
||||
package physics
|
||||
|
||||
import "core:log"
|
||||
import "core:math"
|
||||
import lg "core:math/linalg"
|
||||
import rl "vendor:raylib"
|
||||
|
||||
_ :: log
|
||||
_ :: math
|
||||
|
||||
draw_debug_scene :: proc(scene: ^Scene) {
|
||||
for &body in scene.bodies {
|
||||
@ -22,20 +24,40 @@ draw_debug_scene :: proc(scene: ^Scene) {
|
||||
}
|
||||
}
|
||||
|
||||
for &constraint in scene.suspension_constraints {
|
||||
if constraint.alive {
|
||||
body := get_body(scene, constraint.body)
|
||||
t := constraint.hit_t > 0 ? constraint.hit_t : constraint.rest
|
||||
for _, i in scene.suspension_constraints {
|
||||
wheel := &scene.suspension_constraints_slice[i]
|
||||
if wheel.alive {
|
||||
body := get_body(scene, wheel.body)
|
||||
t := wheel.hit_t > 0 ? wheel.hit_t : wheel.rest
|
||||
|
||||
pos := body.x
|
||||
rot := body.q
|
||||
pos += lg.quaternion_mul_vector3(rot, constraint.rel_pos)
|
||||
dir := lg.quaternion_mul_vector3(rot, constraint.rel_dir)
|
||||
pos += lg.quaternion_mul_vector3(rot, wheel.rel_pos)
|
||||
dir := lg.quaternion_mul_vector3(rot, wheel.rel_dir)
|
||||
|
||||
rl.DrawLine3D(pos, pos + dir * t, rl.ORANGE)
|
||||
|
||||
if constraint.hit {
|
||||
rl.DrawSphereWires(constraint.hit_point, 0.1, 4, 4, rl.RED)
|
||||
rel_wheel_pos := wheel_get_rel_wheel_pos(body, wheel)
|
||||
wheel_pos := body_local_to_world(body, rel_wheel_pos)
|
||||
right := wheel_get_right_vec(body, wheel)
|
||||
|
||||
rl.DrawCylinderWiresEx(
|
||||
wheel_pos - right * 0.1,
|
||||
wheel_pos + right * 0.1,
|
||||
wheel.radius,
|
||||
wheel.radius,
|
||||
16,
|
||||
rl.RED,
|
||||
)
|
||||
|
||||
rl.DrawLine3D(
|
||||
pos + t * dir,
|
||||
pos + t * dir + wheel.applied_impulse.x * right * 10,
|
||||
rl.RED,
|
||||
)
|
||||
|
||||
if wheel.hit {
|
||||
rl.DrawSphereWires(wheel.hit_point, 0.1, 4, 4, rl.RED)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -19,8 +19,49 @@ body_local_to_world :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl
|
||||
return body.x + lg.quaternion_mul_vector3(body.q, pos)
|
||||
}
|
||||
|
||||
body_local_to_world_vec :: #force_inline proc(body: Body_Ptr, vec: rl.Vector3) -> rl.Vector3 {
|
||||
return lg.quaternion_mul_vector3(body.q, vec)
|
||||
}
|
||||
|
||||
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)
|
||||
}
|
||||
|
||||
body_world_to_local_vec :: #force_inline proc(body: Body_Ptr, vec: rl.Vector3) -> rl.Vector3 {
|
||||
inv_q := lg.quaternion_inverse(body.q)
|
||||
return lg.quaternion_mul_vector3(inv_q, vec)
|
||||
}
|
||||
|
||||
body_angular_velocity_at_local_point :: #force_inline proc(
|
||||
body: Body_Ptr,
|
||||
rel_pos: rl.Vector3,
|
||||
) -> rl.Vector3 {
|
||||
return lg.cross(body.w, rel_pos)
|
||||
}
|
||||
|
||||
body_velocity_at_local_point :: #force_inline proc(
|
||||
body: Body_Ptr,
|
||||
rel_pos: rl.Vector3,
|
||||
) -> rl.Vector3 {
|
||||
return body.v + body_angular_velocity_at_local_point(body, rel_pos)
|
||||
}
|
||||
|
||||
wheel_get_rel_wheel_pos :: #force_inline proc(
|
||||
body: Body_Ptr,
|
||||
wheel: Suspension_Constraint_Ptr,
|
||||
) -> rl.Vector3 {
|
||||
t := wheel.hit_t > 0 ? wheel.hit_t : wheel.rest
|
||||
return wheel.rel_pos + wheel.rel_dir * (t - wheel.radius)
|
||||
}
|
||||
|
||||
wheel_get_right_vec :: #force_inline proc(
|
||||
body: Body_Ptr,
|
||||
wheel: Suspension_Constraint_Ptr,
|
||||
) -> rl.Vector3 {
|
||||
local_right := lg.quaternion_mul_vector3(
|
||||
lg.quaternion_angle_axis(wheel.turn_angle, rl.Vector3{0, 1, 0}),
|
||||
rl.Vector3{1, 0, 0},
|
||||
)
|
||||
return body_local_to_world_vec(body, local_right)
|
||||
}
|
||||
|
||||
@ -13,12 +13,15 @@ Body_Config :: struct {
|
||||
inertia_tensor: rl.Vector3,
|
||||
}
|
||||
|
||||
// TODO: rename to wheel
|
||||
Suspension_Constraint_Config :: struct {
|
||||
rel_pos: rl.Vector3,
|
||||
rel_dir: rl.Vector3,
|
||||
body: Body_Handle,
|
||||
rest: f32,
|
||||
compliance: f32,
|
||||
damping: f32,
|
||||
radius: f32,
|
||||
}
|
||||
|
||||
initialize_body_from_config :: proc(body: ^Body, config: Body_Config) {
|
||||
@ -44,6 +47,8 @@ update_suspension_constraint_from_config :: proc(
|
||||
constraint.body = config.body
|
||||
constraint.rest = config.rest
|
||||
constraint.compliance = config.compliance
|
||||
constraint.damping = config.damping
|
||||
constraint.radius = config.radius
|
||||
}
|
||||
|
||||
immediate_body :: proc(
|
||||
|
||||
@ -33,26 +33,34 @@ Body :: struct {
|
||||
}
|
||||
|
||||
Suspension_Constraint :: struct {
|
||||
alive: bool,
|
||||
alive: bool,
|
||||
// Pos relative to the body
|
||||
rel_pos: rl.Vector3,
|
||||
rel_pos: rl.Vector3,
|
||||
// Dir relative to the body
|
||||
rel_dir: rl.Vector3,
|
||||
rel_dir: rl.Vector3,
|
||||
// Handle of the rigid body
|
||||
body: Body_Handle,
|
||||
body: Body_Handle,
|
||||
// Wheel radius
|
||||
radius: f32,
|
||||
// Rest distance
|
||||
rest: f32,
|
||||
rest: f32,
|
||||
// Inverse stiffness
|
||||
compliance: f32,
|
||||
compliance: f32,
|
||||
// How much to damp velocity of the spring
|
||||
damping: f32,
|
||||
|
||||
// Runtime state
|
||||
hit: bool,
|
||||
hit_point: rl.Vector3,
|
||||
hit: bool,
|
||||
hit_point: rl.Vector3,
|
||||
// rel_hit_point = rel_pos + rel_dir * hit_t
|
||||
hit_t: f32,
|
||||
hit_t: f32,
|
||||
turn_angle: f32,
|
||||
drive_impulse: f32,
|
||||
brake_impulse: f32,
|
||||
applied_impulse: rl.Vector3,
|
||||
|
||||
// Free list
|
||||
next_plus_one: i32,
|
||||
next_plus_one: i32,
|
||||
}
|
||||
|
||||
// Index plus one, so handle 0 maps to invalid body
|
||||
@ -98,7 +106,7 @@ add_body :: proc(scene: ^Scene, body: Body) -> Body_Handle {
|
||||
body_copy.alive = true
|
||||
body_copy.next_plus_one = 0
|
||||
|
||||
if scene.first_free_body_plus_one > 1 {
|
||||
if scene.first_free_body_plus_one > 0 {
|
||||
index := scene.first_free_body_plus_one
|
||||
new_body := get_body(scene, Body_Handle(index))
|
||||
next_plus_one := new_body.next_plus_one
|
||||
|
||||
@ -1,16 +1,19 @@
|
||||
package physics
|
||||
|
||||
import "collision"
|
||||
import "core:fmt"
|
||||
import "core:math"
|
||||
import lg "core:math/linalg"
|
||||
import rl "vendor:raylib"
|
||||
|
||||
_ :: math
|
||||
_ :: fmt
|
||||
|
||||
Solver_Config :: struct {
|
||||
// Will automatically do fixed timestep
|
||||
timestep: f32,
|
||||
gravity: rl.Vector3,
|
||||
timestep: f32,
|
||||
gravity: rl.Vector3,
|
||||
substreps_minus_one: i32,
|
||||
}
|
||||
|
||||
Solver_State :: struct {
|
||||
@ -25,16 +28,17 @@ Solver_State :: struct {
|
||||
immediate_suspension_constraints: map[u32]Immedate_State(Suspension_Constraint_Handle),
|
||||
}
|
||||
|
||||
destroy_solver_state :: proc(state: ^Solver_State) {
|
||||
delete(state.immedate_bodies)
|
||||
delete(state.immediate_suspension_constraints)
|
||||
}
|
||||
|
||||
Immedate_State :: struct($T: typeid) {
|
||||
handle: T,
|
||||
// When was this referenced last time (frame number)
|
||||
last_ref: u32,
|
||||
}
|
||||
|
||||
destroy_solver_state :: proc(state: ^Solver_State) {
|
||||
delete(state.immedate_bodies)
|
||||
}
|
||||
|
||||
// Outer simulation loop for fixed timestepping
|
||||
simulate :: proc(scene: ^Scene, state: ^Solver_State, config: Solver_Config, dt: f32) {
|
||||
assert(config.timestep > 0)
|
||||
@ -62,81 +66,147 @@ Body_Sim_State :: struct {
|
||||
}
|
||||
|
||||
simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
body_states := make_soa(#soa[]Body_Sim_State, len(scene.bodies), context.temp_allocator)
|
||||
body_states := make([]Body_Sim_State, len(scene.bodies), context.temp_allocator)
|
||||
|
||||
dt := config.timestep
|
||||
substeps := config.substreps_minus_one + 1
|
||||
|
||||
dt := config.timestep / f32(substeps)
|
||||
inv_dt := 1.0 / dt
|
||||
|
||||
// Integrate positions and rotations
|
||||
for &body, i in scene.bodies {
|
||||
if body.alive {
|
||||
body_states[i].prev_x = body.x
|
||||
body.v += dt * config.gravity
|
||||
body.x += dt * body.v
|
||||
for _ in 0 ..< substeps {
|
||||
// Integrate positions and rotations
|
||||
for &body, i in scene.bodies {
|
||||
if body.alive {
|
||||
body_states[i].prev_x = body.x
|
||||
body.v += dt * config.gravity
|
||||
body.x += dt * body.v
|
||||
|
||||
body_states[i].prev_q = body.q
|
||||
body_states[i].prev_q = body.q
|
||||
|
||||
// TODO: Probably can do it using built in quaternion math but I have no idea how it works
|
||||
// NOTE: figure out how this works https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/
|
||||
q := body.q
|
||||
delta_rot := quaternion(x = body.w.x, y = body.w.y, z = body.w.z, w = 0)
|
||||
delta_rot = delta_rot * q
|
||||
q.x += 0.5 * dt * delta_rot.x
|
||||
q.y += 0.5 * dt * delta_rot.y
|
||||
q.z += 0.5 * dt * delta_rot.z
|
||||
q.w += 0.5 * dt * delta_rot.w
|
||||
q = lg.normalize0(q)
|
||||
// TODO: Probably can do it using built in quaternion math but I have no idea how it works
|
||||
// NOTE: figure out how this works https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/
|
||||
q := body.q
|
||||
delta_rot := quaternion(x = body.w.x, y = body.w.y, z = body.w.z, w = 0)
|
||||
delta_rot = delta_rot * q
|
||||
q.x += 0.5 * dt * delta_rot.x
|
||||
q.y += 0.5 * dt * delta_rot.y
|
||||
q.z += 0.5 * dt * delta_rot.z
|
||||
q.w += 0.5 * dt * delta_rot.w
|
||||
q = lg.normalize0(q)
|
||||
|
||||
body.q = q
|
||||
body.q = q
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for &v in scene.suspension_constraints {
|
||||
if v.alive {
|
||||
body := get_body(scene, v.body)
|
||||
for &v in scene.suspension_constraints {
|
||||
if v.alive {
|
||||
body := get_body(scene, v.body)
|
||||
|
||||
q := body.q
|
||||
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,
|
||||
q := body.q
|
||||
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,
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
solve_velocities(scene, body_states, inv_dt)
|
||||
|
||||
// Solve suspension velocity
|
||||
for _, i in scene.suspension_constraints {
|
||||
v := &scene.suspension_constraints_slice[i]
|
||||
if v.alive {
|
||||
body := get_body(scene, v.body)
|
||||
|
||||
if body.alive && v.hit {
|
||||
wheel_world_pos := body_local_to_world(body, v.rel_pos)
|
||||
body_state := body_states[i32(v.body) - 1]
|
||||
|
||||
// Spring damping
|
||||
{
|
||||
dir := body_local_to_world_vec(body, v.rel_dir)
|
||||
vel_3d := body_velocity_at_local_point(body, v.rel_pos)
|
||||
|
||||
vel := lg.dot(vel_3d, dir)
|
||||
damp_delta := -vel * v.damping * dt * dir
|
||||
|
||||
apply_correction(body, damp_delta, wheel_world_pos)
|
||||
body_solve_velocity(body, body_state, inv_dt)
|
||||
}
|
||||
|
||||
// Drive forces
|
||||
{
|
||||
total_impulse := v.drive_impulse - v.brake_impulse
|
||||
forward := body_local_to_world_vec(body, rl.Vector3{0, 0, 1})
|
||||
|
||||
corr := total_impulse * forward * dt
|
||||
|
||||
apply_correction(body, corr, wheel_world_pos)
|
||||
body_solve_velocity(body, body_state, inv_dt)
|
||||
}
|
||||
|
||||
// Lateral friction
|
||||
{
|
||||
local_contact_pos := v.hit_point - body.x
|
||||
vel_contact := body_velocity_at_local_point(body, local_contact_pos)
|
||||
right := wheel_get_right_vec(body, v)
|
||||
|
||||
lateral_vel := lg.dot(right, vel_contact)
|
||||
|
||||
friction := f32(0.7)
|
||||
impulse := -lateral_vel * friction
|
||||
corr := right * impulse * dt
|
||||
v.applied_impulse.x = impulse
|
||||
|
||||
apply_correction(body, corr, v.hit_point)
|
||||
body_solve_velocity(body, body_state, inv_dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
solve_velocities :: proc(scene: ^Scene, body_states: []Body_Sim_State, inv_dt: f32) {
|
||||
// Compute new linear and angular velocities
|
||||
for &body, i in scene.bodies {
|
||||
for _, i in scene.bodies_slice {
|
||||
body := &scene.bodies_slice[i]
|
||||
if body.alive {
|
||||
body.v = (body.x - body_states[i].prev_x) * inv_dt
|
||||
|
||||
delta_q := body.q * lg.quaternion_inverse(body_states[i].prev_q)
|
||||
body.w = rl.Vector3{delta_q.x, delta_q.y, delta_q.z} * 2.0 * inv_dt
|
||||
|
||||
if delta_q.w < 0 {
|
||||
body.w = -body.w
|
||||
}
|
||||
body_solve_velocity(body, body_states[i], inv_dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
body_solve_velocity :: #force_inline proc(body: Body_Ptr, state: Body_Sim_State, inv_dt: f32) {
|
||||
body.v = (body.x - state.prev_x) * inv_dt
|
||||
|
||||
delta_q := body.q * lg.quaternion_inverse(state.prev_q)
|
||||
body.w = rl.Vector3{delta_q.x, delta_q.y, delta_q.z} * 2.0 * inv_dt
|
||||
|
||||
if delta_q.w < 0 {
|
||||
body.w = -body.w
|
||||
}
|
||||
}
|
||||
|
||||
apply_constraint_correction_unilateral :: proc(
|
||||
dt: f32,
|
||||
body: Body_Ptr,
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user