diff --git a/Cargo.lock b/Cargo.lock index 464d93a..b7fc4a3 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -47,7 +47,7 @@ version = "1.1.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "79947af37f4177cfead1110013d678905c37501914fba0efea834c3fe9a8d60c" dependencies = [ - "windows-sys", + "windows-sys 0.59.0", ] [[package]] @@ -57,7 +57,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2109dbce0e72be3ec00bed26e6a7479ca384ad226efdd66db8fa2e3a38c83125" dependencies = [ "anstyle", - "windows-sys", + "windows-sys 0.59.0", ] [[package]] @@ -75,6 +75,12 @@ version = "1.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" +[[package]] +name = "az" +version = "1.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b7e4c2464d97fe331d41de9d5db0def0a96f4d823b8b32a2efd503578988973" + [[package]] name = "bytemuck" version = "1.20.0" @@ -180,6 +186,16 @@ version = "0.28.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "779ae4bf7e8421cf91c0b3b64e7e8b40b862fba4d393f59150042de7c4965a94" +[[package]] +name = "gmp-mpfr-sys" +version = "1.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b0205cd82059bc63b63cf516d714352a30c44f2c74da9961dfda2617ae6b5918" +dependencies = [ + "libc", + "windows-sys 0.52.0", +] + [[package]] name = "godot" version = "0.2.1" @@ -276,6 +292,12 @@ version = "0.2.168" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5aaeb2981e0606ca11d79718f8bb01164f1d6ed75080182d3abf017e6d244b6d" +[[package]] +name = "libm" +version = "0.2.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8355be11b20d696c8f18f6cc018c4e372165b1fa8126cef092399c9951984ffa" + [[package]] name = "markdown" version = "1.0.0-alpha.21" @@ -343,6 +365,21 @@ version = "0.1.22" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e943b2c21337b7e3ec6678500687cdc741b7639ad457f234693352075c082204" +[[package]] +name = "ndarray" +version = "0.16.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "882ed72dce9365842bf196bdeedf5055305f11fc8c03dee7bb0194a6cad34841" +dependencies = [ + "matrixmultiply", + "num-complex", + "num-integer", + "num-traits", + "portable-atomic", + "portable-atomic-util", + "rawpointer", +] + [[package]] name = "num-bigint" version = "0.4.6" @@ -399,7 +436,10 @@ dependencies = [ "getset", "godot", "nalgebra", + "ndarray", "num-traits", + "roots", + "rug", ] [[package]] @@ -408,6 +448,21 @@ version = "1.0.15" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" +[[package]] +name = "portable-atomic" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "280dc24453071f1b63954171985a0b0d30058d287960968b9b2aca264c8d4ee6" + +[[package]] +name = "portable-atomic-util" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d8a2f0d8d040d7848a709caf78912debcc3f33ee4b3cac47d73d1e1069e83507" +dependencies = [ + "portable-atomic", +] + [[package]] name = "proc-macro-error-attr2" version = "2.0.0" @@ -483,6 +538,26 @@ version = "0.8.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2b15c43186be67a4fd63bee50d0303afffcef381492ebe2c5d87f324e1b8815c" +[[package]] +name = "roots" +version = "0.0.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "082f11ffa03bbef6c2c6ea6bea1acafaade2fd9050ae0234ab44a2153742b058" + +[[package]] +name = "rug" +version = "1.26.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "97ae2c1089ec0575193eb9222881310cc1ed8bce3646ef8b81b44b518595b79d" +dependencies = [ + "az", + "gmp-mpfr-sys", + "libc", + "libm", + "num-integer", + "num-traits", +] + [[package]] name = "safe_arch" version = "0.7.2" @@ -581,6 +656,15 @@ dependencies = [ "safe_arch", ] +[[package]] +name = "windows-sys" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" +dependencies = [ + "windows-targets", +] + [[package]] name = "windows-sys" version = "0.59.0" diff --git a/godot/Main.tscn b/godot/Main.tscn index 74561b3..5af587e 100644 --- a/godot/Main.tscn +++ b/godot/Main.tscn @@ -1,4 +1,4 @@ -[gd_scene load_steps=11 format=3 uid="uid://br738eihq2cus"] +[gd_scene load_steps=13 format=3 uid="uid://br738eihq2cus"] [ext_resource type="Script" path="res://main.gd" id="1_w5dqp"] [ext_resource type="PackedScene" uid="uid://c3vuka4b0smhh" path="res://control_point.tscn" id="2_o72i1"] @@ -20,6 +20,12 @@ LineEdit/colors/font_color = Color(0.875, 0.875, 0.875, 1) [sub_resource type="Theme" id="Theme_b8h1k"] +[sub_resource type="StandardMaterial3D" id="StandardMaterial3D_mwvwa"] +shading_mode = 0 +vertex_color_use_as_albedo = true + +[sub_resource type="ImmediateMesh" id="ImmediateMesh_oh57q"] + [node name="Main" type="Node3D"] script = ExtResource("1_w5dqp") control_point_scene = ExtResource("2_o72i1") @@ -44,6 +50,7 @@ mesh = SubResource("ImmediateMesh_hneuq") [node name="Anim" parent="." instance=ExtResource("4_s6s4g")] visible = false +script = null [node name="VBoxContainer" type="VBoxContainer" parent="."] offset_left = 40.0 @@ -69,6 +76,15 @@ text = "Optimizer" layout_mode = 2 text = "-- iter/s" +[node name="CheckBox" type="CheckBox" parent="VBoxContainer"] +layout_mode = 2 +text = "Manual Physics" + +[node name="AnimStepEdit" parent="VBoxContainer" instance=ExtResource("5_svsvn")] +layout_mode = 2 +_name = "Anim Step:" +value = 0.01 + [node name="LREdit" parent="VBoxContainer" instance=ExtResource("5_svsvn")] layout_mode = 2 _name = "LR:" @@ -125,10 +141,17 @@ use_native_dialog = true [node name="SaveDialogue" type="FileDialog" parent="."] size = Vector2i(303, 180) +ok_button_text = "Save" access = 2 use_native_dialog = true +[node name="HLNormal" type="MeshInstance3D" parent="."] +material_overlay = SubResource("StandardMaterial3D_mwvwa") +mesh = SubResource("ImmediateMesh_oh57q") + [connection signal="toggled" from="VBoxContainer/CheckButton" to="." method="_on_check_button_toggled"] +[connection signal="toggled" from="VBoxContainer/CheckBox" to="." method="_on_check_box_toggled"] +[connection signal="value_changed" from="VBoxContainer/AnimStepEdit" to="." method="_on_anim_step_edit_value_changed"] [connection signal="value_changed" from="VBoxContainer/LREdit" to="." method="_on_lr_edit_value_changed"] [connection signal="value_changed" from="VBoxContainer/MassEdit" to="." method="_on_mass_edit_value_changed"] [connection signal="value_changed" from="VBoxContainer/GravityEdit" to="." method="_on_gravity_edit_value_changed"] diff --git a/godot/anim.tscn b/godot/anim.tscn index 9ee5ef2..37172a7 100644 --- a/godot/anim.tscn +++ b/godot/anim.tscn @@ -1,4 +1,6 @@ -[gd_scene load_steps=4 format=3 uid="uid://dp1iulbnn6wvy"] +[gd_scene load_steps=8 format=3 uid="uid://dp1iulbnn6wvy"] + +[sub_resource type="GDScript" id="GDScript_gxknm"] [sub_resource type="StandardMaterial3D" id="StandardMaterial3D_38yrt"] diffuse_mode = 2 @@ -12,7 +14,17 @@ height = 2.0 [sub_resource type="SphereShape3D" id="SphereShape3D_r0wmb"] radius = 1.0 +[sub_resource type="StandardMaterial3D" id="StandardMaterial3D_q7vat"] +diffuse_mode = 2 + +[sub_resource type="CapsuleMesh" id="CapsuleMesh_ncu23"] +material = SubResource("StandardMaterial3D_q7vat") +height = 5.0 + +[sub_resource type="ImmediateMesh" id="ImmediateMesh_teg0u"] + [node name="Anim" type="Node3D"] +script = SubResource("GDScript_gxknm") [node name="MeshInstance3D" type="MeshInstance3D" parent="."] mesh = SubResource("SphereMesh_su52i") @@ -21,3 +33,10 @@ mesh = SubResource("SphereMesh_su52i") [node name="CollisionShape3D" type="CollisionShape3D" parent="StaticBody3D"] shape = SubResource("SphereShape3D_r0wmb") + +[node name="MeshInstance3D2" type="MeshInstance3D" parent="."] +transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 2.70093, 0) +mesh = SubResource("CapsuleMesh_ncu23") + +[node name="MeshInstance3D3" type="MeshInstance3D" parent="."] +mesh = SubResource("ImmediateMesh_teg0u") diff --git a/godot/control_point.gd b/godot/control_point.gd index f31377d..890eaa3 100644 --- a/godot/control_point.gd +++ b/godot/control_point.gd @@ -11,64 +11,64 @@ signal clicked(index) # Called when the node enters the scene tree for the first time. func _ready() -> void: - var mat: StandardMaterial3D = StandardMaterial3D.new() - mat.vertex_color_use_as_albedo = true - mat.shading_mode = BaseMaterial3D.SHADING_MODE_UNSHADED - axes.material_override = mat - var m: ImmediateMesh = axes.mesh - m.clear_surfaces() - #m.surface_begin(Mesh.PRIMITIVE_TRIANGLES) + var mat: StandardMaterial3D = StandardMaterial3D.new() + mat.vertex_color_use_as_albedo = true + mat.shading_mode = BaseMaterial3D.SHADING_MODE_UNSHADED + axes.material_override = mat + var m: ImmediateMesh = axes.mesh + m.clear_surfaces() + #m.surface_begin(Mesh.PRIMITIVE_TRIANGLES) - m.surface_begin(Mesh.PRIMITIVE_LINES) + m.surface_begin(Mesh.PRIMITIVE_LINES) - m.surface_set_color(Color.RED) - m.surface_add_vertex(Vector3.ZERO) - m.surface_set_color(Color.RED) - m.surface_add_vertex(Vector3(100, 0, 0)) + m.surface_set_color(Color.RED) + m.surface_add_vertex(Vector3.ZERO) + m.surface_set_color(Color.RED) + m.surface_add_vertex(Vector3(100, 0, 0)) - m.surface_set_color(Color.GREEN) - m.surface_add_vertex(Vector3.ZERO) - m.surface_set_color(Color.GREEN) - m.surface_add_vertex(Vector3(0, 100, 0)) + m.surface_set_color(Color.GREEN) + m.surface_add_vertex(Vector3.ZERO) + m.surface_set_color(Color.GREEN) + m.surface_add_vertex(Vector3(0, 100, 0)) - m.surface_set_color(Color.SKY_BLUE) - m.surface_add_vertex(Vector3.ZERO) - m.surface_set_color(Color.SKY_BLUE) - m.surface_add_vertex(Vector3(0, 0, 100)) + m.surface_set_color(Color.SKY_BLUE) + m.surface_add_vertex(Vector3.ZERO) + m.surface_set_color(Color.SKY_BLUE) + m.surface_add_vertex(Vector3(0, 0, 100)) - m.surface_end() + m.surface_end() # Called every frame. 'delta' is the elapsed time since the previous frame. func _process(_delta: float) -> void: - if selected or hovered: - scale = Vector3(2.0, 2.0, 2.0) - else: - scale = Vector3.ONE - axes.visible = selected + if selected or hovered: + scale = Vector3(2.0, 2.0, 2.0) + else: + scale = Vector3.ONE + axes.visible = selected func initialize(pos: Vector3, index_: int) -> void: - translate(pos) - index = index_ + translate(pos) + index = index_ ## Increase size when mouse hovering over control point func _on_static_body_3d_mouse_entered() -> void: - hovered = true + hovered = true ## Reset size when mouse leaves control point func _on_static_body_3d_mouse_exited() -> void: - hovered = false + hovered = false ## Handle mouse click on control point func _on_static_body_3d_input_event( - _camera: Node, - event: InputEvent, - _event_position: Vector3, - _normal: Vector3, - _shape_idx: int + _camera: Node, + event: InputEvent, + _event_position: Vector3, + _normal: Vector3, + _shape_idx: int ) -> void: - if event is InputEventMouseButton and event.button_index == MOUSE_BUTTON_LEFT and event.is_pressed(): - clicked.emit(index) + if event is InputEventMouseButton and event.button_index == MOUSE_BUTTON_LEFT and event.is_pressed(): + clicked.emit(index) diff --git a/godot/hi.json b/godot/hi.json new file mode 100644 index 0000000..8510629 --- /dev/null +++ b/godot/hi.json @@ -0,0 +1,102 @@ +[ + [ + 30, + 24, + 1 + ], + [ + 21, + 6, + 1 + ], + [ + 19, + 12, + 1 + ], + [ + 21, + 18, + 3 + ], + [ + 25, + 16, + 2 + ], + [ + 23, + 9, + 4 + ], + [ + 20, + 13, + 4 + ], + [ + 16, + 2, + 6 + ], + [ + 15, + 7, + 4 + ], + [ + 17, + 7, + 1 + ], + [ + 14, + 3, + 2 + ], + [ + 12, + 5, + 0 + ], + [ + 7, + 6, + 6 + ], + [ + 6, + 8, + 12 + ], + [ + 11, + 9, + 3 + ], + [ + 8, + 13, + 3 + ], + [ + 2, + 5, + 3 + ], + [ + 0, + 0, + 0 + ], + [ + 47, + 0, + 1 + ], + [ + 43, + 0, + 1 + ] +] \ No newline at end of file diff --git a/godot/main.gd b/godot/main.gd index 0b172c6..f06d3d6 100644 --- a/godot/main.gd +++ b/godot/main.gd @@ -3,9 +3,13 @@ extends Node3D # Used to show control points @export var control_point_scene: PackedScene +# Anim Vectors +@onready var hl_normal_mi: MeshInstance3D = $HLNormal + # Relevant child nodes @onready var camera: Camera3D = $PanOrbitCamera; @onready var basic_lines: MeshInstance3D = $BasicLines; + @onready var optimizer: Optimizer = $Optimizer; @onready var anim: Node3D = $Anim; @onready var label: Label = $VBoxContainer/MainStats; @@ -21,17 +25,27 @@ extends Node3D var selected_index; var selected_point; +var manual_physics = false var optimize: bool = false var control_points: Array[ControlPoint] var curve: CoasterCurve -var physics: CoasterPhysics +var physics: CoasterPhysicsV3 # default parameter values var learning_rate: float = 1.0 var mass: float = 1.00 var gravity: float = -0.01 var friction: float = 0.05 +var anim_step_size: float = 0.05 +var com_offset_mag: float = 1.0 + +# parameter editing +@onready var lr_edit: FloatEdit = $VBoxContainer/LREdit +@onready var mass_edit: FloatEdit = $VBoxContainer/MassEdit +@onready var gravity_edit: FloatEdit = $VBoxContainer/GravityEdit +@onready var friction_edit: FloatEdit = $VBoxContainer/FrictionEdit +@onready var anim_step_edit: FloatEdit = $VBoxContainer/AnimStepEdit ## Input is an array of Vector3 func set_points(points: Variant) -> void: @@ -88,6 +102,7 @@ func _ready() -> void: optimizer.set_gravity(gravity) optimizer.set_mu(friction) optimizer.set_lr(learning_rate) + optimizer.set_com_offset_mag(com_offset_mag) # ui setup x_edit.theme.set_color("font_color", "Label", Color.RED) @@ -97,8 +112,16 @@ func _ready() -> void: z_edit.theme.set_color("font_color", "Label", Color.SKY_BLUE) z_edit.theme.set_color("font_color", "LineEdit", Color.SKY_BLUE) + # more ui setup + lr_edit.set_value(learning_rate) + mass_edit.set_value(mass) + gravity_edit.set_value(gravity) + friction_edit.set_value(friction) + anim_step_edit.set_value(anim_step_size) + func _process(_delta: float) -> void: + # handle key input if Input.is_action_just_pressed("reset_curve"): var positions: Array[Vector3] = [] @@ -106,16 +129,56 @@ func _process(_delta: float) -> void: positions.push_back(cp.position) optimizer.set_points(positions) if Input.is_action_just_pressed("run_simulation"): + push_warning("hi2") curve = optimizer.get_curve() - physics = CoasterPhysics.create(mass, gravity, friction) + push_warning("hi3") + physics = CoasterPhysicsV3.create(mass, gravity, curve, 5.0) + push_warning("hi4") # update physics simulation if curve != null: anim.visible = true - physics.step(curve) - var anim_pos = physics.pos(curve) + if (!manual_physics || Input.is_action_just_pressed("step_physics")): + physics.step(curve, anim_step_size) + #var anim_pos = physics.pos(curve) + var anim_pos = physics.pos() + #var anim_vel = physics.velocity() + var anim_vel = physics.vel() + var anim_up = physics.hl_normal() + var m: ImmediateMesh = hl_normal_mi.mesh + m.clear_surfaces() + m.surface_begin(Mesh.PRIMITIVE_LINES) + + const MULT = 200; + + m.surface_set_color(Color.ORANGE) + m.surface_add_vertex(anim_pos) + m.surface_set_color(Color.ORANGE) + m.surface_add_vertex(anim_pos + MULT * physics.ag()) + + m.surface_set_color(Color.RED) + m.surface_add_vertex(anim_pos) + m.surface_set_color(Color.RED) + m.surface_add_vertex(anim_pos + MULT * physics.a()) + + m.surface_set_color(Color.YELLOW) + m.surface_add_vertex(anim_pos + MULT * physics.a()) + m.surface_set_color(Color.YELLOW) + m.surface_add_vertex(anim_pos + MULT * physics.a() - MULT * physics.g()) + + # blue velocity + m.surface_set_color(Color.BLUE) + m.surface_add_vertex(anim_pos) + m.surface_set_color(Color.BLUE) + m.surface_add_vertex(anim_pos + MULT * anim_vel) + + m.surface_end() + #var anim_up = Vector3.UP if anim_pos != null: - anim.position = anim_pos + if anim_pos != anim_pos + anim_vel: + anim.look_at_from_position(anim_pos, anim_pos + anim_vel, anim_up) + #anim.position = anim_pos + #anim. = Quaternion(anim_up, Vector3.UP).get_euler() else: anim.visible = false @@ -124,33 +187,54 @@ func _process(_delta: float) -> void: if len(curve_points) > 1: var m = basic_lines.mesh; m.clear_surfaces(); - m.surface_begin(Mesh.PRIMITIVE_TRIANGLES); + m.surface_begin(Mesh.PRIMITIVE_LINES); Utils.cylinder_line(m, optimizer.as_segment_points(), 0.2) m.surface_end(); # update ui - var format_values; - if physics != null: - format_values = [ - optimizer.cost(), - physics.speed(), - physics.accel(), - physics.g_force(), - physics.max_g_force(), - physics.cost() - ] + #var format_values; + #if physics != null: + # var v = physics.velocity() + # var n = physics.hl_normal() + # format_values = [ + # optimizer.cost(), + # physics.speed(), + # physics.accel(), + # physics.g_force(), + # physics.max_g_force(), + # physics.cost(), + # v, + # n, + # 90 - rad_to_deg(v.signed_angle_to(n, v.cross(n))) + # ] + #else: + # format_values = [ + # optimizer.cost(), + # 0.0, + # 0.0, + # 0.0, + # 0.0, + # 0.0, + # Vector3.ZERO, + # Vector3.ZERO, + # 0.0 + # ] + #label.text = """Cost: %.3f + + # Speed: %.3f + # Accel: %.3f + # Gs: %.3f + # Max Gs: %.3f + # Cost: %.3f + # Velocity: %.3v + # Normal Force: %.3v + # Angle Error: %.3f""" % format_values + if physics == null: + label.text = "physics not initialized" else: - format_values = [ - optimizer.cost(), - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - ] - label.text = "Cost: %.3f\n\nSpeed: %.3f\nAccel: %.3f\nGs: %.3f\nMax Gs: %.3f\nCost: %.3f" % format_values + label.text = physics.description(curve) var ips = optimizer.iters_per_second() if ips == null: optimizer_speed_label.text = "-- iter/s" @@ -220,21 +304,24 @@ func _on_control_point_clicked(index: int) -> void: func _on_x_edit_value_changed(value: float) -> void: - selected_point.set_x(value) - control_points[selected_index].position.x = value - optimizer.set_point(selected_index, selected_point) + if selected_point: + selected_point.set_x(value) + control_points[selected_index].position.x = value + optimizer.set_point(selected_index, selected_point) func _on_y_edit_value_changed(value: float) -> void: - selected_point.set_y(value) - control_points[selected_index].position.y = value - optimizer.set_point(selected_index, selected_point) + if selected_point: + selected_point.set_y(value) + control_points[selected_index].position.y = value + optimizer.set_point(selected_index, selected_point) func _on_z_edit_value_changed(value: float) -> void: - selected_point.set_z(value) - control_points[selected_index].position.z = value - optimizer.set_point(selected_index, selected_point) + if selected_point: + selected_point.set_z(value) + control_points[selected_index].position.z = value + optimizer.set_point(selected_index, selected_point) func _on_load_button_pressed() -> void: @@ -249,6 +336,7 @@ func _on_save_dialogue_file_selected(path: String) -> void: print(path) var file = FileAccess.open(path, FileAccess.WRITE) + print(FileAccess.get_open_error()) var diag = AcceptDialog.new() diag.content_scale_factor = 2 @@ -266,6 +354,9 @@ func _on_save_dialogue_file_selected(path: String) -> void: "\t" ) ) + print(file.get_error()) + file.close() + print(file.get_error()) func _on_load_dialogue_file_selected(path: String) -> void: @@ -300,7 +391,16 @@ func _on_load_dialogue_file_selected(path: String) -> void: var pts = json.map(func(i): return Vector3(i[0], i[1], i[2])) set_points(pts) + file.close() func _on_save_dialogue_confirmed() -> void: print("confirmed") + + +func _on_anim_step_edit_value_changed(value: float) -> void: + anim_step_size = value + + +func _on_check_box_toggled(toggled_on: bool) -> void: + manual_physics = toggled_on diff --git a/godot/project.godot b/godot/project.godot index 7937b2e..92ac758 100644 --- a/godot/project.godot +++ b/godot/project.godot @@ -17,8 +17,8 @@ config/icon="res://icon.svg" [display] -window/size/viewport_width=1600 -window/size/viewport_height=1200 +window/size/viewport_width=2000 +window/size/viewport_height=1600 [input] @@ -97,6 +97,11 @@ run_simulation={ "events": [Object(InputEventKey,"resource_local_to_scene":false,"resource_name":"","device":-1,"window_id":0,"alt_pressed":false,"shift_pressed":false,"ctrl_pressed":false,"meta_pressed":false,"pressed":false,"keycode":0,"physical_keycode":82,"key_label":0,"unicode":114,"location":0,"echo":false,"script":null) ] } +step_physics={ +"deadzone": 0.5, +"events": [Object(InputEventKey,"resource_local_to_scene":false,"resource_name":"","device":-1,"window_id":0,"alt_pressed":false,"shift_pressed":false,"ctrl_pressed":false,"meta_pressed":false,"pressed":false,"keycode":0,"physical_keycode":48,"key_label":0,"unicode":48,"location":0,"echo":false,"script":null) +] +} [rendering] diff --git a/hi2.json b/hi2.json new file mode 100644 index 0000000..1c9cb29 --- /dev/null +++ b/hi2.json @@ -0,0 +1 @@ +[[30, 24, 1], [21, 6, 1], [21, 7, 1]] \ No newline at end of file diff --git a/num_opt/Cargo.toml b/num_opt/Cargo.toml index 6c4ce58..cb00af1 100644 --- a/num_opt/Cargo.toml +++ b/num_opt/Cargo.toml @@ -11,7 +11,10 @@ clap = { version = "4.5.23", features = ["derive"] } getset = "0.1.3" godot = { version = "0.2.1", features = ["register-docs"] } nalgebra = "0.33.2" +ndarray = "0.16.1" num-traits = "0.2.19" +roots = "0.0.8" +rug = { version = "1.26.1", features = ["num-traits"] } [profile.dev.package."*"] opt-level = 3 diff --git a/num_opt/src/gd/gd_coaster_curve.rs b/num_opt/src/gd/gd_coaster_curve.rs index 55c8d37..794f488 100644 --- a/num_opt/src/gd/gd_coaster_curve.rs +++ b/num_opt/src/gd/gd_coaster_curve.rs @@ -1,11 +1,11 @@ use godot::prelude::*; -use crate::hermite; +use crate::{hermite, my_float::MyFloatType}; /// Wrapper around hermite::Spline /// A handle opaque to GDScript #[derive(GodotClass)] #[class(init)] pub struct CoasterCurve { - pub inner: hermite::Spline, + pub inner: hermite::Spline, } \ No newline at end of file diff --git a/num_opt/src/gd/gd_coaster_physics.rs b/num_opt/src/gd/gd_coaster_physics.rs index 89e8867..d8e1fa6 100644 --- a/num_opt/src/gd/gd_coaster_physics.rs +++ b/num_opt/src/gd/gd_coaster_physics.rs @@ -1,34 +1,49 @@ use godot::prelude::*; +use crate::my_float::MyFloatType; use crate::physics; +use crate::my_float::MyFloat; -use super::CoasterCurve; + +use super::{myvec_to_gd, na_to_gd, CoasterCurve}; /// Wrapper around physics::PhysicsState #[derive(GodotClass)] #[class(init)] +#[deprecated] pub struct CoasterPhysics { - inner: Option, + inner: Option, } #[godot_api] impl CoasterPhysics { /// Initialize with mass and gravity #[func] - fn create(mass: f64, gravity: f64, mu: f64) -> Gd { + fn create(mass: f64, gravity: f64, mu: f64, com_offset_mag: f64) -> Gd { Gd::from_object(Self { - inner: Some(physics::PhysicsState::new(mass, gravity, mu)), + inner: Some(physics::legacy::PhysicsState::new( + mass, + gravity, + mu, + com_offset_mag, + )), }) } /// Progress the simulation given a curve #[func] - fn step(&mut self, curve: Gd) { + fn step(&mut self, curve: Gd, step_size: f64) { if let Some(phys) = &mut self.inner { let curve = &curve.bind().inner; let u = phys.u(); - if let Some((dxdu, dydu, dzdu)) = curve.curve_1st_derivative_at(u) { - phys.step(dxdu, dydu, dzdu, physics::StepBehavior::Time); + if let Some(drdu) = curve.curve_1st_derivative_at(&u) { + phys.step( + drdu.x.to_f64(), + drdu.y.to_f64(), + drdu.z.to_f64(), + physics::StepBehavior::Time, + step_size, + ); } } } @@ -37,11 +52,124 @@ impl CoasterPhysics { #[func] fn pos(&self, curve: Gd) -> Variant { if let Some(phys) = &self.inner - && let Some(v) = curve.bind().inner.curve_at(phys.u()) + && let Some(pos) = phys.com_pos(&curve.bind().inner) + { + Variant::from(myvec_to_gd(&pos)) + } else { + Variant::nil() + } + } + + /// Current speed + #[func] + fn speed(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(phys.speed()) + } else { + Variant::nil() + } + } + + /// Current acceleration (scaler) + #[func] + fn accel(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(phys.a()) + } else { + Variant::nil() + } + } + + /// Current g force + #[func] + fn g_force(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(-phys.a() / phys.gravity()) + } else { + Variant::nil() + } + } + + /// Max g force experienced + #[func] + fn max_g_force(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(phys.max_g_force()) + } else { + Variant::nil() + } + } + + /// Accumulating cost value + #[func] + fn cost(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(phys.cost()) + } else { + Variant::nil() + } + } + + /// Velocity vector + #[func] + fn velocity(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(na_to_gd(phys.v())) + } else { + Variant::nil() + } + } + + /// Normal force vector + #[func] + fn normal_force(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(na_to_gd(phys.normal_force())) + } else { + Variant::nil() + } + } +} + +/*/// Wrapper around physics::PhysicsStateV2 +#[derive(GodotClass)] +#[class(init)] +#[deprecated] +pub struct CoasterPhysicsV2 { + inner: Option, +} + +#[godot_api] +impl CoasterPhysicsV2 { + /// Initialize with mass and gravity + #[func] + fn create(mass: f64, gravity: f64, com_offset_mag: f64) -> Gd { + Gd::from_object(Self { + inner: Some(physics::legacy::PhysicsStateV2::new( + mass, + na::Vector3::new(0.0, gravity, 0.0), + com_offset_mag, + )), + }) + } + + /// Progress the simulation given a curve + #[func] + fn step(&mut self, curve: Gd, step_size: f64) { + if let Some(phys) = &mut self.inner { + let curve = &curve.bind().inner; + phys.step(curve, step_size, physics::StepBehavior::Time); + } + } + + /// Current position + #[func] + fn pos(&self, curve: Gd) -> Variant { + if let Some(phys) = &self.inner + && let Some(pos) = phys.com_pos(&curve.bind().inner) { - Variant::from(Vector3::new(v.0 as f32, v.1 as f32, v.2 as f32)) + Variant::from(na_to_gd(pos)) } else { - //godot_error!("pos called on empty Physics, or u out of range"); Variant::nil() } } @@ -95,4 +223,119 @@ impl CoasterPhysics { Variant::nil() } } -} \ No newline at end of file + + /// Velocity vector + #[func] + fn velocity(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(na_to_gd(phys.v())) + } else { + Variant::nil() + } + } + + #[func] + fn hl_normal(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(na_to_gd(phys.hl_normal())) + } else { + Variant::nil() + } + } +} +*/ +/// Wrapper around physics::PhysicsStateV3 +#[derive(GodotClass)] +#[class(init)] +pub struct CoasterPhysicsV3 { + inner: Option>, +} + +#[godot_api] +impl CoasterPhysicsV3 { + /// Initialize with mass and gravity + #[func] + fn create(mass: f64, gravity: f64, curve: Gd, o: f64) -> Gd { + Gd::from_object(Self { + inner: Some(physics::PhysicsStateV3::new( + mass, + gravity, + &curve.bind().inner, + o, + )), + }) + } + + #[func] + fn step(&mut self, curve: Gd, step_size: f64) { + if let Some(phys) = &mut self.inner { + let curve = &curve.bind().inner; + if phys.step(MyFloatType::from_f64(step_size), curve, physics::StepBehavior::Constant).is_none() { + //godot_warn!("Simulation Stuck"); + }; + } + } + + #[func] + fn description(&self, curve: Gd) -> String { + if let Some(phys) = &self.inner { + phys.description(&curve.bind().inner) + } else { + String::new() + } + } + + #[func] + fn pos(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(myvec_to_gd(phys.pos())) + } else { + Variant::nil() + } + } + + #[func] + fn vel(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(myvec_to_gd(phys.vel())) + } else { + Variant::nil() + } + } + + #[func] + fn hl_normal(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(myvec_to_gd(phys.hl_normal())) + } else { + Variant::nil() + } + } + + #[func] + fn ag(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(myvec_to_gd(phys.ag())) + } else { + Variant::nil() + } + } + + #[func] + fn a(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(myvec_to_gd(phys.a())) + } else { + Variant::nil() + } + } + + #[func] + fn g(&self) -> Variant { + if let Some(phys) = &self.inner { + Variant::from(myvec_to_gd(phys.g())) + } else { + Variant::nil() + } + } +} diff --git a/num_opt/src/gd/gd_optimizer.rs b/num_opt/src/gd/gd_optimizer.rs index a5e15e5..e2471f9 100644 --- a/num_opt/src/gd/gd_optimizer.rs +++ b/num_opt/src/gd/gd_optimizer.rs @@ -4,9 +4,11 @@ use std::{ time::Instant, }; -use crate::{hermite, optimizer, physics, point}; +use crate::{hermite, my_float::{MyFloat, MyFloatType}, optimizer, physics::{self, float}, point}; use godot::prelude::*; use num_traits::AsPrimitive; +use crate::physics::PRECISION; +use rug::Float; use super::{CoasterCurve, CoasterPoint}; @@ -25,6 +27,7 @@ enum ToWorker { SetGravity(f64), SetMu(f64), SetLR(f64), + SetComOffsetMag(f64), } /// Messages gotten by an `Optimizer` from its worker thread @@ -38,7 +41,7 @@ enum FromWorker { #[class(base=Node)] pub struct Optimizer { points: Vec>, - curve: hermite::Spline, + curve: hermite::Spline, segment_points_cache: Option>, from_worker: Mutex>, to_worker: mpsc::Sender, @@ -52,6 +55,7 @@ impl INode for Optimizer { /// Starts up the worker thread and sets point and curve to empty values fn init(_base: Base) -> Self { godot_print!("Hello from Optimizer!"); + godot_print!("{}", float!(1e-14f64)); let (to_worker_tx, to_worker_rx) = mpsc::channel::(); let (to_main_tx, to_main_rx) = mpsc::channel(); @@ -62,11 +66,12 @@ impl INode for Optimizer { std::thread::spawn(move || { let mut active = false; let mut points = vec![]; - let mut curve = Default::default(); + let mut curve: hermite::Spline = Default::default(); let mut mass = None; let mut gravity = None; let mut mu = None; let mut lr = None; + let mut com_offset_mag = None; loop { while let Ok(msg) = if active { // avoid blocking if inactive @@ -92,6 +97,7 @@ impl INode for Optimizer { ToWorker::SetGravity(v) => gravity = Some(v), ToWorker::SetMu(v) => mu = Some(v), ToWorker::SetLR(v) => lr = Some(v), + ToWorker::SetComOffsetMag(v) => com_offset_mag = Some(v) } } if active @@ -99,9 +105,10 @@ impl INode for Optimizer { && let Some(gravity) = gravity && let Some(mu) = mu && let Some(lr) = lr + && let Some(com_offset_mag) = com_offset_mag { let prev_cost = optimizer::optimize( - &physics::PhysicsState::new(mass, gravity, mu), + &physics::legacy::PhysicsState::new(mass, gravity, mu, com_offset_mag), &curve, &mut points, lr, @@ -189,12 +196,13 @@ impl Optimizer { .iter_shared() .map(|p| point::Point::new(p.x.as_f64(), p.y.as_f64(), p.z.as_f64())) .collect(); + hermite::set_derivatives_using_catmull_rom(&mut self.points); self.curve = hermite::Spline::new(&self.points); let _ = self .to_worker .send(ToWorker::SetPoints( self.points.clone(), - Derivatives::Ignore, + Derivatives::Keep, )) .map_err(|e| godot_error!("{:#?}", e)); } @@ -210,11 +218,20 @@ impl Optimizer { #[func] fn set_point(&mut self, i: i32, point: Gd) { self.points[i as usize] = point.bind().inner().clone(); + self.curve = hermite::Spline::new(&self.points); + self.segment_points_cache = None; self.to_worker .send(ToWorker::SetPoints(self.points.clone(), Derivatives::Keep)) .unwrap(); } + #[func] + fn set_com_offset_mag(&mut self, mag: f64) { + self.to_worker + .send(ToWorker::SetComOffsetMag(mag)) + .unwrap(); + } + /// Enable the optimizer #[func] fn enable_optimizer(&mut self) { @@ -246,7 +263,7 @@ impl Optimizer { .map(|params| { let pts = hermite::curve_points(params, NonZero::new(10).unwrap()); pts.iter() - .map(|(x,y,z)| Vector3::new(x.as_(), y.as_(), z.as_())) + .map(|(x, y, z)| Vector3::new(x.as_(), y.as_(), z.as_())) .collect::>() }) .flatten() diff --git a/num_opt/src/gd/mod.rs b/num_opt/src/gd/mod.rs index 22b912d..50c0a2f 100644 --- a/num_opt/src/gd/mod.rs +++ b/num_opt/src/gd/mod.rs @@ -11,3 +11,13 @@ pub use gd_coaster_curve::CoasterCurve; #[allow(unused_imports)] pub use gd_coaster_physics::CoasterPhysics; pub use gd_coaster_point::CoasterPoint; + +use crate::{my_float::MyFloat, physics::linalg::MyVector3}; + +pub fn na_to_gd(v: na::Vector3) -> godot::prelude::Vector3 { + godot::prelude::Vector3::new(v.x as f32, v.y as f32, v.z as f32) +} + +pub fn myvec_to_gd(v: &MyVector3) -> godot::prelude::Vector3 { + godot::prelude::Vector3::new(v.x.to_f64() as f32, v.y.to_f64() as f32, v.z.to_f64() as f32) +} diff --git a/num_opt/src/hermite.rs b/num_opt/src/hermite.rs index 2bf4beb..4920f9a 100644 --- a/num_opt/src/hermite.rs +++ b/num_opt/src/hermite.rs @@ -2,18 +2,203 @@ //! Initializing their derivatives using Catmull-Rom //! Getting position and derivative values of the splines +use std::array; // ensure segments for curve sampling are not zero use std::num::NonZeroU32; +use std::ops::{Add, Mul}; +use ndarray::Array1; +use num_traits::Pow; +use rug::ops::CompleteRound; +use rug::Float; + +use crate::my_float::MyFloat; // Refer to a custom module that defines a Point struct used in splines. -use crate::point; +use crate::{physics::float, point}; + +/// Create an 8X8 matrix to interplolate Hermite splines. +/// This matrix transforms given points, tangents, and higher +/// derivatives into polynomial coefficients(x(t),y(t),z(t)). +/// It operates on one dimension at a time, as we don't have tensors. +//#[rustfmt::skip] +use crate::physics::{linalg::MyVector3, PRECISION}; + +fn get_matrix_p() -> ndarray::Array2 { + ndarray::arr2(&vec![ + [ + float!(20.0), + float!(10.0), + float!(2.0), + float!(1.0, 6.0), + float!(-20.0), + float!(10.0), + float!(-2.0), + float!(1.0, 6.0), + ], + [ + float!(-70.0), + float!(-36.0), + float!(-15.0, 2.0), + float!(-2.0, 3.0), + float!(70.0), + float!(-34.0), + float!(13.0, 2.0), + float!(-1.0, 2.0), + ], + [ + float!(84.0), + float!(45.0), + float!(10.0), + float!(1.0), + float!(-84.0), + float!(39.0), + float!(-7.0), + float!(1.0, 2.0), + ], + [ + float!(-35.0), + float!(-20.0), + float!(-5.0), + float!(-2.0, 3.0), + float!(35.0), + float!(-15.0), + float!(5.0, 2.0), + float!(-1.0, 6.0), + ], + [ + float!(0.0), + float!(0.0), + float!(0.0), + float!(1.0 / 6.0), + float!(0.0), + float!(0.0), + float!(0.0), + float!(0.0), + ], + [ + float!(0.0), + float!(0.0), + float!(1.0, 2.0), + float!(0.0), + float!(0.0), + float!(0.0), + float!(0.0), + float!(0.0), + ], + [ + float!(0.0), + float!(1.0), + float!(0.0), + float!(0.0), + float!(0.0), + float!(0.0), + float!(0.0), + float!(0.0), + ], + [ + float!(1.0), + float!(0.0), + float!(0.0), + float!(0.0), + float!(0.0), + float!(0.0), + float!(0.0), + float!(0.0), + ], + ]) +} + +fn get_matrix() -> ndarray::Array2 { + ndarray::arr2(&vec![ + [ + (20.0), + (10.0), + (2.0), + (1.0 / 6.0), + (-20.0), + (10.0), + (-2.0), + (1.0 / 6.0), + ], + [ + (-70.0), + (-36.0), + (-15.0 / 2.0), + (-2.0 / 3.0), + (70.0), + (-34.0), + (13.0 / 2.0), + (-1.0 / 2.0), + ], + [ + (84.0), + (45.0), + (10.0), + (1.0), + (-84.0), + (39.0), + (-7.0), + (1.0 / 2.0), + ], + [ + (-35.0), + (-20.0), + (-5.0), + (-2.0 / 3.0), + (35.0), + (-15.0), + (5.0 / 2.0), + (-1.0 / 6.0), + ], + [ + (0.0), + (0.0), + (0.0), + (1.0 / 6.0), + (0.0), + (0.0), + (0.0), + (0.0), + ], + [ + (0.0), + (0.0), + (1.0 / 2.0), + (0.0), + (0.0), + (0.0), + (0.0), + (0.0), + ], + [ + (0.0), + (1.0), + (0.0), + (0.0), + (0.0), + (0.0), + (0.0), + (0.0), + ], + [ + (1.0), + (0.0), + (0.0), + (0.0), + (0.0), + (0.0), + (0.0), + (0.0), + ], + ]) +} /// Create an 8X8 matrix to interplolate Hermite splines. /// This matrix transforms given points, tangents, and higher /// derivatives into polynomial coefficients(x(t),y(t),z(t)). /// It operates on one dimension at a time, as we don't have tensors. #[rustfmt::skip] -fn get_matrix() -> na::SMatrix { +fn get_matrix_precise() -> na::SMatrix { na::SMatrix::::from_row_slice(&vec![ 20.0, 10.0, 2.0, 1.0/6.0, -20.0, 10.0, -2.0, 1.0/6.0, -70.0, -36.0, -15.0/2.0, -2.0/3.0, 70.0, -34.0, 13.0/2.0, -1.0/2.0, @@ -26,36 +211,35 @@ fn get_matrix() -> na::SMatrix { ]) } -/// Simplify defining functions to calculate position(x,y,z) -/// or its derivatives from the coefficients. -/// Uses a polynomial representation. -macro_rules! curve_params_getter { - ($name:ident, $c:expr, $v:ident) => { - /// Evaluates the polynomial defined by the coefficients in `$c` at `u`, - /// using the $v coordinate - pub fn $name(&self, u: f64) -> f64 { - $c.iter() - .zip(self.$v) - .map(|((coeff, power), param)| *coeff * param * u.powi(*power)) - .sum() - } - }; -} - /// A hermite spline, each curve parameterized by CurveParms. #[derive(Clone)] -pub struct Spline { - params: Vec, +pub struct Spline where T: MyFloat { + params: Vec>, } -impl Default for Spline { +impl Default for Spline where T: MyFloat { /// Creates an empty spline fn default() -> Self { Self { params: vec![] } } } -impl Spline { +macro_rules! spline_getter { + ($self:ident, $x_get:ident, $y_get:ident, $z_get:ident, $u:expr) => {{ + let (i, rem) = $self.u_to_i_rem($u); + if i >= $self.params.len() { + return None; + } + + Some(MyVector3::new( + $self.params[i].$x_get(&rem), + $self.params[i].$y_get(&rem), + $self.params[i].$z_get(&rem), + )) + }}; +} + +impl Spline where T: MyFloat { /// Creates a spline from the given points /// For each pair of points, we need to find a Hermite polynomial /// that smoothly interpolates between them. @@ -70,8 +254,12 @@ impl Spline { Self { params } } + pub fn max_u(&self) -> f64 { + self.params.len() as f64 - 0.00001 + } + /// Iterate through the hermite curves of the spline - pub fn iter<'a>(&'a self) -> impl Iterator + use<'a> { + pub fn iter<'a>(&'a self) -> impl Iterator> { self.params.iter() } @@ -81,47 +269,70 @@ impl Spline { /// while `u = 1` corresponds to the end of the first curve. /// This method evaluates the position by taking the `floor(u)`-th curve /// in the spline, and taking its position at `u - floor(u)`. - pub fn curve_at(&self, u: f64) -> Option<(f64, f64, f64)> { - let i = u.floor(); - let rem = u - i; - let i = i as usize; + pub fn curve_at(&self, u: &T) -> Option> { + let i = u.clone().floor(); + let rem = u.clone() - i.clone(); + let i = i.to_f64() as usize; if i >= self.params.len() { return None; } - Some(( - self.params[i].x_d0(rem), - self.params[i].y_d0(rem), - self.params[i].z_d0(rem), + Some(MyVector3::new( + self.params[i].x_d0(&rem), + self.params[i].y_d0(&rem), + self.params[i].z_d0(&rem), )) } + fn u_to_i_rem(&self, u: &T) -> (usize, T) { + let i = u.clone().floor(); + let rem = u.clone() - i.clone(); + let i = i.to_f64() as usize; + (i, rem) + } + /// Find the 1st derivative ("velocity") of the spline at `u` - pub fn curve_1st_derivative_at(&self, u: f64) -> Option<(f64, f64, f64)> { - let i = u.floor(); - let rem = u - i; - let i = i as usize; - if i >= self.params.len() { - return None; - } + pub fn curve_1st_derivative_at(&self, u: &T) -> Option> { + spline_getter!(self, x_d1, y_d1, z_d1, u) + } - Some(( - self.params[i].x_d1(rem), - self.params[i].y_d1(rem), - self.params[i].z_d1(rem), - )) + /// Find the 2nd derivative ("acceleration") of the spline at `u` + pub fn curve_2nd_derivative_at(&self, u: &T) -> Option> { + spline_getter!(self, x_d2, y_d2, z_d2, u) + } + + /// Find the 3rd derivative ("jerk") of the spline at `u` + pub fn curve_3rd_derivative_at(&self, u: &T) -> Option> { + spline_getter!(self, x_d3, y_d3, z_d3, u) + } + + /// Find the 4th derivative ("snap") of the spline at `u` + pub fn curve_4th_derivative_at(&self, u: &T) -> Option> { + spline_getter!(self, x_d4, y_d4, z_d4, u) } } /// A single hermite curve #[derive(Clone)] -pub struct CurveParams { - x: [f64; 8], // x(t) = x[0] * t^7 + x[1] * t^6 + ... + x[7] * t^0 - y: [f64; 8], // y(t) = y[0] * t^7 + y[1] * t^6 + ... + y[7] * t^0 - z: [f64; 8], // z(t) = z[0] * t^7 + z[1] * t^6 + ... + z[7] * t^0 +pub struct CurveParams where T: MyFloat { + x: [T; 8], // x(t) = x[0] * t^7 + x[1] * t^6 + ... + x[7] * t^0 + y: [T; 8], // y(t) = y[0] * t^7 + y[1] * t^6 + ... + y[7] * t^0 + z: [T; 8], // z(t) = z[0] * t^7 + z[1] * t^6 + ... + z[7] * t^0 } // Stores polynomial coefficients for x(t), y(t), z(t), each up to t7. -impl CurveParams { +impl CurveParams where T: MyFloat { + /// Create a new hermite curve + pub fn new(x: Box<[T]>, y: Box<[T]>, z: Box<[T]>) -> Self // -> CurveParams[Float], y: [Float], z: [Float]) -> Self { + { + assert_eq!(x.len(), 8); + assert_eq!(y.len(), 8); + assert_eq!(z.len(), 8); + let x = array::from_fn(|i| x[i].clone()); + let y = array::from_fn(|i| y[i].clone()); + let z = array::from_fn(|i| z[i].clone()); + Self { x, y, z } + } + // coefficents and power /// position const D0: [(f64, i32); 8] = [ @@ -144,6 +355,50 @@ impl CurveParams { (2.0, 1), (1.0, 0), ]; + /// acceleration + const D2: [(f64, i32); 6] = [ + (7.0 * 6.0, 5), + (6.0 * 5.0, 4), + (5.0 * 4.0, 3), + (4.0 * 3.0, 2), + (3.0 * 2.0, 1), + (2.0, 0), + ]; + /// jerk + const D3: [(f64, i32); 5] = [ + (7.0 * 6.0 * 5.0, 4), + (6.0 * 5.0 * 4.0, 3), + (5.0 * 4.0 * 3.0, 2), + (4.0 * 3.0 * 2.0, 1), + (3.0 * 2.0, 0), + ]; + /// snap + const D4: [(f64, i32); 4] = [ + (7.0 * 6.0 * 5.0 * 4.0, 3), + (6.0 * 5.0 * 4.0 * 3.0, 2), + (5.0 * 4.0 * 3.0 * 2.0, 1), + (4.0 * 3.0 * 2.0, 0), + ]; +} + +/// Simplify defining functions to calculate position(x,y,z) +/// or its derivatives from the coefficients. +/// Uses a polynomial representation. +macro_rules! curve_params_getter { + ($name:ident, $c:expr, $v:ident) => { + /// Evaluates the polynomial defined by the coefficients in `$c` at `u`, + /// using the $v coordinate + pub fn $name(&self, u: &T) -> T { + use rug::ops::Pow; + $c.iter() + .zip(&self.$v) + .map(|((coeff, power), param)| param.clone() * *coeff * u.clone().pow(*power)) + .fold(T::from_f64(0.0), |acc, x| acc + x) + } + }; +} + +impl CurveParams where T: MyFloat { // getters for position and 1st derivative curve_params_getter!(x_d0, Self::D0, x); @@ -152,21 +407,59 @@ impl CurveParams { curve_params_getter!(x_d1, Self::D1, x); curve_params_getter!(y_d1, Self::D1, y); curve_params_getter!(z_d1, Self::D1, z); + curve_params_getter!(x_d2, Self::D2, x); + curve_params_getter!(y_d2, Self::D2, y); + curve_params_getter!(z_d2, Self::D2, z); + curve_params_getter!(x_d3, Self::D3, x); + curve_params_getter!(y_d3, Self::D3, y); + curve_params_getter!(z_d3, Self::D3, z); + curve_params_getter!(x_d4, Self::D4, x); + curve_params_getter!(y_d4, Self::D4, y); + curve_params_getter!(z_d4, Self::D4, z); } /// Given two points, finds a hermite curve interpolating them. /// This step constructs the coefficients of the Hermite polynomial /// that interpolates two points, ensuring that the curve satisfies /// conditions for position, velocity, accerlation, and jerk continuity. -pub fn solve(p: &point::Point, q: &point::Point) -> CurveParams { +pub fn solve(p: &point::Point, q: &point::Point) -> CurveParams where T: MyFloat { let m = get_matrix(); - type SMatrix8x1 = na::SMatrix; - let x_in = SMatrix8x1::from_row_slice(&[p.x, p.xp, p.xpp, p.xppp, q.x, q.xp, q.xpp, q.xppp]); - let y_in = SMatrix8x1::from_row_slice(&[p.y, p.yp, p.ypp, p.yppp, q.y, q.yp, q.ypp, q.yppp]); - let z_in = SMatrix8x1::from_row_slice(&[p.z, p.zp, p.zpp, p.zppp, q.z, q.zp, q.zpp, q.zppp]); - let x_out = m * x_in; - let y_out = m * y_in; - let z_out = m * z_in; + //type SMatrix8x1 = na::SMatrix; + // let x_in = SMatrix8x1::from_row_slice(&[p.x, p.xp, p.xpp, p.xppp, q.x, q.xp, q.xpp, q.xppp]); + let x_in = ndarray::arr1(&[ + p.x, + p.xp, + p.xpp, + p.xppp, + q.x, + q.xp, + q.xpp, + q.xppp, + ]); + let y_in = ndarray::arr1(&[ + p.y, + p.yp, + p.ypp, + p.yppp, + q.y, + q.yp, + q.ypp, + q.yppp, + ]); + let z_in = ndarray::arr1(&[ + p.z, + p.zp, + p.zpp, + p.zppp, + q.z, + q.zp, + q.zpp, + q.zppp, + ]); + + let x_out = m.dot(&x_in); + let y_out = m.dot(&y_in); + let z_out = m.dot(&z_in); //z_in; /*print!("("); for (i, p) in x_out.iter().enumerate() { @@ -177,25 +470,25 @@ pub fn solve(p: &point::Point, q: &point::Point) -> CurveParams { print!("{} * t^{} + ", p, 7 - i); } println!("0)");*/ - CurveParams { - x: x_out.data.0[0], - y: y_out.data.0[0], - z: z_out.data.0[0], - } + CurveParams::new( + x_out.into_iter().map(|x| T::from_f64(x)).collect(), + y_out.into_iter().map(|x| T::from_f64(x)).collect(), + z_out.into_iter().map(|x| T::from_f64(x)).collect(), + ) } /// Samples a hermite curve, splitting it into `segments` segments /// The segments are __not__ equal in length -pub fn curve_points(params: &CurveParams, segments: NonZeroU32) -> Vec<(f64, f64, f64)> { +pub fn curve_points(params: &CurveParams, segments: NonZeroU32) -> Vec<(f64, f64, f64)> where T: MyFloat { (0..segments.get() + 1) .map(|t| { - let t: f64 = t as f64 / segments.get() as f64; - let x = params.x_d0(t); - let y = params.y_d0(t); - let z = params.z_d0(t); + let t = T::from_f64_fraction(t as f64, segments.get() as f64); + let x = params.x_d0(&t); + let y = params.y_d0(&t); + let z = params.z_d0(&t); // just get points on the curve - (x, y, z) + (x.to_f64(), y.to_f64(), z.to_f64()) }) .collect() } @@ -237,8 +530,8 @@ pub fn catmull_rom_recursive(values: &Vec, coeff: f64, depth: u32) -> Vec>) { const SCALE: f64 = 0.5; diff --git a/num_opt/src/lib.rs b/num_opt/src/lib.rs index 9708f33..e75f4ff 100644 --- a/num_opt/src/lib.rs +++ b/num_opt/src/lib.rs @@ -1,4 +1,5 @@ #![feature(let_chains, array_windows)] +//#![recursion_limit = "2048"] extern crate nalgebra as na; @@ -8,6 +9,7 @@ mod hermite; mod optimizer; mod physics; mod point; +mod my_float; // this adds all the godot exports mod gd; diff --git a/num_opt/src/my_float.rs b/num_opt/src/my_float.rs new file mode 100644 index 0000000..253f6ae --- /dev/null +++ b/num_opt/src/my_float.rs @@ -0,0 +1,164 @@ +//! Generic float type support for `rug::Float` and `f64` +//! The float does not have to be `Copy`, allowing `rug::Float` to be used + +use std::{ + fmt::Display, + ops::{Add, Div, Mul, Neg, Sub}, +}; + +use num_traits::Pow; +use rug::Float; + +use crate::physics::{float, PRECISION}; + +pub type MyFloatType = f64; + +pub trait MyFloat: + Clone + + std::fmt::Debug + Display + + Add + + Pow + + Mul + + Add + + Mul + + Sub + + Div + Neg + + PartialEq + PartialOrd + PartialOrd +{ + + fn precision() -> u32; + fn from_f64(f: f64) -> Self; + fn from_f64_fraction(n: f64, d: f64) -> Self; + fn to_f64(&self) -> f64; + fn floor(&self) -> Self; + fn sqrt(&self) -> Self; + fn max(&self, other: &Self) -> Self; + fn min(&self, other: &Self) -> Self; + fn clamp(&self, min: f64, max: f64) -> Self; + fn sin(&self) -> Self; + fn cos(&self) -> Self; + fn acos(&self) -> Self; + fn is_nan(&self) -> bool; + fn abs(&self) -> Self; +} + +pub trait MyIncompleteFloat { + fn complete(&self, precision: u32) -> Completed; +} + +impl MyFloat for f64 { + + fn precision() -> u32 { + 0 + } + + fn from_f64(f: f64) -> Self { + f + } + + fn from_f64_fraction(n: f64, d: f64) -> Self { + n / d + } + + fn to_f64(&self) -> f64 { + *self + } + + fn floor(&self) -> Self { + f64::floor(*self) + } + + fn sqrt(&self) -> Self { + f64::sqrt(*self) + } + + fn clamp(&self, min: f64, max: f64) -> Self { + f64::clamp(*self, min, max) + } + + fn sin(&self) -> Self { + f64::sin(*self) + } + + fn cos(&self) -> Self { + f64::cos(*self) + } + + fn acos(&self) -> Self { + f64::acos(*self) + } + + fn is_nan(&self) -> bool { + f64::is_nan(*self) + } + + fn max(&self, other: &Self) -> Self { + f64::max(*self, *other) + } + + fn min(&self, other: &Self) -> Self { + f64::min(*self, *other) + } + + fn abs(&self) -> Self { + f64::abs(*self) + } +} + +impl MyFloat for Float { + fn precision() -> u32 { + PRECISION + } + + fn from_f64(f: f64) -> Self { + float!(f) + } + + fn from_f64_fraction(n: f64, d: f64) -> Self { + float!(n / d) + } + + fn to_f64(&self) -> f64 { + Float::to_f64(self) + } + + fn floor(&self) -> Self { + Float::floor(self.clone()) + } + + fn sqrt(&self) -> Self { + Float::sqrt(self.clone()) + } + + fn clamp(&self, min: f64, max: f64) -> Self { + Float::clamp(self.clone(), &min,& max) + } + + fn sin(&self) -> Self { + Float::sin(self.clone()) + } + + fn cos(&self) -> Self { + Float::cos(self.clone()) + } + + fn acos(&self) -> Self { + Float::acos(self.clone()) + } + + fn is_nan(&self) -> bool { + Float::is_nan(self) + } + + fn max(&self, other: &Self) -> Self { + Float::max(self.clone(), other) + } + + fn min(&self, other: &Self) -> Self { + Float::min(self.clone(), other) + } + + fn abs(&self) -> Self { + Float::abs(self.clone()) + } +} \ No newline at end of file diff --git a/num_opt/src/optimizer.rs b/num_opt/src/optimizer.rs index 8405807..7081632 100644 --- a/num_opt/src/optimizer.rs +++ b/num_opt/src/optimizer.rs @@ -1,6 +1,8 @@ //! Calculates the cost of a curve, and performs gradient descent to optimize it -use crate::{hermite, physics, point}; +use crate::{hermite, my_float::MyFloat, physics::{self, float}, point}; +use rug::Float; +use crate::physics::PRECISION; /// Given initial state and curve, calculates the total cost of the curve /// @@ -31,10 +33,10 @@ use crate::{hermite, physics, point}; /// /// `phys.cost()` computes the total cost of the curve, if cost is `Nan`, it /// returns `None` to indicate an invalid calculation -fn cost(initial: physics::PhysicsState, curve: &hermite::Spline) -> Option { +fn cost(initial: physics::legacy::PhysicsState, curve: &hermite::Spline) -> Option { let mut phys = initial; - while let Some((dx, dy, dz)) = curve.curve_1st_derivative_at(phys.u()) { - phys.step(dx, dy, dz, physics::StepBehavior::Distance); + while let Some(drdu) = curve.curve_1st_derivative_at(&T::from_f64(phys.u())) { + phys.step(drdu.x.to_f64(), drdu.y.to_f64(), drdu.z.to_f64(), physics::StepBehavior::Distance, 1.0); } if phys.cost().is_nan() { None @@ -50,9 +52,9 @@ fn cost(initial: physics::PhysicsState, curve: &hermite::Spline) -> Option // curve: The Hermite spline to optimize // points: Array of control points for the spline //LR: Determines the step size for gradient descent -pub fn optimize( - initial: &physics::PhysicsState, - curve: &hermite::Spline, +pub fn optimize( + initial: &physics::legacy::PhysicsState, + curve: &hermite::Spline, points: &mut [point::Point], lr: f64, ) -> Option { @@ -67,7 +69,7 @@ pub fn optimize( // for each control point, compute the gradient of the cost function with respect to its derivatives. for np in nudged { controls[i] = np; - let params = hermite::Spline::new(&controls); + let params = hermite::Spline::::new(&controls); let new_cost = cost(initial.clone(), ¶ms); // sublist stores the calculated gradients for this control point. sublist.push(new_cost.map(|c| { diff --git a/num_opt/src/physics.rs b/num_opt/src/physics.rs deleted file mode 100644 index ec32fa2..0000000 --- a/num_opt/src/physics.rs +++ /dev/null @@ -1,164 +0,0 @@ -//! Physics solver and cost function - -/// Possible ways for the physics system to take a step -/// Steps are in `u`, the (0,1) parameterization of hermite curves -#[derive(Debug, Clone, Copy)] -pub enum StepBehavior { - /// Advance by changing `u` to `u + k` where k is a constant - #[allow(dead_code)] - Constant, - /// Advances `u` while trying to keep the arc length traveled constant - Distance, - /// Advances `u` while trying to keep time-step constant - Time, -} - - -/// Physics solver -#[derive(Debug, Clone, getset::CopyGetters)] -#[getset(get_copy = "pub")] - -pub struct PhysicsState { - mass: f64, - gravity: f64, - speed: f64, - v: na::Vector3, - a: f64, - u: f64, - cost: f64, - total_len: f64, - max_g_force: f64, - normal_force: na::Vector3, - friction_force: f64, - mu: f64, -} - -impl PhysicsState { - /// Initialize, all values except those provided are zeroed - pub fn new(mass: f64, gravity: f64, mu: f64) -> Self { - Self { - mass, - gravity, - speed: 0.0, - u: 0.0, - v: Default::default(), - a: 0.0, - cost: 0.0, - total_len: 0.0, - max_g_force: 0.0, - friction_force: 0.0, - normal_force: Default::default(), - mu - } - } - - /// Solve the physics given the current tangent of the curve - pub fn step(&mut self, dxdu: f64, dydu: f64, dzdu: f64, behavior: StepBehavior) { - let drdu = na::Vector3::new(dxdu, dydu, dzdu); - // drdu means tangent value of 3D components - let raw_ds_du = drdu.magnitude(); - // arc length becomes the magnitude of drdu - self.total_len += raw_ds_du; - let ds_du = raw_ds_du.max(0.01); - // In order to keep the continuity, set length not becomes the zero. - - let step = match behavior { - StepBehavior::Constant => 0.01, - StepBehavior::Distance => (1.0 / ds_du).min(0.01), - StepBehavior::Time => (0.1 * self.speed / ds_du).max(0.00001), - }; - // Arc length is ds.du * delta u(stepsize). - - let fg = self.gravity * self.mass; - //let forward_force = fg * dydu / s; - // direction of gravity is always -y direction. - - // SI units used for clarity - - // Work = Force * distance - // Force = Gravity • T / s (kg m/s^2 = N) - // distance = s (m) - // ∴ Work = Gravity • T (Nm = J) - // = <0, fg, 0> • (dJ/ds) - // = fg * dydu (dJ/ds) - // -> fg * dydu * step (ΔJ) - // then add friction... - let dkdu = fg * dydu - self.friction_force; - let delta_k = step * dkdu; - // dkdu is net work for this roller coaster. - // delta k measures how much rollercoaster should be moved. - - // Net Force = Force of Gravity + Normal Force (TODO: + Friction) - // Force = dVdt - - - - - let new_vel = self.vel_from_k(self.k() + delta_k); -// set new velocity to the point at distance k - let new_u = self.u + step; -// set new u to original value of u and add the self.u there - // (m/s) - let new_v = na::Vector3::new(new_vel * dxdu / ds_du, new_vel * dydu / ds_du, new_vel * dzdu / ds_du); - // set new net velocity - // (m/s) - let delta_v = - ((self.v.x - new_v.x).powi(2) + (self.v.y - new_v.y).powi(2) + (self.v.z - new_v.z).powi(2)) - .sqrt(); - // change in net velocity - // (s) - let delta_t = step * ds_du / self.speed; - // change in time - // (m/s^2) - self.a = delta_v / delta_t; - // declare the acceleration - - // Net force, F = ma - let net_f = (new_v - self.v) * self.mass / delta_t; - - // Net - Gravity (- Friction? TODO) - self.normal_force = net_f - na::Vector3::new(0.0, fg, 0.0); - //const FRICTION: f64 = 0.06; - // coefficient of friction constant - let new_friction_force = self.mu * self.normal_force.magnitude(); - - //declare the friction force - - let g_force = self.a / self.gravity.abs(); - self.max_g_force = self.max_g_force.max(g_force); -// - - self.u = new_u; - self.speed = new_vel; - - self.v = new_v; - - self.friction_force = new_friction_force; - - const MIN_VEL: f64 = 0.01; - const MIN_VEL_MULT: f64 = 1.0; - //const MAX_G: f64 = 3.0; - - if delta_t.is_finite() { - // smoothing term - self.cost += (self.a).powi(2) * delta_t; - /*if g_force.is_normal() && delta_t.is_normal() && g_force > MAX_G { - self.cost += 0.001 * (g_force - MAX_G) * delta_t; - }*/ - if self.speed < MIN_VEL { - // penalize going too slow - self.cost += (MIN_VEL - self.speed) * delta_t * MIN_VEL_MULT; - } - } - } - - /// kinetic energy - fn k(&self) -> f64 { - 0.5 * self.mass * self.speed.powi(2) - } - - /// speed from kinetic energy - fn vel_from_k(&self, k: f64) -> f64 { - (2.0 * k / self.mass).sqrt() - } -} diff --git a/num_opt/src/physics/legacy.rs b/num_opt/src/physics/legacy.rs new file mode 100644 index 0000000..a498e51 --- /dev/null +++ b/num_opt/src/physics/legacy.rs @@ -0,0 +1,323 @@ +use crate::hermite; +use crate::physics::StepBehavior; + +use super::linalg::MyVector3; + + +/// Physics solver +#[derive(Debug, Clone, getset::CopyGetters)] +#[getset(get_copy = "pub")] +pub struct PhysicsState { + mass: f64, + gravity: f64, + com_offset_mag: f64, + speed: f64, + v: na::Vector3, + a: f64, + u: f64, + cost: f64, + total_len: f64, + max_g_force: f64, + normal_force: na::Vector3, + friction_force: f64, + mu: f64, +} + +impl PhysicsState { + /// Initialize, all values except those provided are zeroed + pub fn new(mass: f64, gravity: f64, mu: f64, com_offset_mag: f64) -> Self { + Self { + mass, + gravity, + com_offset_mag, + speed: 0.0, + u: 0.0, + v: Default::default(), + a: 0.0, + cost: 0.0, + total_len: 0.0, + max_g_force: 0.0, + friction_force: 0.0, + normal_force: Default::default(), + mu, + } + } + + /// Solve the physics given the current tangent of the curve + pub fn step( + &mut self, + dxdu: f64, + dydu: f64, + dzdu: f64, + behavior: StepBehavior, + step_size: f64, + ) { + // tangent value of 3D components + let drdu = na::Vector3::new(dxdu, dydu, dzdu); + // d(arc length) becomes the magnitude of drdu + let raw_ds_du = drdu.magnitude(); + self.total_len += raw_ds_du; + let ds_du = raw_ds_du.max(0.01); // Ensure numeric stability + + let step = match behavior { + StepBehavior::Constant => step_size, // 0.01 + // Arc length is dsdu * delta u(stepsize). + StepBehavior::Distance => (step_size / ds_du).min(0.01), // 1.0 + StepBehavior::Time => (step_size * self.speed / ds_du).max(0.00001), // 0.1 + }; + + let fg = self.gravity * self.mass; + // let forward_force = fg * dydu / s; + // direction of gravity is always -y direction. + + // SI units used for clarity + + // Work = Force * distance + // Force = Gravity • T / s (kg m/s^2 = N) + // distance = s (m) + // ∴ Work = Gravity • T (Nm = J) + // = <0, fg, 0> • (dJ/ds) + // = fg * dydu (dJ/ds) + // -> fg * dydu * step (ΔJ) + // then add friction... + let dkdu = fg * dydu - self.friction_force; + let delta_k = step * dkdu; + // dkdu is rate of work for this roller coaster. + // delta k is the change in KE + + // Net Force = Force of Gravity + Normal Force (TODO: + Friction) + // Force = dVdt + + // set new velocity based on energy k + Δk + let new_vel = self.vel_from_k(self.k() + delta_k); + + // set new u to original value of u and add the self.u there + let new_u = self.u + step; + + // set new net velocity (m/s) + let new_v = na::Vector3::new( + new_vel * dxdu / ds_du, + new_vel * dydu / ds_du, + new_vel * dzdu / ds_du, + ); + // change in net velocity (m/s) + let delta_v = ((self.v.x - new_v.x).powi(2) + + (self.v.y - new_v.y).powi(2) + + (self.v.z - new_v.z).powi(2)) + .sqrt(); + + // change in time (s) + let delta_t = step * ds_du / self.speed; + + // acceleration (m/s^2) = Δv / Δt + self.a = delta_v / delta_t; + + // Net force, F = ma + let net_f = (new_v - self.v) * self.mass / delta_t; + + // Net - Gravity (- Friction? TODO) + self.normal_force = net_f - na::Vector3::new(0.0, fg, 0.0); + + // Friction = μ * N + let new_friction_force = self.mu * self.normal_force.magnitude(); + + let g_force = self.a / self.gravity.abs(); + self.max_g_force = self.max_g_force.max(g_force); + + self.u = new_u; + self.speed = new_vel; + + self.v = new_v; + + self.friction_force = new_friction_force; + + const MIN_VEL: f64 = 0.01; + const MIN_VEL_MULT: f64 = 1.0; + //const MAX_G: f64 = 3.0; + + if delta_t.is_finite() { + // smoothing term + self.cost += (self.a).powi(2) * delta_t; + /*if g_force.is_normal() && delta_t.is_normal() && g_force > MAX_G { + self.cost += 0.001 * (g_force - MAX_G) * delta_t; + }*/ + if self.speed < MIN_VEL { + // penalize going too slow + self.cost += (MIN_VEL - self.speed) * delta_t * MIN_VEL_MULT; + } + } + } + + /// kinetic energy + fn k(&self) -> f64 { + 0.5 * self.mass * self.speed.powi(2) + } + + /// speed from kinetic energy + fn vel_from_k(&self, k: f64) -> f64 { + (2.0 * k / self.mass).sqrt() + } + + pub fn hl_pos(&self, curve: &hermite::Spline) -> Option> { + curve.curve_at(&self.u) + } + + /// Uses `-self.normal_force()` to determine offset direction + pub fn com_pos(&self, curve: &hermite::Spline) -> Option> { + self.hl_pos(curve).map(|hl| { + let offset = self.normal_force.normalize() * self.com_offset_mag; + hl - MyVector3::new_f64(offset.x, offset.y, offset.z) // (x, y, z) + }) + } +} +/* +/// Physics solver v2 +/// See `Design: adding rotation to physics` in the doc +#[derive(Debug, Clone)] +pub struct PhysicsStateV2 { + m: f64, + g: na::Vector3, + o: f64, + dsdu: f64, + v_com: na::Vector3, + v_hl: na::Vector3, + a_hl: na::Vector3, + n_hl: na::Vector3, + k: f64, + u: f64, + // stats + cost: f64, + max_g_force: f64, +} + +impl PhysicsStateV2 { + pub fn new(m: f64, g: na::Vector3, o: f64) -> Self { + Self { + m, + g, + o, + dsdu: 0.0, + v_com: Default::default(), + v_hl: Default::default(), + a_hl: Default::default(), + n_hl: Default::default(), + k: 0.0, + u: 0.0, + cost: 0.0, + max_g_force: 0.0, + } + } + + pub fn step( + &mut self, + curve: &hermite::Spline, + step_size: f64, + behavior: StepBehavior, + ) -> Option<()> { + self.o = 0.0; + let delta_u_raw = match behavior { + StepBehavior::Constant => panic!(), + StepBehavior::Distance => step_size / self.dsdu, + StepBehavior::Time => step_size * self.v_com.magnitude() / self.dsdu, + }; + let delta_u = if delta_u_raw.is_finite() && delta_u_raw != 0.0 { + delta_u_raw + } else { + 0.001 + }; + let delta_u = 0.1; + let new_u = self.u + delta_u; + let delta_t_raw = match behavior { + StepBehavior::Constant => panic!(), + StepBehavior::Distance => step_size / self.v_com.magnitude(), + StepBehavior::Time => step_size, + }; + let delta_t_raw = self.dsdu * delta_u / self.v_com.magnitude(); + let delta_t = if delta_t_raw.is_finite() { + delta_t_raw + } else { + 0.1 + }; + let a_sub_g = self.a_hl - self.g; + let new_n_hl = (a_sub_g + - a_sub_g.dot(&self.v_hl) * self.v_hl / self.v_hl.magnitude_squared().max(0.0001)) + .normalize(); + let offset = self.o * new_n_hl; + let offset = MyVector3::new_f64(offset.x, offset.y, offset.z); + let delta_p_com = (curve.curve_at(&float!(new_u))? - offset) + - (curve.curve_at(&float!(self.u))? - offset); + let new_k = self.k + self.m * self.g.y * delta_p_com.y; + let new_v_hl = + (curve.curve_at(&float!(new_u))? - curve.curve_at(&float!(self.u))?) / float!(delta_t); + let new_a_hl = (new_v_hl - MyVector3::new_f64(self.v_hl.x, self.v_hl.y, self.v_hl.z)) + / float!(delta_t); + let new_v_com = (2.0 * new_k / self.m).sqrt() * delta_p_com / delta_t; + let new_dsdu = delta_p_com.magnitude() / delta_u; + + self.u = new_u; + self.k = new_k.to_f64(); + self.v_hl = new_v_hl; + self.a_hl = new_a_hl; + self.v_com = new_v_com; + self.n_hl = new_n_hl; + self.dsdu = new_dsdu; + + let g_force = self.a() / self.gravity().abs(); + self.max_g_force = self.max_g_force.max(g_force); + + const MIN_VEL: f64 = 0.01; + const MIN_VEL_MULT: f64 = 1.0; + //const MAX_G: f64 = 3.0; + + godot_print!("{:#?}", self); + + if delta_t.is_finite() { + // smoothing term + self.cost += (self.a_hl.magnitude()).powi(2) * delta_t; + /*if g_force.is_normal() && delta_t.is_normal() && g_force > MAX_G { + self.cost += 0.001 * (g_force - MAX_G) * delta_t; + }*/ + if self.v_hl.magnitude() < MIN_VEL { + // penalize going too slow + self.cost += (MIN_VEL - self.v_hl.magnitude()) * delta_t * MIN_VEL_MULT; + } + } + + Some(()) + } +} + +impl PhysicsStateV2 { + pub fn com_pos(&self, curve: &hermite::Spline) -> Option> { + curve.curve_at(self.u).map(|hl| hl - self.o * self.n_hl) + } + + pub fn speed(&self) -> f64 { + self.v_hl.magnitude() + } + + pub fn a(&self) -> f64 { + self.a_hl.magnitude() + } + + pub fn gravity(&self) -> f64 { + self.g.y + } + + pub fn v(&self) -> na::Vector3 { + self.v_com + } + + pub fn cost(&self) -> f64 { + self.cost + } + + pub fn max_g_force(&self) -> f64 { + self.max_g_force + } + + pub fn hl_normal(&self) -> na::Vector3 { + self.n_hl + } +} +*/ \ No newline at end of file diff --git a/num_opt/src/physics/linalg.rs b/num_opt/src/physics/linalg.rs new file mode 100644 index 0000000..fc056b3 --- /dev/null +++ b/num_opt/src/physics/linalg.rs @@ -0,0 +1,241 @@ +use std::{any::TypeId, ops::{Div, Mul}}; + +use godot::global::godot_warn; + +use crate::my_float::MyFloat; + + +/// Projects `a` onto `b` +pub fn vector_projection(a: MyVector3, b: MyVector3) -> MyVector3 { + b.clone() * (a.dot(&b) / b.magnitude_squared()) +} + +pub fn scaler_projection(a: MyVector3, b: MyVector3) -> T { + a.dot(&b) / b.magnitude() +} + +pub struct Silence; + +/// 3D Vector with higher +/// +#[derive(Clone)] +pub struct MyVector3 { + pub x: T, + pub y: T, + pub z: T, +} + +impl MyVector3 { + pub fn new(x: T, y: T, z: T) -> Self { + Self { x, y, z } + } + + pub fn new_f64(x: f64, y: f64, z: f64) -> Self { + Self { + x: T::from_f64(x), + y: T::from_f64(y), + z: T::from_f64(z), + } + } + + fn dot(&self, other: &MyVector3) -> T { + self.x.clone() * other.x.clone() + self.y.clone() * other.y.clone() + self.z.clone() * other.z.clone() + } + + pub fn magnitude(&self) -> T { + self.magnitude_squared().sqrt() + } + + pub fn magnitude_squared(&self) -> T { + self.x.clone() * self.x.clone() + self.y.clone() * self.y.clone() + self.z.clone() * self.z.clone() + } + + pub fn normalize(self) -> Self { + let mag = self.magnitude(); + Self { + x: self.x / mag.clone(), + y: self.y / mag.clone(), + z: self.z / mag, + } + } + + // struct Checks; + + + + pub fn angle(&self, other: &MyVector3) -> T { + self.angle_dbg::(other) + } + + pub fn angle_dbg(&self, other: &MyVector3) -> T { + let dot = self.dot(other); + let mag1 = self.magnitude(); + let mag2 = other.magnitude(); + let cos = (dot.clone() / (mag1.clone() * mag2.clone())).clamp(-1.0, 1.0); + if TypeId::of::() == TypeId::of::<()>() { + godot_warn!("cos: {} mag1: {} mag2: {} dot: {}", cos, mag1, mag2, dot); //godot_warn!("cos: {} mag1: {} mag2: {}", cos, mag1, mag2); //godot_warn!("cos: {}", cos); + //} + } else if TypeId::of::() != TypeId::of::() { + panic!(); + } + + cos.acos() + } + + pub fn cross(&self, other: &MyVector3) -> MyVector3 { + MyVector3 { + x: (self.y.clone() * other.z.clone() - self.z.clone() * other.y.clone()), + y: (self.z.clone() * other.x.clone() - self.x.clone() * other.z.clone()), + z: (self.x.clone() * other.y.clone() - self.y.clone() * other.x.clone()), + } + } + + pub fn has_nan(&self) -> bool { + self.x.is_nan() || self.y.is_nan() || self.z.is_nan() + } +} + +impl std::fmt::Debug for MyVector3 { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.debug_struct("MV").field("x", &self.x).field("y", &self.y).field("z", &self.z).finish() + } +} + +impl std::ops::Add for MyVector3 { + type Output = Self; + fn add(self, other: Self) -> Self { + Self { + x: self.x + other.x, + y: self.y + other.y, + z: self.z + other.z, + } + } +} + +impl std::ops::Sub for MyVector3 { + type Output = Self; + fn sub(self, other: Self) -> Self { + Self { + x: self.x - other.x, + y: self.y - other.y, + z: self.z - other.z, + } + } +} + +impl std::ops::Neg for MyVector3 { + type Output = Self; + fn neg(self) -> Self { + Self { + x: -self.x, + y: -self.y, + z: -self.z, + } + } +} + +impl Div for MyVector3 { + type Output = MyVector3; + fn div(self, other: T) -> MyVector3 { + MyVector3 { + x: self.x / other.clone(), + y: self.y / other.clone(), + z: self.z / other.clone(), + } + } +} + +impl Mul for MyVector3 { + type Output = MyVector3; + fn mul(self, other: T) -> MyVector3 { + MyVector3 { + x: self.x * other.clone(), + y: self.y * other.clone(), + z: self.z * other, + } + } +} + +impl Default for MyVector3 { + fn default() -> Self { + Self { + x: T::from_f64(0.0), //float!(0.0), + y: T::from_f64(0.0), //float!(0.0), + z: T::from_f64(0.0), //float!(0.0), + } + } +} + +/// Only suitable for rotations +#[derive(Clone, Debug)] +pub struct MyQuaternion { + pub q0: T, + pub q1: T, + pub q2: T, + pub q3: T, +} + +impl MyQuaternion { + pub fn from_scaled_axis(x: MyVector3) -> Self { + let half_angle: T = x.magnitude() / T::from_f64(2.0); + Self { + q0: half_angle.clone().cos(), + q1: x.x * half_angle.clone().sin(), + q2: x.y * half_angle.clone().sin(), + q3: x.z * half_angle.sin(), + } + } + + pub fn normalize(self) -> Self { + let mag = ((self.q0.clone() * self.q0.clone() + self.q1.clone() * self.q1.clone()) + + self.q2.clone() * self.q2.clone() + + self.q3.clone() * self.q3.clone()) + .sqrt(); + Self { + q0: self.q0 / mag.clone(), + q1: self.q1 / mag.clone(), + q2: self.q2 / mag.clone(), + q3: self.q3 / mag.clone(), + } + } + + pub fn unit_inverse(self) -> Self { + Self { + q0: self.q0, + q1: -self.q1, + q2: -self.q2, + q3: -self.q3, + } + } + + pub fn rotate(self, other: MyVector3) -> MyVector3 { + let other = MyQuaternion { + q0: T::from_f64(0.0), //float!(0.0), + q1: other.x, + q2: other.y, + q3: other.z, + }; + let s = self.normalize(); + let inv = s.clone().unit_inverse(); + let other = inv * other * s; + MyVector3 { x: other.q1, y: other.q2, z: other.q3 } + } +} + +impl Mul> for MyQuaternion { + type Output = MyQuaternion; + fn mul(self, other: MyQuaternion) -> MyQuaternion { + MyQuaternion { + q0: (self.q0.clone() * other.q0.clone() - self.q1.clone() * other.q1.clone()) + - self.q2.clone() * other.q2.clone() + - self.q3.clone() * other.q3.clone(), + q1: (self.q0.clone() * other.q1.clone() + self.q1.clone() * other.q0.clone()) - self.q2.clone() * other.q3.clone() + + self.q3.clone() * other.q2.clone(), + q2: (self.q0.clone() * other.q2.clone() + self.q1.clone() * other.q3.clone()) + + self.q2.clone() * other.q0.clone() + - self.q3.clone() * other.q1.clone(), + q3: (self.q0.clone() * other.q3.clone() - self.q1.clone() * other.q2.clone()) + self.q2.clone() * other.q1.clone() + + self.q3.clone() * other.q0.clone(), + } + } +} diff --git a/num_opt/src/physics/mod.rs b/num_opt/src/physics/mod.rs new file mode 100644 index 0000000..8c7f027 --- /dev/null +++ b/num_opt/src/physics/mod.rs @@ -0,0 +1,392 @@ +//! Physics solver and cost function + +use godot::global::godot_warn; +use linalg::{scaler_projection, vector_projection, MyQuaternion, MyVector3, Silence}; + +use crate::{hermite, my_float::MyFloat}; + +pub mod legacy; +pub mod linalg; + +/// Possible ways for the physics system to take a step +/// Steps are in `u`, the (0,1) parameterization of hermite curves +#[derive(Debug, Clone, Copy)] +pub enum StepBehavior { + /// Advance by changing `u` to `u + k` where k is a constant + #[allow(dead_code)] + Constant, + /// Advances `u` while trying to keep the arc length traveled constant + Distance, + /// Advances `u` while trying to keep time-step constant + Time, +} + +const FALLBACK_STEP: f64 = 0.001; +pub const PRECISION: u32 = 64; + +macro_rules! float { + () => { + Float::with_val(PRECISION, 0.0) + }; + ($e:expr) => { + Float::with_val(PRECISION, $e) + }; + ($n:expr, $d: expr) => { + Float::with_val(PRECISION, $n) / Float::with_val(PRECISION, $d) + }; +} + +pub(crate) use float; + +fn find_root_bisection(a: T, b: T, f: impl Fn(&T) -> T, epsilon: f64) -> Option { + let mut a = a; + let mut b = b; + if a > b { + std::mem::swap(&mut a, &mut b); + } + let mut fa = f(&a); + let fb = f(&b); + if (fa.clone() * fb) > 0.0 { + // they have the same sign, so there is no root in this interval + return None; + } + while (b.clone() - a.clone()).abs() > epsilon { + let m = (a.clone() + b.clone()) / T::from_f64(2.0); + let fm = f(&m); + if (fa.clone() * fm.clone()) < 0.0 { + b = m; + //fb = fm; + } else { + a = m; + fa = fm; + } + } + Some((a + b) / T::from_f64(2.0)) +} + +/// Physics solver v3 +/// ### Overview +/// Store simulation parameters (m,g) +/// Track the state of the particle (position, velocity, u(curve parameter)) +/// Compute intermediate values like delta_x(displacement), F_N(force), and delta_t(time step). +/// Use a quadratic equation to determine the correcr time step(roots). +#[derive(Debug)] +pub struct PhysicsStateV3 { + // params + m: T, + g: MyVector3, + o: T, + + // simulation state + u: T, + // center of mass + x: MyVector3, + v: MyVector3, + // heart line + hl_normal: MyVector3, + hl_pos: MyVector3, + hl_vel: MyVector3, + hl_accel: MyVector3, + // rotation + torque_exceeded: bool, + w: MyVector3, // angular velocity + I: T, // moment of inertia + + // stats, info, persisted intermediate values + delta_u_: T, + delta_t_: T, + delta_x_actual_: MyVector3, + delta_x_target_: MyVector3, + delta_hl_normal_actual_: MyVector3, + delta_hl_normal_target_: MyVector3, + target_hl_normal_: MyVector3, + ag_: MyVector3, + F_N_: MyVector3, + torque_: MyVector3, +} + +impl PhysicsStateV3 { + /// Initialize the physics state: `m` mass, `g` Gravity vector, + /// `curve.curve_at(0.0)`: Starting position of the curve at `u=0` + pub fn new(m: f64, g: f64, curve: &hermite::Spline, o: f64) -> Self { + let g = MyVector3::new_f64(0.0, g, 0.0); + let hl_pos = curve.curve_at(&T::from_f64(0.0)).unwrap(); + let mut hl_forward = curve.curve_1st_derivative_at(&T::from_f64(0.0)).unwrap(); + + if hl_forward.magnitude() == 0.0 { + hl_forward = curve.curve_2nd_derivative_at(&T::from_f64(0.0)).unwrap(); + } + if hl_forward.magnitude() == 0.0 { + hl_forward = curve.curve_3rd_derivative_at(&T::from_f64(0.0)).unwrap(); + } + if hl_forward.magnitude() == 0.0 { + hl_forward = curve.curve_4th_derivative_at(&T::from_f64(0.0)).unwrap(); + } + assert!(hl_forward.magnitude() > 0.0); + let hl_normal = (-g.clone() - vector_projection(-g.clone(), hl_forward)).normalize(); + let s = Self { + // constants + m: T::from_f64(m), + I: T::from_f64(1.0), + g, + o: T::from_f64(o), + // simulation state + u: T::from_f64(0.0), //0.0, + // center of mass + x: hl_pos.clone() - hl_normal.clone() * T::from_f64(o), //o, + v: Default::default(), + // heart line + hl_pos, + hl_normal, + hl_vel: Default::default(), + hl_accel: Default::default(), + // rotation + torque_exceeded: false, + w: Default::default(), + // stats, info, persisted intermediate values + delta_u_: T::from_f64(0.0), //0.0, + delta_x_actual_: Default::default(), + delta_x_target_: Default::default(), + delta_t_: T::from_f64(0.0), + delta_hl_normal_actual_: Default::default(), + delta_hl_normal_target_: Default::default(), + target_hl_normal_: Default::default(), + F_N_: Default::default(), + ag_: Default::default(), + torque_: Default::default(), + }; + godot_warn!("{:#?}", s); + s + } + + fn calc_new_u_from_delta_t( + &mut self, + step: &T, + init_bracket_amount: T, + curve: &hermite::Spline, + ) -> Option { + // Semi-implicit euler position update + let new_pos = + self.x.clone() + (self.v.clone() + self.g.clone() * step.clone()) * step.clone(); + + let u_lower_bound = (self.u.clone() - init_bracket_amount.clone()).max(&T::from_f64(0.0)); + let u_upper_bound = + (self.u.clone() + init_bracket_amount.clone()).min(&T::from_f64(curve.max_u())); + if u_upper_bound == curve.max_u() { + return None; + } + let roots = find_root_bisection( + u_lower_bound, + u_upper_bound, + |u| { + /*let v1 = new_pos.clone() - curve.curve_at(u).unwrap(); + let v2 = curve.curve_1st_derivative_at(u).unwrap(); + godot_print!("v1: {:.3?} v2: {:.3?}", v1, v2);*/ + scaler_projection( + new_pos.clone() - curve.curve_at(u).unwrap(), + curve.curve_1st_derivative_at(u).unwrap(), + ) + }, + 1e-14f64, + ); + match roots { + Some(root) => Some(root), + None => self.calc_new_u_from_delta_t(step, init_bracket_amount * 2.0, curve), + } + } + + /// ### Step Sizes + /// `StepBehavior::Constant`: Use a fixed step size step. + /// `StepBehavior::Distance`: Adjust delta_u to keep the traveled arc length constant + /// `StepBehavior::Time`: Adjust delta_u based on both velocity and arc length. + /// if `dsdu` is zero, a fallback step size is used + pub fn step( + &mut self, + step: T, + curve: &hermite::Spline, + behavior: StepBehavior, + ) -> Option<()> { + if self.torque_exceeded { + //return None; + } + + // 0.00001 is unstable (either due to rounding error) + let max_curve_angle: f64 = 0.0001; + const MIN_STEP: f64 = 0.001; + const MIN_DELTA_X: f64 = 0.0001; + const MAX_TORQUE: f64 = 10.0; + const ALLOWED_ANGULAR_WIGGLE: f64 = 0.01; + + self.delta_t_ = step.clone(); + let new_u = self + .calc_new_u_from_delta_t(&step, T::from_f64(0.00001), curve) + .unwrap(); + self.delta_u_ = new_u.clone() - self.u.clone(); + + // Advance the parametric value u by delta_u and calculate the new position along the curve. + // The displacement vector delta_x is the difference between the new position and the current position. + //let new_u = self.u + self.delta_u_; + self.ag_ = self.hl_accel.clone() - self.g.clone(); + self.target_hl_normal_ = if self.torque_exceeded { + self.hl_normal.clone() + } else { + let ortho_to = curve.curve_1st_derivative_at(&new_u).unwrap().normalize(); + let tmp = (self.ag_.clone().normalize() + - vector_projection(self.ag_.clone().normalize(), ortho_to.clone())) + .normalize(); + (tmp.clone() - vector_projection(tmp.clone(), ortho_to.clone())).normalize() + }; + self.delta_x_target_ = curve.curve_at(&new_u).unwrap() + - self.target_hl_normal_.clone() * self.o.clone() + - self.x.clone(); + + let step_too_big = { + self.v.angle(&self.delta_x_target_) > max_curve_angle + && self.delta_u_ != FALLBACK_STEP + && step > MIN_STEP + && self.delta_x_target_.magnitude() > MIN_DELTA_X + }; + /*if step_too_big { + self.step(step / 2.0, curve, behavior).unwrap(); + //self.step(step / 2.0, curve, behavior).unwrap(); + return Some(()); + }*/ + //self.delta_t_ = self.calc_delta_t_from_delta_u(step).unwrap(); + + // Normal Force: Derived from displacement, velocity, and gravity. + // v: semi implicit Euler integration + // Position(x): Advance based on velocity + // u: A Advances to the next point. + //self.F_N_ = na::Vector3::zeros(); + + self.F_N_ = (self.delta_x_target_.clone() / self.delta_t_.clone().pow(2) + - self.v.clone() / self.delta_t_.clone() + - self.g.clone()) + * self.m.clone(); + #[allow(non_snake_case)] + let F = self.F_N_.clone() + self.g.clone() * self.m.clone(); + // hl values + let new_hl_vel = + (curve.curve_at(&new_u)? - curve.curve_at(&self.u)?) / self.delta_t_.clone(); + let new_hl_accel = (new_hl_vel.clone() - self.hl_vel.clone()) / self.delta_t_.clone(); + // rotation + let cross = self.hl_normal.cross(&self.target_hl_normal_).normalize(); + self.delta_hl_normal_target_ = + cross * self.hl_normal.angle_dbg::(&self.target_hl_normal_); + + if self.torque_exceeded { + self.w = MyVector3::default(); + } else { + self.torque_ = (self.delta_hl_normal_target_.clone() / self.delta_t_.clone().pow(2) + - self.w.clone() / self.delta_t_.clone()) + * self.I.clone(); + } + if !self.torque_exceeded + && self.torque_.magnitude() > MAX_TORQUE + && self.delta_hl_normal_target_.magnitude() > ALLOWED_ANGULAR_WIGGLE + { + self.torque_exceeded = true; + //let delta_y = self.hl_normal.clone() * self.o.clone(); + let curr_energy = self.energy(); + self.o = T::from_f64(0.0); + self.x = curve.curve_at(&self.u).unwrap(); + self.v = self.v.clone().normalize() + * ((curr_energy - self.potential_energy()).max(&T::from_f64(0.0)) * 2.0 / self.m.clone()).sqrt(); + // REMEMBER TO REMOVE + std::thread::sleep(std::time::Duration::from_millis(200)); + return self.step(step, curve, behavior); + } + + // semi-implicit euler update rule + self.v = self.v.clone() + F / self.m.clone() * self.delta_t_.clone(); + let new_x = self.x.clone() + self.v.clone() * self.delta_t_.clone(); + self.delta_x_actual_ = new_x.clone() - self.x.clone(); + self.x = new_x.clone(); + + // cop-out update + //self.x = curve.curve_at(new_u).unwrap(); + + // semi-implicit euler update rule, rotation + if !self.torque_exceeded { + self.w = self.w.clone() + self.torque_.clone() * self.delta_t_.clone() / self.I.clone(); + } + + self.delta_hl_normal_actual_ = self.w.clone() * self.delta_t_.clone(); + + self.hl_normal = MyQuaternion::from_scaled_axis(self.delta_hl_normal_actual_.clone()) + .rotate(self.hl_normal.clone()); + + // cop-out rotation + //self.hl_normal = self.target_hl_normal_.clone(); + // updates + self.hl_vel = new_hl_vel; + self.hl_accel = new_hl_accel; + self.u = new_u; + + //godot_warn!("{:#?}", self); + + Some(()) + } + + fn energy(&self) -> T { + self.v.magnitude_squared() * 0.5 * self.m.clone() + self.potential_energy() + } + + fn potential_energy(&self) -> T { + self.m.clone() * self.g.magnitude() * self.x.y.clone() + } + + pub fn description(&self, curve: &hermite::Spline) -> String { + format!( + "x: {:.2?} +E: {:.3?} +speed: {:.3} +hl speed: {:.2} +g force: {:.2} +delta-x: {:.3?} err: {} +F_N: {:.2?} +T-Exceeded: {} +w: {:.3} +Torque: {:.3} +delta-t: {:.6} +delta-u: {:.6?}", + self.x, + self.energy(), + self.v.magnitude(), + self.hl_vel.magnitude(), + self.ag_.magnitude() / self.g.magnitude(), + self.delta_x_target_.magnitude(), + (self.delta_x_actual_.clone() - self.delta_x_target_.clone()).magnitude(), + self.F_N_, + self.torque_exceeded, + self.w.magnitude(), + self.torque_.magnitude(), + self.delta_t_, + self.delta_u_ + ) + } + + pub fn pos(&self) -> &MyVector3 { + &self.x + } + + pub fn vel(&self) -> &MyVector3 { + &self.v + } + + pub fn hl_normal(&self) -> &MyVector3 { + &self.hl_normal + } + + pub fn ag(&self) -> &MyVector3 { + &self.ag_ + } + + pub fn a(&self) -> &MyVector3 { + &self.hl_accel + } + + pub fn g(&self) -> &MyVector3 { + &self.g + } +}