339 lines
8.8 KiB
Odin
339 lines
8.8 KiB
Odin
package game
|
|
|
|
import "base:builtin"
|
|
import "core:math"
|
|
import lg "core:math/linalg"
|
|
import "game:debug"
|
|
import rl "vendor:raylib"
|
|
import "vendor:raylib/rlgl"
|
|
|
|
SPLINE_SUBDIVS_U :: 1
|
|
SPLINE_SUBDIVS_V :: 16
|
|
ROAD_WIDTH :: 8.0
|
|
CATMULL_ROM_ALPHA :: 1.0
|
|
CATMULL_ROM_TENSION :: 0.0
|
|
|
|
calculate_spline_ctrl_rotations :: proc(
|
|
points: []rl.Vector3,
|
|
allocator := context.allocator,
|
|
) -> []lg.Quaternionf32 {
|
|
points_len := len(points)
|
|
ctrl_rotations := make([]lg.Quaternionf32, points_len, allocator)
|
|
|
|
// Normals for control points
|
|
for i in 0 ..< points_len - 1 {
|
|
pos := points[i]
|
|
tangent := lg.normalize0(points[i + 1] - pos)
|
|
bitangent := lg.normalize0(lg.cross(tangent, rl.Vector3{0, 1, 0}))
|
|
normal := lg.normalize0(lg.cross(bitangent, tangent))
|
|
rotation_matrix: lg.Matrix3f32
|
|
rotation_matrix[0], rotation_matrix[1], rotation_matrix[2] = bitangent, normal, -tangent
|
|
ctrl_rotations[i] = lg.quaternion_from_matrix3(rotation_matrix)
|
|
|
|
if points_len >= 2 {
|
|
ctrl_rotations[points_len - 1] = ctrl_rotations[points_len - 2]
|
|
}
|
|
}
|
|
return ctrl_rotations
|
|
}
|
|
|
|
Interpolated_Point :: struct {
|
|
pos: rl.Vector3,
|
|
normal: rl.Vector3,
|
|
}
|
|
|
|
sample_spline :: proc(points: []rl.Vector3, t: f32) -> rl.Vector3 {
|
|
points_len := len(points)
|
|
if points_len >= 2 {
|
|
t_mul_len := math.saturate(t) * f32(len(points))
|
|
i := int(t_mul_len)
|
|
frac_t := t_mul_len - f32(i)
|
|
|
|
extended_start := points[0] + (points[0] - points[1])
|
|
extended_end := points[points_len - 1] + points[points_len - 1] - points[points_len - 2]
|
|
extended_end2 := extended_end + points[points_len - 1] - points[points_len - 2]
|
|
|
|
prev := i > 0 ? points[i - 1] : extended_start
|
|
current := points[i]
|
|
next := i < points_len - 1 ? points[i + 1] : extended_end
|
|
next2 := i < points_len - 2 ? points[i + 2] : extended_end2
|
|
|
|
a, b, c, d := catmull_rom_coefs(
|
|
prev,
|
|
current,
|
|
next,
|
|
next2,
|
|
CATMULL_ROM_ALPHA,
|
|
CATMULL_ROM_TENSION,
|
|
)
|
|
|
|
return catmull_rom(a, b, c, d, frac_t)
|
|
} else if len(points) == 1 {
|
|
return points[0]
|
|
}
|
|
|
|
return {}
|
|
}
|
|
|
|
get_point_frame :: #force_inline proc(
|
|
ps: #soa[]Interpolated_Point,
|
|
i: int,
|
|
) -> (
|
|
tangent: rl.Vector3,
|
|
bitangent: rl.Vector3,
|
|
) {
|
|
if len(ps) >= 2 {
|
|
tangent =
|
|
ps[i + 1].pos - ps[i].pos if (i < len(ps) - 1) else ps[len(ps) - 1].pos - ps[len(ps) - 2].pos
|
|
normal := ps[i].normal
|
|
bitangent = lg.normalize0(lg.cross(tangent, normal))
|
|
}
|
|
|
|
return
|
|
}
|
|
|
|
point_to_quad_sdf :: proc(p, a, b, c, d: rl.Vector3) -> f32 {
|
|
ba := b - a
|
|
pa := p - a
|
|
cb := c - b
|
|
pb := p - b
|
|
dc := d - c
|
|
pc := p - c
|
|
ad := a - d
|
|
pd := p - d
|
|
nor := lg.cross(ba, ad)
|
|
|
|
sqrt :: math.sqrt
|
|
dot :: lg.dot
|
|
cross :: lg.cross
|
|
length2 :: lg.length2
|
|
min :: math.min
|
|
sign :: math.sign
|
|
clamp :: math.clamp
|
|
|
|
return sqrt(
|
|
(sign(dot(cross(ba, nor), pa)) + sign(dot(cross(cb, nor), pb)) + sign(dot(cross(dc, nor), pc)) + sign(dot(cross(ad, nor), pd)) < 3.0) ? (min(min(min(length2(ba * clamp(dot(ba, pa) / length2(ba), 0.0, 1.0) - pa), length2(cb * clamp(dot(cb, pb) / length2(cb), 0.0, 1.0) - pb)), length2(dc * clamp(dot(dc, pc) / length2(dc), 0.0, 1.0) - pc)), length2(ad * clamp(dot(ad, pd) / length2(ad), 0.0, 1.0) - pd))) : (dot(nor, pa) * dot(nor, pa) / length2(nor)),
|
|
)
|
|
}
|
|
|
|
// point_to_segmented_line_distance :: proc(
|
|
// ps: #soa[]Interpolated_Point,
|
|
// p: rl.Vector3,
|
|
// ) -> (
|
|
// min_distance: f32,
|
|
// segment_idx: int,
|
|
// ok: bool, // is point within spline
|
|
// ) {
|
|
// min_distance = math.F32_MAX
|
|
// segment_idx = -1
|
|
// for i in 0 ..< len(ps) - 1 {
|
|
// cur, next := ps[i].pos, ps[i + 1].pos
|
|
//
|
|
// tangent, bitangent := get_point_frame(ps, i)
|
|
// next_tangent, next_bitangent := get_point_frame(ps, i + 1)
|
|
// normal, next_normal := ps[i].normal, ps[i + 1].normal
|
|
//
|
|
// tangent_len := lg.length(tangent)
|
|
// tangent_norm := tangent / tangent_len if tangent_len > 0 else 0
|
|
//
|
|
// translated_p := p - cur
|
|
//
|
|
// // point_to_quad_sdf(p, cur - bitangent * -ROAD_WIDTH, cur)
|
|
//
|
|
// dot_v := lg.dot(tangent_norm, translated_p)
|
|
// // dot_u := lg.dot(bitangent)
|
|
//
|
|
// distance: f32
|
|
// if dot_v < 0 {
|
|
// distance = lg.length(translated_p)
|
|
// ok = false
|
|
// } else if dot_v > tangent_len {
|
|
// distance = lg.distance(tangent, translated_p)
|
|
// ok = false
|
|
// } else {
|
|
// projected_p := tangent_norm * dot_v
|
|
// distance = lg.distance(projected_p, translated_p)
|
|
// ok = true
|
|
// }
|
|
//
|
|
// if distance < min_distance {
|
|
// segment_idx = i
|
|
// min_distance = distance
|
|
// }
|
|
// }
|
|
//
|
|
// return min_distance, segment_idx, ok
|
|
// }
|
|
|
|
/// Find collision with the closest
|
|
raycast_spline_tube :: proc(
|
|
ps: #soa[]Interpolated_Point,
|
|
ray: rl.Ray,
|
|
) -> (
|
|
collision: rl.RayCollision,
|
|
segment_idx: int,
|
|
) {
|
|
for i in 0 ..< len(ps) - 1 {
|
|
cur, next := ps[i].pos, ps[i + 1].pos
|
|
|
|
_, bitangent := get_point_frame(ps, i)
|
|
_, next_bitangent := get_point_frame(ps, i + 1)
|
|
// normal, next_normal := ps[i].normal, ps[i + 1].normal
|
|
|
|
p1 := cur + bitangent * -ROAD_WIDTH
|
|
p2 := cur + bitangent * ROAD_WIDTH
|
|
p3 := next + next_bitangent * -ROAD_WIDTH
|
|
p4 := next + next_bitangent * ROAD_WIDTH
|
|
|
|
segment_idx = i
|
|
|
|
collision = rl.GetRayCollisionTriangle(ray, p1, p2, p3)
|
|
if collision.hit {
|
|
break
|
|
}
|
|
collision = rl.GetRayCollisionTriangle(ray, p2, p4, p3)
|
|
if collision.hit {
|
|
break
|
|
}
|
|
}
|
|
|
|
return
|
|
}
|
|
|
|
calculate_spline_interpolated_points :: proc(
|
|
points: []rl.Vector3,
|
|
allocator := context.allocator,
|
|
) -> #soa[]Interpolated_Point {
|
|
points_len := len(points)
|
|
|
|
ctrl_rotations := calculate_spline_ctrl_rotations(points, context.temp_allocator)
|
|
|
|
if points_len >= 2 {
|
|
interpolated_points := make_soa(
|
|
#soa[]Interpolated_Point,
|
|
(points_len - 1) * SPLINE_SUBDIVS_V + 1,
|
|
allocator,
|
|
)
|
|
|
|
extended_start := points[0] + (points[0] - points[1])
|
|
extended_end := points[points_len - 1] + points[points_len - 1] - points[points_len - 2]
|
|
extended_end2 := extended_end + points[points_len - 1] - points[points_len - 2]
|
|
|
|
for i in 0 ..< points_len - 1 {
|
|
prev := i > 0 ? points[i - 1] : extended_start
|
|
current := points[i]
|
|
next := i < points_len - 1 ? points[i + 1] : extended_end
|
|
next2 := i < points_len - 2 ? points[i + 2] : extended_end2
|
|
|
|
cur_frame := ctrl_rotations[i]
|
|
next_frame := ctrl_rotations[i + 1]
|
|
|
|
a, b, c, d := catmull_rom_coefs(
|
|
prev,
|
|
current,
|
|
next,
|
|
next2,
|
|
CATMULL_ROM_ALPHA,
|
|
CATMULL_ROM_TENSION,
|
|
)
|
|
|
|
v_dt := 1.0 / f32(SPLINE_SUBDIVS_V)
|
|
for v_index in 0 ..< SPLINE_SUBDIVS_V {
|
|
v_t := f32(v_index) * v_dt
|
|
out_point := &interpolated_points[i * SPLINE_SUBDIVS_V + v_index]
|
|
|
|
rotation := lg.quaternion_slerp(cur_frame, next_frame, v_t)
|
|
normal := lg.matrix3_from_quaternion(rotation)[1]
|
|
out_point.pos = catmull_rom(a, b, c, d, v_t)
|
|
out_point.normal = normal
|
|
}
|
|
}
|
|
|
|
interpolated_points[len(interpolated_points) - 1] =
|
|
interpolated_points[len(interpolated_points) - 2]
|
|
|
|
return interpolated_points
|
|
}
|
|
|
|
return nil
|
|
}
|
|
|
|
debug_draw_spline :: proc(interpolated_points: #soa[]Interpolated_Point) {
|
|
rlgl.Begin(rlgl.LINES)
|
|
defer rlgl.End()
|
|
for i in 0 ..< len(interpolated_points) - 1 {
|
|
cur, next := interpolated_points[i], interpolated_points[i + 1]
|
|
|
|
tangent := lg.normalize0(next.pos - cur.pos)
|
|
normal := interpolated_points[i].normal
|
|
bitangent := lg.cross(tangent, normal)
|
|
|
|
rlgl.Color4ub(255, 255, 255, 255)
|
|
debug.rlgl_vertex3v(cur.pos)
|
|
debug.rlgl_vertex3v(next.pos)
|
|
|
|
rlgl.Color4ub(255, 0, 0, 255)
|
|
debug.rlgl_vertex3v(cur.pos)
|
|
debug.rlgl_vertex3v(cur.pos + tangent)
|
|
|
|
rlgl.Color4ub(0, 255, 0, 255)
|
|
debug.rlgl_vertex3v(cur.pos)
|
|
debug.rlgl_vertex3v(cur.pos + bitangent * ROAD_WIDTH)
|
|
|
|
rlgl.Color4ub(0, 0, 255, 255)
|
|
debug.rlgl_vertex3v(cur.pos)
|
|
debug.rlgl_vertex3v(cur.pos + normal)
|
|
|
|
}
|
|
}
|
|
debug_draw_spline_mesh :: proc(interpolated_points: #soa[]Interpolated_Point) {
|
|
rlgl.Begin(rlgl.TRIANGLES)
|
|
defer rlgl.End()
|
|
|
|
for i in 0 ..< len(interpolated_points) - 1 {
|
|
cur, next := interpolated_points[i], interpolated_points[i + 1]
|
|
|
|
tangent := lg.normalize0(next.pos - cur.pos)
|
|
normal := interpolated_points[i].normal
|
|
bitangent := lg.normalize0(lg.cross(tangent, normal))
|
|
|
|
next_tangent: rl.Vector3
|
|
if i < len(interpolated_points) - 2 {
|
|
next2 := interpolated_points[i + 2]
|
|
next_tangent = next2.pos - next.pos
|
|
} else {
|
|
next_tangent = tangent
|
|
}
|
|
|
|
next_normal := interpolated_points[i + 1].normal
|
|
next_bitangent := lg.normalize0(lg.cross(next_tangent, next_normal))
|
|
|
|
u_dt := 1.0 / f32(SPLINE_SUBDIVS_U)
|
|
for u in 0 ..< SPLINE_SUBDIVS_U {
|
|
u_t := u_dt * f32(u)
|
|
u_t2 := u_t + u_dt
|
|
|
|
// [-1, 1]
|
|
u_t = u_t * 2 - 1
|
|
u_t2 = u_t2 * 2 - 1
|
|
u_t *= ROAD_WIDTH
|
|
u_t2 *= ROAD_WIDTH
|
|
|
|
p1 := cur.pos + bitangent * u_t
|
|
p2 := cur.pos + bitangent * u_t2
|
|
p3 := next.pos + next_bitangent * u_t
|
|
p4 := next.pos + next_bitangent * u_t2
|
|
|
|
debug.rlgl_color3v(normal * 0.5 + 0.5)
|
|
debug.rlgl_vertex3v(p1)
|
|
debug.rlgl_vertex3v(p2)
|
|
debug.rlgl_vertex3v(p3)
|
|
|
|
debug.rlgl_color3v(next_normal * 0.5 + 0.5)
|
|
debug.rlgl_vertex3v(p2)
|
|
debug.rlgl_vertex3v(p4)
|
|
debug.rlgl_vertex3v(p3)
|
|
}
|
|
}
|
|
}
|