Good spline frame algorithm

game/game.odin

@@ -15,6 +15,7 @@
 package game
 
 import "assets"
+import "core:math"
 import "core:math/linalg"
 import rl "vendor:raylib"
 import "vendor:raylib/rlgl"
@@ -193,7 +194,7 @@ update_editor :: proc() {
 	switch es.track_edit_state {
 	case .Select:
 		{
-			if rl.IsKeyPressed(.ENTER) {
+			if rl.IsKeyPressed(.F) {
 				add_track_spline_point()
 			}
 
@@ -289,6 +290,34 @@ update :: proc() {
 	}
 }
 
+catmull_rom_coefs :: proc(
+	v0, v1, v2, v3: rl.Vector3,
+	alpha, tension: f32,
+) -> (
+	a, b, c, d: rl.Vector3,
+) {
+	t01 := math.pow(linalg.distance(v0, v1), alpha)
+	t12 := math.pow(linalg.distance(v1, v2), alpha)
+	t23 := math.pow(linalg.distance(v2, v3), alpha)
+
+	m1 := (1.0 - tension) * (v2 - v1 + t12 * ((v1 - v0) / t01 - (v2 - v0) / (t01 + t12)))
+	m2 := (1.0 - tension) * (v2 - v1 + t12 * ((v3 - v2) / t23 - (v3 - v1) / (t12 + t23)))
+
+	a = 2.0 * (v1 - v2) + m1 + m2
+	b = -3.0 * (v1 - v2) - m1 - m1 - m2
+	c = m1
+	d = v1
+
+	return
+}
+
+catmull_rom :: proc(a, b, c, d: rl.Vector3, t: f32) -> rl.Vector3 {
+	t2 := t * t
+	t3 := t2 * t
+
+	return a * t3 + b * t2 + c * t + d
+}
+
 draw :: proc() {
 	rl.BeginDrawing()
 	defer rl.EndDrawing()
@@ -325,58 +354,182 @@ draw :: proc() {
 
 
 		SPLINE_SUBDIVS_U :: 4
-		SPLINE_SUBDIVS_V :: 8
+		SPLINE_SUBDIVS_V :: 16
 		ROAD_WIDTH :: 2.0
+		CATMULL_ROM_ALPHA :: 1.0
+		CATMULL_ROM_TENSION :: 0.0
 
 		{
-			rlgl.Begin(rlgl.LINES)
+			// Debug draw spline road
-			defer rlgl.End()
+			{
+				rlgl.EnableWireMode()
+				defer rlgl.DisableWireMode()
 
-			rlgl.Color3f(1, 0, 0)
+				rlgl.Color3f(1, 0, 0)
 
-			for i in 0 ..< points_len {
+
-				if i >= 1 && i < points_len - 2 {
+				Frame :: struct {
-					for v in 0 ..< SPLINE_SUBDIVS_V {
+					tangent, normal: rl.Vector3,
-						t := f32(v) / f32(SPLINE_SUBDIVS_V)
+					rotation:        linalg.Quaternionf32,
-						t2 := f32(v + 1) / f32(SPLINE_SUBDIVS_V)
+				}
-						point := linalg.catmull_rom(
+
-							points[i - 1],
+				ctrl_frames := make_soa(#soa[]Frame, points_len, context.temp_allocator)
-							points[i],
+
-							points[i + 1],
+				// Normals for control points
-							points[i + 2],
+				for i in 0 ..< points_len - 1 {
-							t,
+					pos := points[i]
-						)
+					tangent := linalg.normalize0(points[i + 1] - pos)
-						point2 := linalg.catmull_rom(
+					bitangent := linalg.normalize0(linalg.cross(tangent, rl.Vector3{0, 1, 0}))
-							points[i - 1],
+					normal := linalg.normalize0(linalg.cross(bitangent, tangent))
-							points[i],
+					rotation_matrix: linalg.Matrix3f32
-							points[i + 1],
+					rotation_matrix[0], rotation_matrix[1], rotation_matrix[2] =
-							points[i + 2],
+						bitangent, normal, -tangent
-							t2,
+					ctrl_frames[i].tangent = tangent
+					ctrl_frames[i].normal = normal
+					ctrl_frames[i].rotation = linalg.quaternion_from_matrix3(rotation_matrix)
+				}
+				ctrl_frames[points_len - 1] = ctrl_frames[points_len - 2]
+
+				interpolated_points := make(
+					[]rl.Vector3,
+					(points_len - 1) * SPLINE_SUBDIVS_V + 1,
+					context.temp_allocator,
+				)
+
+				if points_len >= 2 {
+					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
+
+						a, b, c, d := catmull_rom_coefs(
+							prev,
+							current,
+							next,
+							next2,
+							CATMULL_ROM_ALPHA,
+							CATMULL_ROM_TENSION,
 						)
 
-						tangent := linalg.normalize0(point2 - point)
+						v_dt := 1.0 / f32(SPLINE_SUBDIVS_V)
-						right := -linalg.cross(tangent, rl.Vector3{0, 1, 0})
+						for v_index in 0 ..< SPLINE_SUBDIVS_V {
-						// normal := linalg.cross(tangent, right)
+							v_t := f32(v_index) * v_dt
 
-						for u in 0 ..< SPLINE_SUBDIVS_U {
+							interpolated_pos := catmull_rom(a, b, c, d, v_t)
-							offset := f32(u) / f32(SPLINE_SUBDIVS_U)
+							interpolated_points[i * SPLINE_SUBDIVS_V + v_index] = interpolated_pos
-							offset = offset * 2 - 1
+						}
-							offset2 := f32(u + 1) / f32(SPLINE_SUBDIVS_U)
+					}
-							offset2 = offset2 * 2 - 1
 
-							p1 := point + offset * right
+					interpolated_points[len(interpolated_points) - 1] = points[points_len - 1]
-							p2 := point + offset2 * right
 
-							rlgl.Vertex3f(p1.x, p1.y, p1.z)
+					frames := make_soa(
-							rlgl.Vertex3f(p2.x, p2.y, p2.z)
+						#soa[]Frame,
+						len(interpolated_points),
+						context.temp_allocator,
+					)
+
+					{
+						for i in 0 ..< len(frames) - 1 {
+							ctrl_index := i / SPLINE_SUBDIVS_V
+							v_t := f32(i % SPLINE_SUBDIVS_V) / (SPLINE_SUBDIVS_V)
+
+							pos := interpolated_points[i]
+							cur_frame := ctrl_frames[ctrl_index]
+							next_frame := ctrl_frames[ctrl_index + 1]
+
+							frames[i].rotation = linalg.quaternion_slerp(
+								cur_frame.rotation,
+								next_frame.rotation,
+								v_t,
+							)
+							normal := linalg.matrix3_from_quaternion(frames[i].rotation)[1]
+							tangent := linalg.normalize0(interpolated_points[i + 1] - pos)
+
+							frames[i].tangent = tangent
+							frames[i].normal = normal
 						}
 
-						rlgl.Vertex3f(point.x, point.y, point.z)
+						frames[len(frames) - 1] = frames[len(frames) - 2]
-						rlgl.Vertex3f(point2.x, point2.y, point2.z)
+					}
+
+					// Parallel frame algorithm (doesn't work because it rotates the road too much, but looks pretty)
+					if false {
+						// Calculate reference normal
+						{
+							tangent := linalg.normalize0(
+								interpolated_points[1] - interpolated_points[0],
+							)
+							frames[0].tangent = tangent
+							bitangent := linalg.normalize0(
+								linalg.cross(tangent, rl.Vector3{0, 1, 0}),
+							)
+							frames[0].normal = linalg.normalize0(linalg.cross(bitangent, tangent))
+						}
+
+						for i in 1 ..< len(frames) {
+							cur, next := interpolated_points[i], interpolated_points[i + 1]
+
+							tangent := linalg.normalize0(next - cur)
+							bitangent := linalg.cross(frames[i - 1].tangent, tangent)
+							bitangent_len := linalg.length(bitangent)
+							normal: rl.Vector3
+							if bitangent_len < math.F32_EPSILON {
+								normal = frames[i - 1].normal
+							} else {
+								bitangent /= bitangent_len
+								angle := math.acos(linalg.dot(frames[i - 1].tangent, tangent))
+								normal =
+									linalg.matrix3_rotate(angle, bitangent) * frames[i - 1].normal
+							}
+
+							frames[i].tangent = tangent
+							frames[i].normal = normal
+						}
+					}
+
+					{
+						rlgl.Begin(rlgl.LINES)
+						defer rlgl.End()
+
+						for i in 0 ..< len(interpolated_points) - 1 {
+							cur, next := interpolated_points[i], interpolated_points[i + 1]
+
+							// rotation := linalg.matrix3_look_at(cur, next, rl.Vector3{0, 1, 0})
+							tangent := frames[i].tangent
+							normal := frames[i].normal
+							bitangent := linalg.cross(tangent, normal)
+
+							rlgl.Color4ub(255, 255, 255, 255)
+							rlgl.Vertex3f(cur.x, cur.y, cur.z)
+							rlgl.Vertex3f(next.x, next.y, next.z)
+
+							rlgl.Color4ub(255, 0, 0, 255)
+							rlgl.Vertex3f(cur.x, cur.y, cur.z)
+							rlgl.Vertex3f(cur.x + tangent.x, cur.y + tangent.y, cur.z + tangent.z)
+
+							rlgl.Color4ub(0, 255, 0, 255)
+							rlgl.Vertex3f(cur.x, cur.y, cur.z)
+							rlgl.Vertex3f(
+								cur.x + bitangent.x,
+								cur.y + bitangent.y,
+								cur.z + bitangent.z,
+							)
+
+							rlgl.Color4ub(0, 0, 255, 255)
+							rlgl.Vertex3f(cur.x, cur.y, cur.z)
+							rlgl.Vertex3f(cur.x + normal.x, cur.y + normal.y, cur.z + normal.z)
+
+						}
 					}
 				}
-			}
 
+			}
 
 			if g_mem.editor {
 				es := &g_mem.es
@@ -384,6 +537,8 @@ draw :: proc() {
 				switch es.track_edit_state {
 				case .Select:
 				case .Move:
+					rlgl.Begin(rlgl.LINES)
+					defer rlgl.End()
 					axes_buf: [2]rl.Vector3
 					colors_buf: [2]rl.Color
 					axes, colors := get_movement_axes(es.move_axis, &axes_buf, &colors_buf)
@@ -421,7 +576,6 @@ draw :: proc() {
 		points_len := len(points)
 
 		for i in 0 ..< points_len {
-
 			if spline_handle(g_mem.track.points[i], camera, es.selected_track_point == i) {
 				g_mem.es.selected_track_point = i
 			}