Remove unnecessary rl dependency in physics engine
This commit is contained in:
parent
c2b831a4d6
commit
c91018fec8
@ -16,8 +16,8 @@ _ :: debug
|
|||||||
draw_debug_shape :: proc(
|
draw_debug_shape :: proc(
|
||||||
sim_state: ^Sim_State,
|
sim_state: ^Sim_State,
|
||||||
shape: Collision_Shape,
|
shape: Collision_Shape,
|
||||||
pos: rl.Vector3,
|
pos: Vec3,
|
||||||
rot: rl.Quaternion,
|
rot: Quat,
|
||||||
color: rl.Color,
|
color: rl.Color,
|
||||||
) {
|
) {
|
||||||
mat := lg.matrix4_from_trs(pos, rot, 1)
|
mat := lg.matrix4_from_trs(pos, rot, 1)
|
||||||
@ -48,9 +48,9 @@ draw_debug_scene :: proc(scene: ^Scene) {
|
|||||||
pos := body.x
|
pos := body.x
|
||||||
|
|
||||||
q := body.q
|
q := body.q
|
||||||
x := lg.quaternion_mul_vector3(q, rl.Vector3{1, 0, 0})
|
x := lg.quaternion_mul_vector3(q, Vec3{1, 0, 0})
|
||||||
y := lg.quaternion_mul_vector3(q, rl.Vector3{0, 1, 0})
|
y := lg.quaternion_mul_vector3(q, Vec3{0, 1, 0})
|
||||||
z := lg.quaternion_mul_vector3(q, rl.Vector3{0, 0, 1})
|
z := lg.quaternion_mul_vector3(q, Vec3{0, 0, 1})
|
||||||
|
|
||||||
rl.DrawLine3D(pos, pos + x, rl.RED)
|
rl.DrawLine3D(pos, pos + x, rl.RED)
|
||||||
rl.DrawLine3D(pos, pos + y, rl.GREEN)
|
rl.DrawLine3D(pos, pos + y, rl.GREEN)
|
||||||
@ -130,13 +130,13 @@ draw_debug_scene :: proc(scene: ^Scene) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
debug_transform_points_local_to_world :: proc(body: Body_Ptr, points: []rl.Vector3) {
|
debug_transform_points_local_to_world :: proc(body: Body_Ptr, points: []Vec3) {
|
||||||
for i in 0 ..< len(points) {
|
for i in 0 ..< len(points) {
|
||||||
points[i] = body_local_to_world(body, points[i])
|
points[i] = body_local_to_world(body, points[i])
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
debug_draw_manifold_points :: proc(normal: rl.Vector3, points: []rl.Vector3, color: rl.Color) {
|
debug_draw_manifold_points :: proc(normal: Vec3, points: []Vec3, color: rl.Color) {
|
||||||
if len(points) >= 3 {
|
if len(points) >= 3 {
|
||||||
// Triangle or quad
|
// Triangle or quad
|
||||||
v1 := points[0]
|
v1 := points[0]
|
||||||
|
@ -3,7 +3,6 @@ package physics
|
|||||||
import "collision"
|
import "collision"
|
||||||
import lg "core:math/linalg"
|
import lg "core:math/linalg"
|
||||||
import "game:halfedge"
|
import "game:halfedge"
|
||||||
import rl "vendor:raylib"
|
|
||||||
|
|
||||||
inertia_tensor_sphere :: proc(radius: f32) -> (tensor: Matrix3) {
|
inertia_tensor_sphere :: proc(radius: f32) -> (tensor: Matrix3) {
|
||||||
tensor = radius * radius * (2.0 / 3.0)
|
tensor = radius * radius * (2.0 / 3.0)
|
||||||
@ -11,7 +10,7 @@ inertia_tensor_sphere :: proc(radius: f32) -> (tensor: Matrix3) {
|
|||||||
return
|
return
|
||||||
}
|
}
|
||||||
|
|
||||||
inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: Matrix3) {
|
inertia_tensor_box :: proc(size: Vec3) -> (tensor: Matrix3) {
|
||||||
CONSTANT :: f32(1.0 / 12.0)
|
CONSTANT :: f32(1.0 / 12.0)
|
||||||
|
|
||||||
tensor[0][0] = size.z * size.z + size.y * size.y
|
tensor[0][0] = size.z * size.z + size.y * size.y
|
||||||
@ -35,32 +34,32 @@ inertia_tensor_collision_shape :: proc(shape: Input_Shape) -> (tensor: Matrix3)
|
|||||||
return
|
return
|
||||||
}
|
}
|
||||||
|
|
||||||
body_local_to_world :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
|
body_local_to_world :: #force_inline proc(body: Body_Ptr, pos: Vec3) -> Vec3 {
|
||||||
return body.x + lg.quaternion_mul_vector3(body.q, pos)
|
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 {
|
body_local_to_world_vec :: #force_inline proc(body: Body_Ptr, vec: Vec3) -> Vec3 {
|
||||||
return lg.normalize0(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 {
|
body_world_to_local :: #force_inline proc(body: Body_Ptr, pos: Vec3) -> Vec3 {
|
||||||
inv_q := lg.quaternion_normalize0(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)
|
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 {
|
body_world_to_local_vec :: #force_inline proc(body: Body_Ptr, vec: Vec3) -> Vec3 {
|
||||||
inv_q := lg.quaternion_inverse(body.q)
|
inv_q := lg.quaternion_inverse(body.q)
|
||||||
return lg.normalize0(lg.quaternion_mul_vector3(inv_q, vec))
|
return lg.normalize0(lg.quaternion_mul_vector3(inv_q, vec))
|
||||||
}
|
}
|
||||||
|
|
||||||
body_velocity_at_point :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
|
body_velocity_at_point :: #force_inline proc(body: Body_Ptr, pos: Vec3) -> Vec3 {
|
||||||
return body.v + lg.cross(body.w, pos - body.x)
|
return body.v + lg.cross(body.w, pos - body.x)
|
||||||
}
|
}
|
||||||
|
|
||||||
wheel_get_rel_wheel_pos :: #force_inline proc(
|
wheel_get_rel_wheel_pos :: #force_inline proc(
|
||||||
body: Body_Ptr,
|
body: Body_Ptr,
|
||||||
wheel: Suspension_Constraint_Ptr,
|
wheel: Suspension_Constraint_Ptr,
|
||||||
) -> rl.Vector3 {
|
) -> Vec3 {
|
||||||
t := wheel.hit_t > 0 ? wheel.hit_t : wheel.rest
|
t := wheel.hit_t > 0 ? wheel.hit_t : wheel.rest
|
||||||
return wheel.rel_pos + wheel.rel_dir * (t - wheel.radius)
|
return wheel.rel_pos + wheel.rel_dir * (t - wheel.radius)
|
||||||
}
|
}
|
||||||
@ -68,15 +67,15 @@ wheel_get_rel_wheel_pos :: #force_inline proc(
|
|||||||
wheel_get_right_vec :: #force_inline proc(
|
wheel_get_right_vec :: #force_inline proc(
|
||||||
body: Body_Ptr,
|
body: Body_Ptr,
|
||||||
wheel: Suspension_Constraint_Ptr,
|
wheel: Suspension_Constraint_Ptr,
|
||||||
) -> rl.Vector3 {
|
) -> Vec3 {
|
||||||
local_right := lg.quaternion_mul_vector3(
|
local_right := lg.quaternion_mul_vector3(
|
||||||
lg.quaternion_angle_axis(wheel.turn_angle, rl.Vector3{0, 1, 0}),
|
lg.quaternion_angle_axis(wheel.turn_angle, Vec3{0, 1, 0}),
|
||||||
rl.Vector3{1, 0, 0},
|
Vec3{1, 0, 0},
|
||||||
)
|
)
|
||||||
return body_local_to_world_vec(body, local_right)
|
return body_local_to_world_vec(body, local_right)
|
||||||
}
|
}
|
||||||
|
|
||||||
body_get_shape_offset_local :: proc(body: Body_Ptr) -> (offset: rl.Vector3) {
|
body_get_shape_offset_local :: proc(body: Body_Ptr) -> (offset: Vec3) {
|
||||||
#partial switch s in body.shape {
|
#partial switch s in body.shape {
|
||||||
case Internal_Shape_Convex:
|
case Internal_Shape_Convex:
|
||||||
offset = -s.center_of_mass
|
offset = -s.center_of_mass
|
||||||
@ -86,7 +85,7 @@ body_get_shape_offset_local :: proc(body: Body_Ptr) -> (offset: rl.Vector3) {
|
|||||||
|
|
||||||
// Returns the shape's world position
|
// Returns the shape's world position
|
||||||
// Shape can be offset from COM
|
// Shape can be offset from COM
|
||||||
body_get_shape_pos :: proc(body: Body_Ptr) -> rl.Vector3 {
|
body_get_shape_pos :: proc(body: Body_Ptr) -> Vec3 {
|
||||||
offset := body_get_shape_offset_local(body)
|
offset := body_get_shape_offset_local(body)
|
||||||
return body_local_to_world(body, offset)
|
return body_local_to_world(body, offset)
|
||||||
}
|
}
|
||||||
|
@ -2,10 +2,11 @@ package physics
|
|||||||
|
|
||||||
import "collision"
|
import "collision"
|
||||||
import lg "core:math/linalg"
|
import lg "core:math/linalg"
|
||||||
import rl "vendor:raylib"
|
|
||||||
|
|
||||||
MAX_CONTACTS :: 1024
|
MAX_CONTACTS :: 1024
|
||||||
|
|
||||||
|
Vec3 :: [3]f32
|
||||||
|
Quat :: quaternion128
|
||||||
Matrix3 :: # row_major matrix[3, 3]f32
|
Matrix3 :: # row_major matrix[3, 3]f32
|
||||||
|
|
||||||
Sim_State :: struct {
|
Sim_State :: struct {
|
||||||
@ -37,13 +38,13 @@ Body :: struct {
|
|||||||
// Is this body alive (if not it doesn't exist)
|
// Is this body alive (if not it doesn't exist)
|
||||||
alive: bool,
|
alive: bool,
|
||||||
// Pos
|
// Pos
|
||||||
x: rl.Vector3,
|
x: Vec3,
|
||||||
// Linear vel
|
// Linear vel
|
||||||
v: rl.Vector3,
|
v: Vec3,
|
||||||
// Orientation
|
// Orientation
|
||||||
q: rl.Quaternion,
|
q: Quat,
|
||||||
// Angular vel (omega)
|
// Angular vel (omega)
|
||||||
w: rl.Vector3,
|
w: Vec3,
|
||||||
// Mass
|
// Mass
|
||||||
inv_mass: f32,
|
inv_mass: f32,
|
||||||
// Moment of inertia
|
// Moment of inertia
|
||||||
@ -58,18 +59,18 @@ Shape_Sphere :: struct {
|
|||||||
}
|
}
|
||||||
|
|
||||||
Shape_Box :: struct {
|
Shape_Box :: struct {
|
||||||
size: rl.Vector3,
|
size: Vec3,
|
||||||
}
|
}
|
||||||
|
|
||||||
Internal_Shape_Convex :: struct {
|
Internal_Shape_Convex :: struct {
|
||||||
mesh: Convex_Handle,
|
mesh: Convex_Handle,
|
||||||
center_of_mass: rl.Vector3,
|
center_of_mass: Vec3,
|
||||||
inertia_tensor: Matrix3,
|
inertia_tensor: Matrix3,
|
||||||
}
|
}
|
||||||
|
|
||||||
Shape_Convex :: struct {
|
Shape_Convex :: struct {
|
||||||
mesh: collision.Convex,
|
mesh: collision.Convex,
|
||||||
center_of_mass: rl.Vector3,
|
center_of_mass: Vec3,
|
||||||
inertia_tensor: Matrix3,
|
inertia_tensor: Matrix3,
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -81,9 +82,9 @@ Collision_Shape :: union {
|
|||||||
Suspension_Constraint :: struct {
|
Suspension_Constraint :: struct {
|
||||||
alive: bool,
|
alive: bool,
|
||||||
// Pos relative to the body
|
// Pos relative to the body
|
||||||
rel_pos: rl.Vector3,
|
rel_pos: Vec3,
|
||||||
// Dir relative to the body
|
// Dir relative to the body
|
||||||
rel_dir: rl.Vector3,
|
rel_dir: Vec3,
|
||||||
// Handle of the rigid body
|
// Handle of the rigid body
|
||||||
body: Body_Handle,
|
body: Body_Handle,
|
||||||
// Wheel radius
|
// Wheel radius
|
||||||
@ -97,13 +98,13 @@ Suspension_Constraint :: struct {
|
|||||||
|
|
||||||
// Runtime state
|
// Runtime state
|
||||||
hit: bool,
|
hit: bool,
|
||||||
hit_point: rl.Vector3,
|
hit_point: Vec3,
|
||||||
// rel_hit_point = rel_pos + rel_dir * hit_t
|
// rel_hit_point = rel_pos + rel_dir * hit_t
|
||||||
hit_t: f32,
|
hit_t: f32,
|
||||||
turn_angle: f32,
|
turn_angle: f32,
|
||||||
drive_impulse: f32,
|
drive_impulse: f32,
|
||||||
brake_impulse: f32,
|
brake_impulse: f32,
|
||||||
applied_impulse: rl.Vector3,
|
applied_impulse: Vec3,
|
||||||
|
|
||||||
// Free list
|
// Free list
|
||||||
next_plus_one: i32,
|
next_plus_one: i32,
|
||||||
@ -149,11 +150,14 @@ get_prev_sim_state :: proc(scene: ^Scene) -> ^Sim_State {
|
|||||||
}
|
}
|
||||||
|
|
||||||
get_next_sim_state :: proc(scene: ^Scene) -> ^Sim_State {
|
get_next_sim_state :: proc(scene: ^Scene) -> ^Sim_State {
|
||||||
return &scene.simulation_states[(scene.simulation_state_index + 1) %% i32(len(scene.simulation_states))]
|
return(
|
||||||
|
&scene.simulation_states[(scene.simulation_state_index + 1) %% i32(len(scene.simulation_states))] \
|
||||||
|
)
|
||||||
}
|
}
|
||||||
|
|
||||||
flip_sim_state :: proc(scene: ^Scene) {
|
flip_sim_state :: proc(scene: ^Scene) {
|
||||||
scene.simulation_state_index = (scene.simulation_state_index + 1) %% i32(len(scene.simulation_states))
|
scene.simulation_state_index =
|
||||||
|
(scene.simulation_state_index + 1) %% i32(len(scene.simulation_states))
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Returns pointer to soa slice. NEVER STORE IT
|
/// Returns pointer to soa slice. NEVER STORE IT
|
||||||
@ -180,10 +184,10 @@ Input_Shape :: union {
|
|||||||
}
|
}
|
||||||
|
|
||||||
Body_Config :: struct {
|
Body_Config :: struct {
|
||||||
initial_pos: rl.Vector3,
|
initial_pos: Vec3,
|
||||||
initial_rot: rl.Quaternion,
|
initial_rot: Quat,
|
||||||
initial_vel: rl.Vector3,
|
initial_vel: Vec3,
|
||||||
initial_ang_vel: rl.Vector3,
|
initial_ang_vel: Vec3,
|
||||||
shape: Input_Shape,
|
shape: Input_Shape,
|
||||||
mass: f32,
|
mass: f32,
|
||||||
inertia_mode: Body_Config_Inertia_Mode,
|
inertia_mode: Body_Config_Inertia_Mode,
|
||||||
@ -193,8 +197,8 @@ Body_Config :: struct {
|
|||||||
|
|
||||||
// TODO: rename to wheel
|
// TODO: rename to wheel
|
||||||
Suspension_Constraint_Config :: struct {
|
Suspension_Constraint_Config :: struct {
|
||||||
rel_pos: rl.Vector3,
|
rel_pos: Vec3,
|
||||||
rel_dir: rl.Vector3,
|
rel_dir: Vec3,
|
||||||
body: Body_Handle,
|
body: Body_Handle,
|
||||||
rest: f32,
|
rest: f32,
|
||||||
compliance: f32,
|
compliance: f32,
|
||||||
|
@ -1,21 +1,26 @@
|
|||||||
package physics
|
package physics
|
||||||
|
|
||||||
import he "game:halfedge"
|
|
||||||
import "game:container/spanpool"
|
import "game:container/spanpool"
|
||||||
|
import he "game:halfedge"
|
||||||
|
|
||||||
Convex_Container :: struct {
|
Convex_Container :: struct {
|
||||||
vertices: spanpool.Span_Pool(he.Vertex),
|
vertices: spanpool.Span_Pool(he.Vertex),
|
||||||
faces: spanpool.Span_Pool(he.Face),
|
faces: spanpool.Span_Pool(he.Face),
|
||||||
edges: spanpool.Span_Pool(he.Half_Edge),
|
edges: spanpool.Span_Pool(he.Half_Edge),
|
||||||
}
|
}
|
||||||
|
|
||||||
Convex_Handle :: struct {
|
Convex_Handle :: struct {
|
||||||
vertices: spanpool.Handle,
|
vertices: spanpool.Handle,
|
||||||
faces: spanpool.Handle,
|
faces: spanpool.Handle,
|
||||||
edges: spanpool.Handle,
|
edges: spanpool.Handle,
|
||||||
}
|
}
|
||||||
|
|
||||||
convex_container_add :: proc(container: ^Convex_Container, mesh: he.Half_Edge_Mesh) -> (handle: Convex_Handle) {
|
convex_container_add :: proc(
|
||||||
|
container: ^Convex_Container,
|
||||||
|
mesh: he.Half_Edge_Mesh,
|
||||||
|
) -> (
|
||||||
|
handle: Convex_Handle,
|
||||||
|
) {
|
||||||
handle.vertices = spanpool.allocate_elems(&container.vertices, ..mesh.vertices)
|
handle.vertices = spanpool.allocate_elems(&container.vertices, ..mesh.vertices)
|
||||||
handle.faces = spanpool.allocate_elems(&container.faces, ..mesh.faces)
|
handle.faces = spanpool.allocate_elems(&container.faces, ..mesh.faces)
|
||||||
handle.edges = spanpool.allocate_elems(&container.edges, ..mesh.edges)
|
handle.edges = spanpool.allocate_elems(&container.edges, ..mesh.edges)
|
||||||
@ -23,7 +28,12 @@ convex_container_add :: proc(container: ^Convex_Container, mesh: he.Half_Edge_Me
|
|||||||
return
|
return
|
||||||
}
|
}
|
||||||
|
|
||||||
convex_container_get_mesh :: proc(container: ^Convex_Container, handle: Convex_Handle) -> (mesh: he.Half_Edge_Mesh) {
|
convex_container_get_mesh :: proc(
|
||||||
|
container: ^Convex_Container,
|
||||||
|
handle: Convex_Handle,
|
||||||
|
) -> (
|
||||||
|
mesh: he.Half_Edge_Mesh,
|
||||||
|
) {
|
||||||
mesh.vertices = spanpool.resolve_slice(&container.vertices, handle.vertices)
|
mesh.vertices = spanpool.resolve_slice(&container.vertices, handle.vertices)
|
||||||
mesh.faces = spanpool.resolve_slice(&container.faces, handle.faces)
|
mesh.faces = spanpool.resolve_slice(&container.faces, handle.faces)
|
||||||
mesh.edges = spanpool.resolve_slice(&container.edges, handle.edges)
|
mesh.edges = spanpool.resolve_slice(&container.edges, handle.edges)
|
||||||
@ -86,7 +96,7 @@ convex_container_destroy :: proc(container: ^Convex_Container) {
|
|||||||
// }
|
// }
|
||||||
//
|
//
|
||||||
// box_to_convex :: proc(box: Shape_Box, allocator := context.allocator) -> (convex: Shape_Convex) {
|
// box_to_convex :: proc(box: Shape_Box, allocator := context.allocator) -> (convex: Shape_Convex) {
|
||||||
// vertices := make([]rl.Vector3, 8, context.temp_allocator)
|
// vertices := make([]Vec3, 8, context.temp_allocator)
|
||||||
//
|
//
|
||||||
// for corner, i in BOX_CORNERS_NORM {
|
// for corner, i in BOX_CORNERS_NORM {
|
||||||
// vertices[i] = corner * box.size
|
// vertices[i] = corner * box.size
|
||||||
@ -98,7 +108,7 @@ convex_container_destroy :: proc(container: ^Convex_Container) {
|
|||||||
// }
|
// }
|
||||||
//
|
//
|
||||||
// Shape_Box :: struct {
|
// Shape_Box :: struct {
|
||||||
// size: rl.Vector3,
|
// size: Vec3,
|
||||||
// }
|
// }
|
||||||
//
|
//
|
||||||
// Shape_Union :: struct {
|
// Shape_Union :: struct {
|
||||||
@ -112,8 +122,8 @@ convex_container_destroy :: proc(container: ^Convex_Container) {
|
|||||||
// }
|
// }
|
||||||
//
|
//
|
||||||
// Shape_Instance :: struct {
|
// Shape_Instance :: struct {
|
||||||
// pos: rl.Vector3,
|
// pos: Vec3,
|
||||||
// rot: rl.Quaternion,
|
// rot: Quat,
|
||||||
// shape: Shape_Variant,
|
// shape: Shape_Variant,
|
||||||
// }
|
// }
|
||||||
//
|
//
|
||||||
@ -128,8 +138,8 @@ convex_container_destroy :: proc(container: ^Convex_Container) {
|
|||||||
// }
|
// }
|
||||||
//
|
//
|
||||||
// Shape_Desc :: struct {
|
// Shape_Desc :: struct {
|
||||||
// pos: rl.Vector3,
|
// pos: Vec3,
|
||||||
// rot: rl.Quaternion,
|
// rot: Quat,
|
||||||
// shape: Shape_Desc_Variant,
|
// shape: Shape_Desc_Variant,
|
||||||
// using link: list.Node,
|
// using link: list.Node,
|
||||||
// }
|
// }
|
||||||
|
@ -6,7 +6,6 @@ import "core:math"
|
|||||||
import lg "core:math/linalg"
|
import lg "core:math/linalg"
|
||||||
import "core:slice"
|
import "core:slice"
|
||||||
import "libs:tracy"
|
import "libs:tracy"
|
||||||
import rl "vendor:raylib"
|
|
||||||
|
|
||||||
_ :: math
|
_ :: math
|
||||||
_ :: fmt
|
_ :: fmt
|
||||||
@ -14,7 +13,7 @@ _ :: fmt
|
|||||||
Solver_Config :: struct {
|
Solver_Config :: struct {
|
||||||
// Will automatically do fixed timestep
|
// Will automatically do fixed timestep
|
||||||
timestep: f32,
|
timestep: f32,
|
||||||
gravity: rl.Vector3,
|
gravity: Vec3,
|
||||||
substreps_minus_one: i32,
|
substreps_minus_one: i32,
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -117,21 +116,21 @@ simulate :: proc(
|
|||||||
}
|
}
|
||||||
|
|
||||||
Body_Sim_State :: struct {
|
Body_Sim_State :: struct {
|
||||||
prev_x: rl.Vector3,
|
prev_x: Vec3,
|
||||||
prev_v: rl.Vector3,
|
prev_v: Vec3,
|
||||||
prev_w: rl.Vector3,
|
prev_w: Vec3,
|
||||||
prev_q: rl.Quaternion,
|
prev_q: Quat,
|
||||||
}
|
}
|
||||||
|
|
||||||
GLOBAL_PLANE :: collision.Plane {
|
GLOBAL_PLANE :: collision.Plane {
|
||||||
normal = rl.Vector3{0, 1, 0},
|
normal = Vec3{0, 1, 0},
|
||||||
dist = 0,
|
dist = 0,
|
||||||
}
|
}
|
||||||
|
|
||||||
Contact_Pair :: struct {
|
Contact_Pair :: struct {
|
||||||
a, b: Body_Handle,
|
a, b: Body_Handle,
|
||||||
prev_x_a, prev_x_b: rl.Vector3,
|
prev_x_a, prev_x_b: Vec3,
|
||||||
prev_q_a, prev_q_b: rl.Quaternion,
|
prev_q_a, prev_q_b: Quat,
|
||||||
manifold: collision.Contact_Manifold,
|
manifold: collision.Contact_Manifold,
|
||||||
applied_corrections: int,
|
applied_corrections: int,
|
||||||
lambda_normal: [4]f32,
|
lambda_normal: [4]f32,
|
||||||
@ -552,7 +551,7 @@ simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) {
|
|||||||
// Drive forces
|
// Drive forces
|
||||||
if true {
|
if true {
|
||||||
total_impulse := v.drive_impulse - v.brake_impulse
|
total_impulse := v.drive_impulse - v.brake_impulse
|
||||||
forward := body_local_to_world_vec(body, rl.Vector3{0, 0, 1})
|
forward := body_local_to_world_vec(body, Vec3{0, 0, 1})
|
||||||
|
|
||||||
_ = apply_constraint_correction_unilateral(
|
_ = apply_constraint_correction_unilateral(
|
||||||
dt,
|
dt,
|
||||||
@ -600,7 +599,7 @@ body_solve_velocity :: #force_inline proc(body: Body_Ptr, state: Body_Sim_State,
|
|||||||
body.v = (body.x - state.prev_x) * inv_dt
|
body.v = (body.x - state.prev_x) * inv_dt
|
||||||
|
|
||||||
delta_q := body.q * lg.quaternion_inverse(state.prev_q)
|
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
|
body.w = Vec3{delta_q.x, delta_q.y, delta_q.z} * 2.0 * inv_dt
|
||||||
|
|
||||||
if delta_q.w < 0 {
|
if delta_q.w < 0 {
|
||||||
body.w = -body.w
|
body.w = -body.w
|
||||||
@ -612,13 +611,13 @@ calculate_constraint_params1 :: proc(
|
|||||||
body: Body_Ptr,
|
body: Body_Ptr,
|
||||||
compliance: f32,
|
compliance: f32,
|
||||||
error: f32,
|
error: f32,
|
||||||
error_gradient: rl.Vector3,
|
error_gradient: Vec3,
|
||||||
pos: rl.Vector3,
|
pos: Vec3,
|
||||||
other_combined_inv_mass: f32,
|
other_combined_inv_mass: f32,
|
||||||
) -> (
|
) -> (
|
||||||
lambda: f32,
|
lambda: f32,
|
||||||
w: f32,
|
w: f32,
|
||||||
correction: rl.Vector3,
|
correction: Vec3,
|
||||||
ok: bool,
|
ok: bool,
|
||||||
) {
|
) {
|
||||||
if error == 0 {
|
if error == 0 {
|
||||||
@ -647,13 +646,13 @@ calculate_constraint_params2 :: proc(
|
|||||||
body2: Body_Ptr,
|
body2: Body_Ptr,
|
||||||
compliance: f32,
|
compliance: f32,
|
||||||
error: f32,
|
error: f32,
|
||||||
error_gradient: rl.Vector3,
|
error_gradient: Vec3,
|
||||||
pos1: rl.Vector3,
|
pos1: Vec3,
|
||||||
pos2: rl.Vector3,
|
pos2: Vec3,
|
||||||
) -> (
|
) -> (
|
||||||
lambda: f32,
|
lambda: f32,
|
||||||
correction1: rl.Vector3,
|
correction1: Vec3,
|
||||||
correction2: rl.Vector3,
|
correction2: Vec3,
|
||||||
ok: bool,
|
ok: bool,
|
||||||
) {
|
) {
|
||||||
if error == 0 {
|
if error == 0 {
|
||||||
@ -682,14 +681,14 @@ apply_constraint_correction_unilateral :: proc(
|
|||||||
body: Body_Ptr,
|
body: Body_Ptr,
|
||||||
compliance: f32,
|
compliance: f32,
|
||||||
error: f32,
|
error: f32,
|
||||||
error_gradient: rl.Vector3,
|
error_gradient: Vec3,
|
||||||
pos: rl.Vector3,
|
pos: Vec3,
|
||||||
other_combined_inv_mass: f32 = 0,
|
other_combined_inv_mass: f32 = 0,
|
||||||
) -> (
|
) -> (
|
||||||
lambda: f32,
|
lambda: f32,
|
||||||
) {
|
) {
|
||||||
w: f32
|
w: f32
|
||||||
correction: rl.Vector3
|
correction: Vec3
|
||||||
ok: bool
|
ok: bool
|
||||||
lambda, w, correction, ok = calculate_constraint_params1(
|
lambda, w, correction, ok = calculate_constraint_params1(
|
||||||
dt,
|
dt,
|
||||||
@ -708,7 +707,7 @@ apply_constraint_correction_unilateral :: proc(
|
|||||||
return
|
return
|
||||||
}
|
}
|
||||||
|
|
||||||
multiply_inv_intertia :: proc(body: Body_Ptr, vec: rl.Vector3) -> (result: rl.Vector3) {
|
multiply_inv_intertia :: proc(body: Body_Ptr, vec: Vec3) -> (result: Vec3) {
|
||||||
q := body.q
|
q := body.q
|
||||||
inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q))
|
inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q))
|
||||||
|
|
||||||
@ -719,7 +718,7 @@ multiply_inv_intertia :: proc(body: Body_Ptr, vec: rl.Vector3) -> (result: rl.Ve
|
|||||||
return result
|
return result
|
||||||
}
|
}
|
||||||
|
|
||||||
apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) {
|
apply_correction :: proc(body: Body_Ptr, corr: Vec3, pos: Vec3) {
|
||||||
// rl.DrawSphereWires(pos, 0.5, 4, 4, rl.BLUE)
|
// rl.DrawSphereWires(pos, 0.5, 4, 4, rl.BLUE)
|
||||||
// rl.DrawLine3D(pos, pos + corr, rl.BLUE)
|
// rl.DrawLine3D(pos, pos + corr, rl.BLUE)
|
||||||
|
|
||||||
@ -744,7 +743,7 @@ apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) {
|
|||||||
body.q = q
|
body.q = q
|
||||||
}
|
}
|
||||||
|
|
||||||
get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: rl.Vector3) -> f32 {
|
get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: Vec3) -> f32 {
|
||||||
q := body.q
|
q := body.q
|
||||||
inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q))
|
inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q))
|
||||||
|
|
||||||
|
Loading…
x
Reference in New Issue
Block a user