Optimize edge separation test, use SOA structs in more places
This commit is contained in:
parent
a1e8d0f231
commit
860f9f59b6
@ -73,7 +73,7 @@ Car :: struct {
|
||||
}
|
||||
|
||||
SOLVER_CONFIG :: physics.Solver_Config {
|
||||
timestep = 1.0 / 60,
|
||||
timestep = 1.0 / 120,
|
||||
gravity = rl.Vector3{0, -9.8, 0},
|
||||
substreps_minus_one = 2 - 1,
|
||||
}
|
||||
@ -259,7 +259,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
&runtime_world.solver_state,
|
||||
#hash("car", "fnv32a"),
|
||||
physics.Body_Config {
|
||||
initial_pos = {0, 4, 0},
|
||||
initial_pos = {0, 4, -10},
|
||||
initial_rot = linalg.QUATERNIONF32_IDENTITY,
|
||||
// initial_rot = linalg.quaternion_angle_axis(
|
||||
// math.RAD_PER_DEG * 180,
|
||||
@ -277,18 +277,17 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
)
|
||||
|
||||
if true {
|
||||
|
||||
for x in 0 ..< 10 {
|
||||
for y in -3 ..< 100 {
|
||||
for x in 0 ..< 20 {
|
||||
for y in 0 ..< 10 {
|
||||
physics.immediate_body(
|
||||
&world.physics_scene,
|
||||
&runtime_world.solver_state,
|
||||
hash.fnv32a(slice.to_bytes([]int{(x | y << 8)})),
|
||||
physics.Body_Config {
|
||||
initial_pos = {f32(x) * 3 - 10, 5 + f32(y) * 3, 0},
|
||||
initial_pos = {0, 0.5 + f32(y) * 1.1, f32(x) * 3 + 10},
|
||||
initial_rot = linalg.QUATERNIONF32_IDENTITY,
|
||||
shape = physics.Shape_Box{size = 1},
|
||||
mass = 10,
|
||||
mass = 1,
|
||||
},
|
||||
)
|
||||
}
|
||||
@ -648,7 +647,7 @@ draw :: proc() {
|
||||
}
|
||||
}
|
||||
|
||||
// rl.DrawModel(car_model, physics.body_get_shape_pos(car_body), 1, rl.WHITE)
|
||||
rl.DrawModel(car_model, physics.body_get_shape_pos(car_body), 1, rl.WHITE)
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
@ -1,27 +1,15 @@
|
||||
package halfedge
|
||||
|
||||
import "game:debug"
|
||||
import rl "vendor:raylib"
|
||||
|
||||
debug_draw_mesh_wires :: proc(mesh: Half_Edge_Mesh, color: rl.Color) {
|
||||
for face, f in mesh.faces {
|
||||
for _, f in mesh.faces {
|
||||
it := iterator_face_edges(mesh, Face_Index(f))
|
||||
|
||||
center: Vec3
|
||||
num_points := 0
|
||||
|
||||
for edge in iterate_next_edge(&it) {
|
||||
a, b := get_edge_points(mesh, edge)
|
||||
|
||||
center += a
|
||||
num_points += 1
|
||||
|
||||
rl.DrawLine3D(a, b, color)
|
||||
}
|
||||
|
||||
center /= f32(num_points)
|
||||
|
||||
rl.DrawSphereWires(center, 0.1, 4, 4, rl.RED)
|
||||
rl.DrawLine3D(center, center + face.normal, debug.int_to_color(i32(f + 1)))
|
||||
}
|
||||
}
|
||||
|
||||
@ -129,13 +129,29 @@ mesh_from_vertex_index_list :: proc(
|
||||
return mesh
|
||||
}
|
||||
|
||||
get_edge_points :: proc(mesh: Half_Edge_Mesh, edge: Half_Edge) -> (a: Vec3, b: Vec3) {
|
||||
get_edge_points :: #force_inline proc(
|
||||
mesh: Half_Edge_Mesh,
|
||||
edge: Half_Edge,
|
||||
) -> (
|
||||
a: Vec3,
|
||||
b: Vec3,
|
||||
) {
|
||||
a = mesh.vertices[edge.origin].pos
|
||||
b = mesh.vertices[mesh.edges[edge.next].origin].pos
|
||||
return
|
||||
}
|
||||
|
||||
get_edge_direction_normalized :: proc(mesh: Half_Edge_Mesh, edge: Half_Edge) -> (dir: Vec3) {
|
||||
get_edge_direction :: #force_inline proc(mesh: Half_Edge_Mesh, edge: Half_Edge) -> (dir: Vec3) {
|
||||
a, b := get_edge_points(mesh, edge)
|
||||
return b - a
|
||||
}
|
||||
|
||||
get_edge_direction_normalized :: #force_inline proc(
|
||||
mesh: Half_Edge_Mesh,
|
||||
edge: Half_Edge,
|
||||
) -> (
|
||||
dir: Vec3,
|
||||
) {
|
||||
a, b := get_edge_points(mesh, edge)
|
||||
return lg.normalize0(b - a)
|
||||
}
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
package collision
|
||||
|
||||
import "core:container/bit_array"
|
||||
import "core:log"
|
||||
import "core:math"
|
||||
import lg "core:math/linalg"
|
||||
@ -164,6 +165,33 @@ find_furthest_point_from_point :: proc(points: []Vec3, ref: Vec3) -> (p: Vec3) {
|
||||
return
|
||||
}
|
||||
|
||||
// See: Physics for Game Programmers: The Separating Axis Test between Convex Polyhedra
|
||||
// https://gdcvault.com/play/1017646/Physics-for-Game-Programmers-The
|
||||
is_minkowski_face :: #force_inline proc(bxa, dxc, a, b, c, d: Vec3) -> bool {
|
||||
cba := lg.dot(c, bxa)
|
||||
dba := lg.dot(d, bxa)
|
||||
adc := lg.dot(a, dxc)
|
||||
bdc := lg.dot(b, dxc)
|
||||
|
||||
return cba * dba < 0 && adc * bdc < 0 && cba * bdc > 0
|
||||
}
|
||||
|
||||
build_minkowski_face :: #force_inline proc(
|
||||
mesh_a, mesh_b: Convex,
|
||||
edge_a, edge_b: halfedge.Half_Edge,
|
||||
) -> bool {
|
||||
a := mesh_a.faces[edge_a.face].normal
|
||||
b := mesh_a.faces[mesh_a.edges[edge_a.twin].face].normal
|
||||
c := mesh_b.faces[edge_b.face].normal
|
||||
d := mesh_b.faces[mesh_b.edges[edge_b.twin].face].normal
|
||||
|
||||
bxa := halfedge.get_edge_direction(mesh_a, edge_a)
|
||||
dxc := halfedge.get_edge_direction(mesh_b, edge_b)
|
||||
|
||||
// Negate normals c and d because we're interested in minkowski difference instead of sum
|
||||
return is_minkowski_face(bxa, dxc, a, b, -c, -d)
|
||||
}
|
||||
|
||||
query_separation_edges :: proc(
|
||||
a, b: Convex,
|
||||
) -> (
|
||||
@ -178,123 +206,56 @@ query_separation_edges :: proc(
|
||||
a_edge = -1
|
||||
b_edge = -1
|
||||
|
||||
step := 0
|
||||
checked_pairs: bit_array.Bit_Array
|
||||
bit_array.init(&checked_pairs, len(a.edges) * len(b.edges), 0, context.temp_allocator)
|
||||
a_len := len(a.edges)
|
||||
|
||||
if false {
|
||||
calc_pair_index :: proc(a, b, a_len: int) -> int {
|
||||
return (b * a_len) + a
|
||||
}
|
||||
|
||||
separating_plane_p: Vec3
|
||||
success_step: int
|
||||
for edge_a, edge_a_idx in a.edges {
|
||||
for edge_b, edge_b_idx in b.edges {
|
||||
pair_idx := calc_pair_index(edge_a_idx, edge_b_idx, a_len)
|
||||
if bit_array.get(&checked_pairs, pair_idx) {
|
||||
continue
|
||||
}
|
||||
|
||||
Edge_Pair :: [2]halfedge.Edge_Index
|
||||
checked_pairs := make_map_cap(
|
||||
map[Edge_Pair]bool,
|
||||
len(a.edges) * len(b.edges),
|
||||
context.temp_allocator,
|
||||
)
|
||||
|
||||
for edge_a, edge_a_idx in a.edges {
|
||||
for edge_b, edge_b_idx in b.edges {
|
||||
pair := Edge_Pair{halfedge.Edge_Index(edge_a_idx), halfedge.Edge_Index(edge_b_idx)}
|
||||
if checked_pairs[pair] {
|
||||
continue
|
||||
}
|
||||
|
||||
checked_pairs[pair] = true
|
||||
if edge_a.twin >= 0 {
|
||||
checked_pairs[{edge_a.twin, halfedge.Edge_Index(edge_b_idx)}] = true
|
||||
}
|
||||
if edge_b.twin >= 0 {
|
||||
checked_pairs[{halfedge.Edge_Index(edge_a_idx), edge_b.twin}] = true
|
||||
}
|
||||
if edge_a.twin >= 0 && edge_b.twin >= 0 {
|
||||
checked_pairs[{edge_a.twin, edge_b.twin}] = true
|
||||
}
|
||||
|
||||
edge_a_dir := halfedge.get_edge_direction_normalized(a, edge_a)
|
||||
edge_b_dir := halfedge.get_edge_direction_normalized(b, edge_b)
|
||||
// TODO: sort edges so twins are next to each other, then can just iterate with step = 2 and skip this bitfield
|
||||
bit_array.set(&checked_pairs, pair_idx)
|
||||
bit_array.set(&checked_pairs, calc_pair_index(int(edge_a.twin), edge_b_idx, a_len))
|
||||
bit_array.set(&checked_pairs, calc_pair_index(edge_a_idx, int(edge_b.twin), a_len))
|
||||
bit_array.set(
|
||||
&checked_pairs,
|
||||
calc_pair_index(int(edge_a.twin), int(edge_b.twin), a_len),
|
||||
)
|
||||
|
||||
if build_minkowski_face(a, b, edge_a, edge_b) {
|
||||
edge_a1, edge_a2 := halfedge.get_edge_points(a, edge_a)
|
||||
edge_b1, edge_b2 := halfedge.get_edge_points(b, edge_b)
|
||||
edge_a_dir := edge_a2 - edge_a1
|
||||
edge_b_dir := edge_b2 - edge_b1
|
||||
axis := lg.normalize0(lg.cross(edge_a_dir, edge_b_dir))
|
||||
|
||||
if axis == 0 {
|
||||
continue
|
||||
}
|
||||
|
||||
edge_a_origin, _ := halfedge.get_edge_points(a, edge_a)
|
||||
if lg.dot(axis, edge_a_origin - a.center) < 0 {
|
||||
if lg.dot(axis, edge_a1 - a.center) < 0 {
|
||||
axis = -axis
|
||||
}
|
||||
plane_a := plane_from_point_normal(edge_a_origin, axis)
|
||||
vert_a, _, _ := find_support_point(a, plane_a.normal)
|
||||
vert_b, vert_b_idx, _ := find_support_point(b, -plane_a.normal)
|
||||
|
||||
// We found the support vert on mesh b, but now we need to find the
|
||||
// best edge that includes that point
|
||||
vert_b_edge: halfedge.Half_Edge
|
||||
vert_b_edge_idx: halfedge.Edge_Index = -1
|
||||
{
|
||||
min_b2_distance := max(f32)
|
||||
it := halfedge.iterator_vertex_edges(b, vert_b_idx)
|
||||
for edge, edge_idx in halfedge.iterate_next_vertex_edge(&it) {
|
||||
_, vert_b2 := halfedge.get_edge_points(b, edge)
|
||||
plane_a := plane_from_point_normal(edge_a1, axis)
|
||||
|
||||
distance_b2 := signed_distance_plane(vert_b2, plane_a)
|
||||
if distance_b2 < min_b2_distance {
|
||||
min_b2_distance = distance_b2
|
||||
vert_b_edge = edge
|
||||
vert_b_edge_idx = edge_idx
|
||||
}
|
||||
}
|
||||
|
||||
if vert_b_edge_idx < 0 {
|
||||
continue
|
||||
}
|
||||
}
|
||||
|
||||
distance_a := signed_distance_plane(vert_a.pos, plane_a)
|
||||
if distance_a > 0 {
|
||||
continue
|
||||
}
|
||||
distance_b := signed_distance_plane(vert_b.pos, plane_a)
|
||||
vert_b_projected := vert_b.pos + plane_a.normal * -distance_b
|
||||
|
||||
if step == -1 {
|
||||
// a1, a2 := halfedge.get_edge_points(a, edge_a)
|
||||
// edge_a_center := (a1 + a2) * 0.5
|
||||
a1, a2 := halfedge.get_edge_points(halfedge.Half_Edge_Mesh(a), edge_a)
|
||||
b1, b2 := halfedge.get_edge_points(halfedge.Half_Edge_Mesh(b), vert_b_edge)
|
||||
|
||||
rl.DrawLine3D(edge_a_origin, edge_a_origin + plane_a.normal, rl.BLUE)
|
||||
rl.DrawLine3D(a1 + 0.1, a2 + 0.1, rl.ORANGE)
|
||||
rl.DrawLine3D(b1 + 0.1, b2 + 0.1, rl.PURPLE)
|
||||
|
||||
rl.DrawSphereWires(edge_a_origin, 0.1, 4, 4, rl.ORANGE)
|
||||
rl.DrawSphereWires(vert_b.pos, 0.05, 4, 4, rl.BLUE)
|
||||
rl.DrawSphereWires(vert_b_projected, 0.05, 4, 4, rl.BLUE)
|
||||
rl.DrawLine3D(vert_b.pos, vert_b_projected, rl.VIOLET)
|
||||
log.debugf("dist: %v", distance_b)
|
||||
|
||||
{
|
||||
// rl.BeginBlendMode(.ALPHA)
|
||||
// defer rl.EndBlendMode()
|
||||
debug_draw_plane(edge_a_origin, plane_a, rl.Color{0, 228, 48, 100})
|
||||
}
|
||||
}
|
||||
|
||||
if distance_b > separation {
|
||||
separation = distance_b
|
||||
distance := signed_distance_plane(edge_b1, plane_a)
|
||||
if distance > separation {
|
||||
separation = distance
|
||||
a_edge = halfedge.Edge_Index(edge_a_idx)
|
||||
b_edge = vert_b_edge_idx
|
||||
b_edge = halfedge.Edge_Index(edge_b_idx)
|
||||
separating_plane = plane_a
|
||||
separating_plane_p = edge_a_origin
|
||||
success_step = step
|
||||
}
|
||||
|
||||
step += 1
|
||||
}
|
||||
}
|
||||
// log.debugf("step: %v", success_step)
|
||||
// debug_draw_plane(separating_plane_p, separating_plane, rl.Color{228, 0, 48, 100})
|
||||
}
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
|
||||
@ -108,8 +108,8 @@ draw_debug_scene :: proc(scene: ^Scene) {
|
||||
}
|
||||
}
|
||||
|
||||
if true {
|
||||
for &contact, contact_idx in sim_state.contact_pairs[:sim_state.contact_pairs_len] {
|
||||
if false {
|
||||
for &contact, contact_idx in sim_state.contact_pairs {
|
||||
points_a := contact.manifold.points_a
|
||||
points_b := contact.manifold.points_b
|
||||
points_a_slice, points_b_slice :=
|
||||
|
||||
@ -26,8 +26,7 @@ Sim_State :: struct {
|
||||
first_free_suspension_constraint_plus_one: i32,
|
||||
|
||||
// Persistent stuff for simulation
|
||||
contact_pairs: [MAX_CONTACTS]Contact_Pair,
|
||||
contact_pairs_len: int,
|
||||
contact_pairs: #soa[dynamic]Contact_Pair,
|
||||
convex_container: Convex_Container,
|
||||
}
|
||||
|
||||
@ -381,6 +380,7 @@ _get_first_free_body :: proc(sim_state: ^Sim_State) -> i32 {
|
||||
destry_sim_state :: proc(sim_state: ^Sim_State) {
|
||||
delete_soa(sim_state.bodies)
|
||||
delete_soa(sim_state.suspension_constraints)
|
||||
delete_soa(sim_state.contact_pairs)
|
||||
convex_container_destroy(&sim_state.convex_container)
|
||||
}
|
||||
|
||||
|
||||
@ -10,6 +10,7 @@ import "libs:tracy"
|
||||
|
||||
_ :: math
|
||||
_ :: fmt
|
||||
_ :: slice
|
||||
|
||||
Solver_Config :: struct {
|
||||
// Will automatically do fixed timestep
|
||||
@ -229,9 +230,9 @@ simulate_step :: proc(
|
||||
) {
|
||||
tracy.Zone()
|
||||
|
||||
body_states := make([]Body_Sim_State, len(sim_state.bodies), context.temp_allocator)
|
||||
body_states := make_soa(#soa[]Body_Sim_State, len(sim_state.bodies), context.temp_allocator)
|
||||
|
||||
sim_state.contact_pairs_len = 0
|
||||
resize_soa(&sim_state.contact_pairs, 0)
|
||||
|
||||
substeps := config.substreps_minus_one + 1
|
||||
|
||||
@ -283,6 +284,16 @@ simulate_step :: proc(
|
||||
assert(body.alive)
|
||||
assert(body2.alive)
|
||||
|
||||
aabb1, aabb2 := body_get_aabb(body), body_get_aabb(body2)
|
||||
|
||||
// TODO: extract common math functions into a sane place
|
||||
if !collision.test_aabb_vs_aabb(
|
||||
{min = aabb1.center - aabb1.extent, max = aabb1.center + aabb1.extent},
|
||||
{min = aabb2.center - aabb2.extent, max = aabb2.center + aabb2.extent},
|
||||
) {
|
||||
continue
|
||||
}
|
||||
|
||||
m1, m2 :=
|
||||
body_get_convex_shape_world(sim_state, body),
|
||||
body_get_convex_shape_world(sim_state, body2)
|
||||
@ -291,7 +302,9 @@ simulate_step :: proc(
|
||||
raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2)
|
||||
|
||||
if collision {
|
||||
contact_pair := &sim_state.contact_pairs[sim_state.contact_pairs_len]
|
||||
new_contact_idx := len(sim_state.contact_pairs)
|
||||
resize_soa(&sim_state.contact_pairs, new_contact_idx + 1)
|
||||
contact_pair := &sim_state.contact_pairs[new_contact_idx]
|
||||
contact_pair^ = Contact_Pair {
|
||||
a = Body_Handle(i + 1),
|
||||
b = Body_Handle(j + 1),
|
||||
@ -301,7 +314,6 @@ simulate_step :: proc(
|
||||
prev_q_b = body2.q,
|
||||
manifold = raw_manifold,
|
||||
}
|
||||
sim_state.contact_pairs_len += 1
|
||||
manifold := &contact_pair.manifold
|
||||
|
||||
// Convert manifold contact from world to local space
|
||||
@ -350,7 +362,7 @@ simulate_step :: proc(
|
||||
{
|
||||
tracy.ZoneN("simulate_step::static_friction")
|
||||
|
||||
if false {
|
||||
when false {
|
||||
context = context
|
||||
context.user_ptr = sim_state
|
||||
slice.sort_by(
|
||||
@ -383,7 +395,7 @@ simulate_step :: proc(
|
||||
)
|
||||
}
|
||||
|
||||
for &contact_pair in sim_state.contact_pairs[:sim_state.contact_pairs_len] {
|
||||
for &contact_pair in sim_state.contact_pairs {
|
||||
manifold := contact_pair.manifold
|
||||
body, body2 :=
|
||||
get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b)
|
||||
@ -428,7 +440,7 @@ simulate_step :: proc(
|
||||
p2,
|
||||
)
|
||||
|
||||
STATIC_FRICTION :: 0.8
|
||||
STATIC_FRICTION :: 0.6
|
||||
if ok_tangent && delta_lambda_tangent < STATIC_FRICTION * lambda_norm {
|
||||
contact_pair.applied_static_friction[point_idx] = true
|
||||
contact_pair.lambda_tangent[point_idx] = delta_lambda_tangent
|
||||
@ -475,13 +487,21 @@ simulate_step :: proc(
|
||||
}
|
||||
}
|
||||
|
||||
solve_velocities(sim_state, body_states, inv_dt)
|
||||
{
|
||||
// Compute new linear and angular velocities
|
||||
for _, i in sim_state.bodies_slice {
|
||||
body := &sim_state.bodies_slice[i]
|
||||
if body.alive {
|
||||
body_solve_velocity(body, body_states[i].prev_x, body_states[i].prev_q, inv_dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Restituion
|
||||
{
|
||||
tracy.ZoneN("simulate_step::restitution")
|
||||
|
||||
for &pair in sim_state.contact_pairs[:sim_state.contact_pairs_len] {
|
||||
for &pair in sim_state.contact_pairs {
|
||||
i, j := int(pair.a) - 1, int(pair.b) - 1
|
||||
manifold := &pair.manifold
|
||||
|
||||
@ -507,7 +527,7 @@ simulate_step :: proc(
|
||||
prev_v_normal := lg.dot(prev_v, manifold.normal)
|
||||
v_normal := lg.dot(v, manifold.normal)
|
||||
|
||||
RESTITUTION :: 1
|
||||
RESTITUTION :: 0
|
||||
|
||||
restitution := f32(RESTITUTION)
|
||||
|
||||
@ -537,7 +557,7 @@ simulate_step :: proc(
|
||||
if true {
|
||||
tracy.ZoneN("simulate_step::dynamic_friction")
|
||||
|
||||
for &pair in sim_state.contact_pairs[:sim_state.contact_pairs_len] {
|
||||
for &pair in sim_state.contact_pairs {
|
||||
manifold := &pair.manifold
|
||||
body1 := get_body(sim_state, pair.a)
|
||||
body2 := get_body(sim_state, pair.b)
|
||||
@ -557,7 +577,7 @@ simulate_step :: proc(
|
||||
v_normal := lg.dot(manifold.normal, v) * manifold.normal
|
||||
v_tangent := v - v_normal
|
||||
|
||||
DYNAMIC_FRICTION :: 0.6
|
||||
DYNAMIC_FRICTION :: 0.3
|
||||
v_tangent_len := lg.length(v_tangent)
|
||||
if v_tangent_len > 0 {
|
||||
v_tangent_norm := v_tangent / v_tangent_len
|
||||
@ -624,7 +644,7 @@ simulate_step :: proc(
|
||||
pos = wheel_world_pos,
|
||||
)
|
||||
|
||||
body_solve_velocity(body, body_state, inv_dt)
|
||||
body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt)
|
||||
}
|
||||
|
||||
// Drive forces
|
||||
@ -640,7 +660,7 @@ simulate_step :: proc(
|
||||
-forward,
|
||||
wheel_world_pos,
|
||||
)
|
||||
body_solve_velocity(body, body_state, inv_dt)
|
||||
body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt)
|
||||
}
|
||||
|
||||
// Lateral friction
|
||||
@ -656,7 +676,7 @@ simulate_step :: proc(
|
||||
v.applied_impulse.x = impulse
|
||||
|
||||
apply_correction(body, corr, v.hit_point)
|
||||
body_solve_velocity(body, body_state, inv_dt)
|
||||
body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -665,19 +685,17 @@ simulate_step :: proc(
|
||||
}
|
||||
|
||||
solve_velocities :: proc(scene: ^Sim_State, body_states: []Body_Sim_State, inv_dt: f32) {
|
||||
// Compute new linear and angular velocities
|
||||
for _, i in scene.bodies_slice {
|
||||
body := &scene.bodies_slice[i]
|
||||
if body.alive {
|
||||
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
|
||||
body_solve_velocity :: #force_inline proc(
|
||||
body: Body_Ptr,
|
||||
prev_x: Vec3,
|
||||
prev_q: Quat,
|
||||
inv_dt: f32,
|
||||
) {
|
||||
body.v = (body.x - prev_x) * inv_dt
|
||||
|
||||
delta_q := body.q * lg.quaternion_inverse(state.prev_q)
|
||||
delta_q := body.q * lg.quaternion_inverse(prev_q)
|
||||
body.w = Vec3{delta_q.x, delta_q.y, delta_q.z} * 2.0 * inv_dt
|
||||
|
||||
if delta_q.w < 0 {
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user