Fixed unstable suspension (feel like an idiot)
This commit is contained in:
parent
7a77d1c409
commit
e22e667e27
24
game/debug/helpers.odin
Normal file
24
game/debug/helpers.odin
Normal file
@ -0,0 +1,24 @@
|
||||
package debug
|
||||
|
||||
import rl "vendor:raylib"
|
||||
|
||||
int_to_color :: proc(num: i32) -> (color: rl.Color) {
|
||||
x := int_hash(num)
|
||||
|
||||
color.r = u8(x % 256)
|
||||
color.g = u8((x / 256) % 256)
|
||||
color.b = u8((x / 256 / 256) % 256)
|
||||
color.a = 255
|
||||
return
|
||||
}
|
||||
|
||||
int_hash :: proc(num: i32) -> u32 {
|
||||
x := cast(u32)num
|
||||
|
||||
x = ((x >> 16) ~ x) * 0x45d9f3b
|
||||
x = ((x >> 16) ~ x) * 0x45d9f3b
|
||||
x = (x >> 16) ~ x
|
||||
|
||||
return x
|
||||
}
|
||||
|
||||
@ -64,7 +64,7 @@ Car :: struct {
|
||||
SOLVER_CONFIG :: physics.Solver_Config {
|
||||
timestep = 1.0 / 120,
|
||||
gravity = rl.Vector3{0, -9.8, 0},
|
||||
substreps_minus_one = 8 - 1,
|
||||
substreps_minus_one = 4 - 1,
|
||||
}
|
||||
|
||||
Game_Memory :: struct {
|
||||
@ -235,18 +235,31 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
#hash("car", "fnv32a"),
|
||||
physics.Body_Config {
|
||||
initial_pos = {0, 2, 0},
|
||||
initial_rot = linalg.QUATERNIONF32_IDENTITY,
|
||||
initial_rot = linalg.quaternion_angle_axis(
|
||||
math.RAD_PER_DEG * 100,
|
||||
rl.Vector3{0, 1, 0},
|
||||
),
|
||||
initial_ang_vel = {0, 0, 0},
|
||||
shape = physics.Shape_Box{size = car_bounds.max - car_bounds.min},
|
||||
mass = 100,
|
||||
inertia_tensor = physics.inertia_tensor_box(car_bounds.max - car_bounds.min),
|
||||
},
|
||||
)
|
||||
|
||||
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
|
||||
car_body := physics.get_body(&world.physics_scene, runtime_world.car_handle)
|
||||
|
||||
camera := &runtime_world.camera
|
||||
|
||||
camera.up = rl.Vector3{0, 1, 0}
|
||||
camera.fovy = 60
|
||||
camera.projection = .PERSPECTIVE
|
||||
camera.position = physics.body_local_to_world(
|
||||
car_body,
|
||||
physics.body_world_to_local(
|
||||
car_body,
|
||||
physics.body_local_to_world(car_body, rl.Vector3{1, 0, -2}),
|
||||
),
|
||||
)
|
||||
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}
|
||||
}
|
||||
@ -257,7 +270,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
rest := f32(1)
|
||||
suspension_stiffness := f32(2000)
|
||||
compliance := 1.0 / suspension_stiffness
|
||||
damping := f32(0.1)
|
||||
damping := f32(0.01)
|
||||
radius := f32(0.6)
|
||||
|
||||
wheel_fl := physics.immediate_suspension_constraint(
|
||||
@ -322,7 +335,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
|
||||
DRIVE_IMPULSE :: 1
|
||||
BRAKE_IMPULSE :: 2
|
||||
TURN_ANGLE :: -f32(10) * math.RAD_PER_DEG
|
||||
TURN_ANGLE :: -f32(30) * math.RAD_PER_DEG
|
||||
|
||||
for wheel_handle in drive_wheels {
|
||||
wheel := physics.get_suspension_constraint(&world.physics_scene, wheel_handle)
|
||||
@ -469,6 +482,8 @@ draw :: proc() {
|
||||
|
||||
rl.DrawGrid(100, 1)
|
||||
|
||||
physics.draw_debug_scene(&world.physics_scene)
|
||||
|
||||
{
|
||||
mesh_bvh := assets.get_bvh(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
|
||||
|
||||
@ -505,15 +520,13 @@ draw :: proc() {
|
||||
car_matrix := rl.QuaternionToMatrix(car_body.q)
|
||||
car_model.transform = car_matrix
|
||||
|
||||
rl.DrawModel(car_model, car_body.x - runtime_world.car_com, 1, rl.WHITE)
|
||||
rl.DrawModel(car_model, car_body.x + runtime_world.car_com, 1, rl.WHITE)
|
||||
} else {
|
||||
if g_mem.draw_car {
|
||||
rl.DrawModel(car_model, 0, 1, rl.WHITE)
|
||||
}
|
||||
}
|
||||
|
||||
physics.draw_debug_scene(&world.physics_scene)
|
||||
|
||||
{
|
||||
// Debug draw spline road
|
||||
{
|
||||
@ -596,8 +609,20 @@ draw :: proc() {
|
||||
)
|
||||
}
|
||||
} else {
|
||||
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)
|
||||
car := physics.get_body(&world.physics_scene, runtime_world.car_handle)
|
||||
rl.DrawText(
|
||||
fmt.ctprintf(
|
||||
"p: %v\nv: %v\nw: %v\ng: %v",
|
||||
car.x,
|
||||
car.v,
|
||||
car.w,
|
||||
SOLVER_CONFIG.gravity,
|
||||
),
|
||||
5,
|
||||
32,
|
||||
8,
|
||||
rl.ORANGE,
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -7,6 +7,7 @@ import "core:log"
|
||||
import "core:math"
|
||||
import lg "core:math/linalg"
|
||||
import "core:mem"
|
||||
import "game:debug"
|
||||
import rl "vendor:raylib"
|
||||
|
||||
_ :: log
|
||||
@ -285,7 +286,7 @@ internal_traverse_bvh_ray_triangles :: proc(
|
||||
|
||||
rl.DrawBoundingBox(
|
||||
{min = node.aabb.min, max = node.aabb.max},
|
||||
debug_int_to_color(node_idx),
|
||||
debug.int_to_color(node_idx),
|
||||
)
|
||||
|
||||
if is_leaf_node(node^) {
|
||||
@ -312,9 +313,9 @@ internal_ray_tri_test :: proc(ray: Ray, mesh: Mesh, tri: u16, col: ^Collision) {
|
||||
v1, v2, v3 := mesh.vertices[i1], mesh.vertices[i2], mesh.vertices[i3]
|
||||
|
||||
col.triangle_tests += 1
|
||||
rl.DrawTriangle3D(v1, v2, v3, debug_int_to_color(i32(tri) + 1))
|
||||
rl.DrawTriangle3D(v1, v2, v3, debug.int_to_color(i32(tri) + 1))
|
||||
|
||||
t, _, barycentric, ok := collision.intersect_segment_triangle(
|
||||
t, _, barycentric, ok := collision.intersect_ray_triangle(
|
||||
{ray.origin, ray.origin + ray.dir},
|
||||
{v1, v2, v3},
|
||||
)
|
||||
|
||||
@ -5,6 +5,7 @@ import "core:container/queue"
|
||||
import "core:fmt"
|
||||
import "core:log"
|
||||
import lg "core:math/linalg"
|
||||
import "game:debug"
|
||||
import rl "vendor:raylib"
|
||||
import "vendor:raylib/rlgl"
|
||||
|
||||
@ -40,7 +41,7 @@ debug_draw_bvh_bounds :: proc(bvh: ^BVH, mesh: Mesh, pos: rl.Vector3, node_index
|
||||
if should_draw {
|
||||
rl.DrawBoundingBox(
|
||||
rl.BoundingBox{node.aabb.min + pos, node.aabb.max + pos},
|
||||
debug_int_to_color(node_idx + 1),
|
||||
debug.int_to_color(node_idx + 1),
|
||||
)
|
||||
}
|
||||
|
||||
@ -73,40 +74,20 @@ debug_draw_bvh_bounds :: proc(bvh: ^BVH, mesh: Mesh, pos: rl.Vector3, node_index
|
||||
}
|
||||
|
||||
size := lg.length(aabb.max - aabb.min)
|
||||
rl.DrawTriangle3D(v1, v2, v3, debug_int_to_color(i32(tri) + 1))
|
||||
rl.DrawTriangle3D(v1, v2, v3, debug.int_to_color(i32(tri) + 1))
|
||||
rl.DrawBoundingBox(
|
||||
rl.BoundingBox{aabb.min, aabb.max},
|
||||
debug_int_to_color(i32(tri) + 2),
|
||||
debug.int_to_color(i32(tri) + 2),
|
||||
)
|
||||
|
||||
if size < 1 {
|
||||
rl.DrawCubeWiresV(centroid, 0.05, debug_int_to_color(i32(tri) + 3))
|
||||
rl.DrawCubeWiresV(centroid, 0.05, debug.int_to_color(i32(tri) + 3))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
debug_int_to_color :: proc(num: i32) -> (color: rl.Color) {
|
||||
x := debug_hash(num)
|
||||
|
||||
color.r = u8(x % 256)
|
||||
color.g = u8((x / 256) % 256)
|
||||
color.b = u8((x / 256 / 256) % 256)
|
||||
color.a = 255
|
||||
return
|
||||
}
|
||||
|
||||
debug_hash :: proc(num: i32) -> u32 {
|
||||
x := cast(u32)num
|
||||
|
||||
x = ((x >> 16) ~ x) * 0x45d9f3b
|
||||
x = ((x >> 16) ~ x) * 0x45d9f3b
|
||||
x = (x >> 16) ~ x
|
||||
|
||||
return x
|
||||
}
|
||||
|
||||
bvh_mesh_from_rl_mesh :: proc(mesh: rl.Mesh) -> Mesh {
|
||||
return Mesh {
|
||||
vertices = (cast([^]Vec3)mesh.vertices)[:mesh.vertexCount],
|
||||
|
||||
@ -567,7 +567,7 @@ intersect_ray_polyhedron :: proc(
|
||||
return t, true
|
||||
}
|
||||
|
||||
intersect_segment_triangle :: proc(
|
||||
intersect_ray_triangle :: proc(
|
||||
segment: [2]Vec3,
|
||||
triangle: [3]Vec3,
|
||||
) -> (
|
||||
@ -633,7 +633,7 @@ intersect_segment_plane :: proc(
|
||||
return t, point, true
|
||||
}
|
||||
|
||||
return t, segment[0], false
|
||||
return linalg.length(ab), segment[1], false
|
||||
}
|
||||
|
||||
// TODO: alternative with capsule endcaps
|
||||
|
||||
@ -3,10 +3,38 @@ package physics
|
||||
import "core:log"
|
||||
import "core:math"
|
||||
import lg "core:math/linalg"
|
||||
import "game:debug"
|
||||
import rl "vendor:raylib"
|
||||
import "vendor:raylib/rlgl"
|
||||
|
||||
_ :: log
|
||||
_ :: math
|
||||
_ :: debug
|
||||
|
||||
draw_debug_shape :: proc(
|
||||
shape: Collision_Shape,
|
||||
pos: rl.Vector3,
|
||||
rot: rl.Quaternion,
|
||||
color: rl.Color,
|
||||
) {
|
||||
rlgl.PushMatrix()
|
||||
defer rlgl.PopMatrix()
|
||||
|
||||
rlgl.Begin(rlgl.LINES)
|
||||
defer rlgl.End()
|
||||
|
||||
switch s in shape {
|
||||
case Shape_Sphere:
|
||||
rl.DrawSphere(pos, s.radius, color)
|
||||
case Shape_Box:
|
||||
mat := lg.matrix4_from_trs(pos, rot, 1)
|
||||
|
||||
rlgl.LoadIdentity()
|
||||
rlgl.MultMatrixf(cast([^]f32)&mat)
|
||||
|
||||
rl.DrawCubeV(0, s.size, color)
|
||||
}
|
||||
}
|
||||
|
||||
draw_debug_scene :: proc(scene: ^Scene) {
|
||||
for &body in scene.bodies {
|
||||
@ -21,6 +49,8 @@ draw_debug_scene :: proc(scene: ^Scene) {
|
||||
rl.DrawLine3D(pos, pos + x, rl.RED)
|
||||
rl.DrawLine3D(pos, pos + y, rl.GREEN)
|
||||
rl.DrawLine3D(pos, pos + z, rl.BLUE)
|
||||
|
||||
// draw_debug_shape(body.shape, body.x, body.q, debug.int_to_color(i32(i + 1)))
|
||||
}
|
||||
}
|
||||
|
||||
@ -50,11 +80,15 @@ draw_debug_scene :: proc(scene: ^Scene) {
|
||||
rl.RED,
|
||||
)
|
||||
|
||||
rl.DrawLine3D(
|
||||
pos + t * dir,
|
||||
pos + t * dir + wheel.applied_impulse.x * right * 10,
|
||||
rl.RED,
|
||||
)
|
||||
rl.DrawLine3D(wheel_pos, wheel_pos + right * 10, rl.RED)
|
||||
|
||||
if wheel.hit {
|
||||
// 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)
|
||||
|
||||
@ -3,6 +3,12 @@ package physics
|
||||
import lg "core:math/linalg"
|
||||
import rl "vendor:raylib"
|
||||
|
||||
inertia_tensor_sphere :: proc(radius: f32) -> (tensor: rl.Vector3) {
|
||||
tensor = radius * radius * (2.0 / 3.0)
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: rl.Vector3) {
|
||||
CONSTANT :: f32(1.0 / 12.0)
|
||||
|
||||
@ -15,22 +21,33 @@ inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: rl.Vector3) {
|
||||
return
|
||||
}
|
||||
|
||||
inertia_tensor_collision_shape :: proc(shape: Collision_Shape) -> (tensor: rl.Vector3) {
|
||||
switch s in shape {
|
||||
case Shape_Sphere:
|
||||
tensor = inertia_tensor_sphere(s.radius)
|
||||
case Shape_Box:
|
||||
tensor = inertia_tensor_box(s.size)
|
||||
}
|
||||
|
||||
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_local_to_world_vec :: #force_inline proc(body: Body_Ptr, vec: rl.Vector3) -> rl.Vector3 {
|
||||
return lg.quaternion_mul_vector3(body.q, vec)
|
||||
return lg.normalize0(lg.quaternion_mul_vector3(body.q, vec))
|
||||
}
|
||||
|
||||
body_world_to_local :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
|
||||
inv_q := lg.quaternion_inverse(body.q)
|
||||
inv_q := lg.quaternion_normalize0(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)
|
||||
return lg.normalize0(lg.quaternion_mul_vector3(inv_q, vec))
|
||||
}
|
||||
|
||||
body_angular_velocity_at_local_point :: #force_inline proc(
|
||||
|
||||
@ -1,14 +1,22 @@
|
||||
package physics
|
||||
|
||||
import lg "core:math/linalg"
|
||||
import rl "vendor:raylib"
|
||||
|
||||
Body_Config_Inertia_Mode :: enum {
|
||||
From_Shape,
|
||||
Explicit,
|
||||
}
|
||||
|
||||
// Immediate mode stuff for testing
|
||||
Body_Config :: struct {
|
||||
initial_pos: rl.Vector3,
|
||||
initial_rot: rl.Quaternion,
|
||||
initial_vel: rl.Vector3,
|
||||
initial_ang_vel: rl.Vector3,
|
||||
shape: Collision_Shape,
|
||||
mass: f32,
|
||||
inertia_mode: Body_Config_Inertia_Mode,
|
||||
// Unit inertia tensor
|
||||
inertia_tensor: rl.Vector3,
|
||||
}
|
||||
@ -24,18 +32,40 @@ Suspension_Constraint_Config :: struct {
|
||||
radius: f32,
|
||||
}
|
||||
|
||||
calculate_body_params_from_config :: proc(
|
||||
config: Body_Config,
|
||||
) -> (
|
||||
inv_mass: f32,
|
||||
inv_inertia_tensor: rl.Vector3,
|
||||
) {
|
||||
inv_mass = config.mass == 0 ? 0 : 1.0 / config.mass
|
||||
|
||||
inertia_tensor: rl.Vector3
|
||||
if config.inertia_mode == .Explicit {
|
||||
inertia_tensor = config.inertia_tensor
|
||||
} else {
|
||||
inertia_tensor = inertia_tensor_collision_shape(config.shape)
|
||||
}
|
||||
inertia_tensor *= config.mass
|
||||
|
||||
inv_inertia_tensor = lg.all(lg.equal(inertia_tensor, rl.Vector3(0))) ? 0 : 1.0 / inertia_tensor
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
initialize_body_from_config :: proc(body: ^Body, config: Body_Config) {
|
||||
body.x = config.initial_pos
|
||||
body.q = config.initial_rot
|
||||
body.v = config.initial_vel
|
||||
body.w = config.initial_ang_vel
|
||||
body.inv_mass = 1.0 / config.mass
|
||||
body.inv_inertia_tensor = 1.0 / (config.inertia_tensor * config.mass)
|
||||
body.shape = config.shape
|
||||
|
||||
body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config)
|
||||
}
|
||||
|
||||
update_body_from_config :: proc(body: Body_Ptr, config: Body_Config) {
|
||||
body.inv_mass = 1.0 / config.mass
|
||||
body.inv_inertia_tensor = 1.0 / (config.inertia_tensor * config.mass)
|
||||
body.shape = config.shape
|
||||
body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config)
|
||||
}
|
||||
|
||||
update_suspension_constraint_from_config :: proc(
|
||||
@ -66,6 +96,7 @@ immediate_body :: proc(
|
||||
state.num_referenced_bodies += 1
|
||||
}
|
||||
handle = body.handle
|
||||
update_body_from_config(get_body(scene, handle), config)
|
||||
} else {
|
||||
new_body: Body
|
||||
state.num_referenced_bodies += 1
|
||||
|
||||
@ -28,10 +28,24 @@ Body :: struct {
|
||||
inv_mass: f32,
|
||||
// Moment of inertia
|
||||
inv_inertia_tensor: rl.Vector3,
|
||||
shape: Collision_Shape,
|
||||
//
|
||||
next_plus_one: i32,
|
||||
}
|
||||
|
||||
Shape_Sphere :: struct {
|
||||
radius: f32,
|
||||
}
|
||||
|
||||
Shape_Box :: struct {
|
||||
size: rl.Vector3,
|
||||
}
|
||||
|
||||
Collision_Shape :: union {
|
||||
Shape_Sphere,
|
||||
Shape_Box,
|
||||
}
|
||||
|
||||
Suspension_Constraint :: struct {
|
||||
alive: bool,
|
||||
// Pos relative to the body
|
||||
|
||||
@ -48,7 +48,7 @@ simulate :: proc(scene: ^Scene, state: ^Solver_State, config: Solver_Config, dt:
|
||||
state.accumulated_time += dt
|
||||
|
||||
num_steps := 0
|
||||
for state.accumulated_time >= config.timestep {
|
||||
for state.accumulated_time > config.timestep {
|
||||
num_steps += 1
|
||||
state.accumulated_time -= config.timestep
|
||||
|
||||
@ -78,15 +78,17 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
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.v += config.gravity * dt
|
||||
body.x += body.v * dt
|
||||
|
||||
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)
|
||||
|
||||
w := body.w
|
||||
delta_rot := quaternion(x = w.x, y = w.y, z = 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
|
||||
@ -102,15 +104,13 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
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)
|
||||
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 := v.hit_point - pos
|
||||
distance := lg.length(corr)
|
||||
@ -139,40 +139,56 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
|
||||
if body.alive && v.hit {
|
||||
wheel_world_pos := body_local_to_world(body, v.rel_pos)
|
||||
dir := body_local_to_world_vec(body, v.rel_dir)
|
||||
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)
|
||||
if true {
|
||||
vel_3d := body_velocity_at_local_point(body, wheel_world_pos - body.x)
|
||||
|
||||
vel := lg.dot(vel_3d, dir)
|
||||
damp_delta := -vel * v.damping * dt * dir
|
||||
damping := -vel * min(v.damping * dt, 1)
|
||||
|
||||
apply_constraint_correction_unilateral(
|
||||
dt,
|
||||
body,
|
||||
0,
|
||||
error = damping,
|
||||
error_gradient = dir,
|
||||
pos = wheel_world_pos,
|
||||
)
|
||||
|
||||
apply_correction(body, damp_delta, wheel_world_pos)
|
||||
body_solve_velocity(body, body_state, inv_dt)
|
||||
}
|
||||
|
||||
// Drive forces
|
||||
{
|
||||
if true {
|
||||
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_constraint_correction_unilateral(
|
||||
dt,
|
||||
body,
|
||||
0,
|
||||
total_impulse * dt * dt,
|
||||
forward,
|
||||
wheel_world_pos,
|
||||
)
|
||||
apply_correction(body, corr, wheel_world_pos)
|
||||
body_solve_velocity(body, body_state, inv_dt)
|
||||
}
|
||||
|
||||
// Lateral friction
|
||||
{
|
||||
if true {
|
||||
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)
|
||||
friction := f32(0.5)
|
||||
impulse := -lateral_vel * friction
|
||||
corr := right * impulse * dt
|
||||
v.applied_impulse.x = impulse
|
||||
@ -239,7 +255,7 @@ 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)
|
||||
inv_q := lg.quaternion_normalize0(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)
|
||||
@ -259,14 +275,17 @@ apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) {
|
||||
|
||||
get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: rl.Vector3) -> f32 {
|
||||
q := body.q
|
||||
inv_q := lg.quaternion_inverse(q)
|
||||
inv_q := lg.quaternion_normalize0(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 :=
|
||||
rn.x * rn.x * body.inv_inertia_tensor.x +
|
||||
rn.y * rn.y * body.inv_inertia_tensor.y +
|
||||
rn.z * rn.z * body.inv_inertia_tensor.z
|
||||
|
||||
w += body.inv_mass
|
||||
|
||||
return w
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user