feat: ik solver

Cargo.toml

@@ -7,7 +7,7 @@ opt-level = 1
 opt-level = 3
 
 [workspace.package]
-version = "0.0.4-rc.3"
+version = "0.0.4-rc.2"
 edition = "2021"
 description = "A very flexible AI behavior library for games and robotics."
 documentation = "https://beetmash.com/docs/beet"
@@ -38,12 +38,12 @@ members = [
 
 [workspace.dependencies]
 ## internal
-beet = { path = "./", version = "0.0.4-rc.3" }
-beet_spatial = { path = "crates/beet_spatial", version = "0.0.4-rc.3", default-features = false }
-beet_flow = { path = "crates/beet_flow", version = "0.0.4-rc.3" }
-beet_flow_macros = { path = "crates/beet_flow/macros", version = "0.0.4-rc.3" }
-beet_ml = { path = "crates/beet_ml", version = "0.0.4-rc.3" }
-beet_examples = { path = "crates/beet_examples", version = "0.0.4-rc.3" }
+beet = { path = "./", version = "0.0.4-rc.2" }
+beet_spatial = { path = "crates/beet_spatial", version = "0.0.4-rc.2", default-features = false }
+beet_flow = { path = "crates/beet_flow", version = "0.0.4-rc.2" }
+beet_flow_macros = { path = "crates/beet_flow/macros", version = "0.0.4-rc.2" }
+beet_ml = { path = "crates/beet_ml", version = "0.0.4-rc.2" }
+beet_examples = { path = "crates/beet_examples", version = "0.0.4-rc.2" }
 
 ## local
 beetmash = { version = "0.0.6-rc.2", path = "../beetmash" }
@@ -91,8 +91,8 @@ serde_json = "1"
 ron = "0.8"
 
 #💡 game
-bevy = { version = "0.14", default-features = false, features = [
-# bevy = { git = "https://github.com/bevyengine/bevy", rev = "0c959f77007c29eead7f902bddd3342a1ecbca20", default-features = false, features = [
+# bevy = { version = "0.14", default-features = false, features = [
+bevy = { git = "https://github.com/bevyengine/bevy", rev = "0c959f77007c29eead7f902bddd3342a1ecbca20", default-features = false, features = [
 	"bevy_scene",
 	"serialize",
 	# "bevy_color",

crates/beet_examples/src/components/follow_cursor.rs

@@ -34,13 +34,54 @@ pub fn follow_cursor_2d(
 }
 
 
-#[derive(Default, Component, Reflect)]
+#[derive(Component, Reflect)]
 #[reflect(Default, Component)]
-pub struct FollowCursor3d;
+pub struct FollowCursor3d {
+	pub intersect_point: Vec3,
+	pub intersect_plane: InfinitePlane3d,
+}
+
+impl FollowCursor3d {
+	/// Follows the cursor on the XZ plane with the Y axis as the normal.
+	pub const ORIGIN_Y: Self = Self {
+		intersect_point: Vec3::ZERO,
+		intersect_plane: InfinitePlane3d { normal: Dir3::Y },
+	};
+
+	/// Follows the cursor on the XY plane with the Z axis as the normal.
+	pub const ORIGIN_Z: Self = Self {
+		intersect_point: Vec3::ZERO,
+		intersect_plane: InfinitePlane3d { normal: Dir3::Z },
+	};
+
+
+	pub fn new(intersect_point: Vec3, intersect_plane: Vec3) -> Self {
+		Self {
+			intersect_point,
+			intersect_plane: InfinitePlane3d::new(intersect_plane),
+		}
+	}
+	pub fn with_intersect_point(self, intersect_point: Vec3) -> Self {
+		Self {
+			intersect_point,
+			..self
+		}
+	}
+	pub fn with_intersect_plane(self, intersect_plane: Vec3) -> Self {
+		Self {
+			intersect_plane: InfinitePlane3d::new(intersect_plane),
+			..self
+		}
+	}
+}
+
+impl Default for FollowCursor3d {
+	fn default() -> Self { Self::ORIGIN_Y }
+}
 
 pub fn follow_cursor_3d(
 	camera_query: Query<(&Camera, &GlobalTransform)>,
-	mut cursor_query: Query<&mut Transform, With<FollowCursor3d>>,
+	mut cursor_query: Query<(&mut Transform, &FollowCursor3d)>,
 	windows: Query<&Window>,
 ) {
 	let Ok((camera, camera_transform)) = camera_query.get_single() else {
@@ -60,14 +101,14 @@ pub fn follow_cursor_3d(
 		return;
 	};
 
-	let Some(dist) =
-		ray.intersect_plane(Vec3::ZERO, InfinitePlane3d::new(Vec3::Y))
-	else {
-		return;
-	};
-	let point = ray.get_point(dist);
-
-	for mut transform in cursor_query.iter_mut() {
+	for (mut transform, follow_cursor) in cursor_query.iter_mut() {
+		let Some(dist) = ray.intersect_plane(
+			follow_cursor.intersect_point,
+			follow_cursor.intersect_plane,
+		) else {
+			continue;
+		};
+		let point = ray.get_point(dist);
 		transform.translation = point;
 	}
 }

crates/beet_examples/src/scenes/spatial/inverse_kinematics.rs

@@ -0,0 +1,91 @@
+use crate::prelude::FollowCursor3d;
+use beet_spatial::prelude::Ik2Dof;
+use beet_spatial::prelude::Ik2DofTransforms;
+use beet_spatial::prelude::IkSegment;
+// use crate::beet::prelude::*;
+// use beetmash::core::scenes::Foxie;
+use beetmash::prelude::*;
+// use bevy::animation::RepeatAnimation;
+use bevy::{
+	color::palettes::tailwind,
+	prelude::*,
+};
+// use std::time::Duration;
+
+
+pub fn inverse_kinematics(mut commands: Commands) {
+	let ik_solver = Ik2Dof::new(IkSegment::DEG_360, IkSegment::DEG_360);
+	let arm_width = 0.1;
+
+	commands.spawn((
+		Name::new("Camera"),
+		BundlePlaceholder::Camera3d,
+		Transform::from_xyz(0., 0., 5.0).looking_at(Vec3::ZERO, Vec3::Y),
+	));
+	let target = commands
+		.spawn((
+			Name::new("Mouse"),
+			FollowCursor3d::ORIGIN_Z,
+			Transform::default().looking_to(-Vec3::Z, Vec3::Y),
+			BundlePlaceholder::Pbr {
+				mesh: Circle::new(0.2).into(),
+				material: MaterialPlaceholder::unlit(tailwind::BLUE_500),
+			},
+		))
+		.id();
+
+
+	let root = commands
+		.spawn((
+			Name::new("IK Root"),
+			Transform::default().looking_to(Vec3::X, Vec3::Y),
+		))
+		.id();
+	let mut entity1 = Entity::PLACEHOLDER;
+	let mut entity2 = Entity::PLACEHOLDER;
+
+	commands.entity(root).with_children(|parent| {
+		entity1 = ik_segment(
+			&mut parent.spawn_empty(),
+			&ik_solver.segment1,
+			Transform::default(),
+			arm_width,
+		);
+	});
+
+	commands.entity(entity1).with_children(|parent| {
+		entity2 = ik_segment(
+			&mut parent.spawn_empty(),
+			&ik_solver.segment2,
+			Transform::from_xyz(0., 0., -ik_solver.segment1.len),
+			arm_width,
+		);
+	});
+
+	commands.spawn((
+		Name::new("IK Solver"),
+		Ik2DofTransforms::new(ik_solver, target, entity1, entity2),
+	));
+}
+
+
+pub fn ik_segment(
+	commands: &mut EntityCommands,
+	seg: &IkSegment,
+	transform: Transform,
+	arm_width: f32,
+) -> Entity {
+	commands
+		.insert((Name::new("Segment"), transform, Visibility::Visible))
+		.with_children(|parent| {
+			parent.spawn((
+				Name::new("Mesh"),
+				Transform::from_xyz(0., 0., -seg.len * 0.5),
+				BundlePlaceholder::Pbr {
+					mesh: Cuboid::new(arm_width, arm_width, seg.len).into(),
+					material: MaterialPlaceholder::unlit(tailwind::AMBER_500),
+				},
+			));
+		})
+		.id()
+}

crates/beet_examples/src/scenes/spatial/mod.rs

@@ -10,6 +10,9 @@ pub use self::forage::*;
 pub mod hello_animation;
 #[allow(unused_imports)]
 pub use self::hello_animation::*;
+pub mod inverse_kinematics;
+#[allow(unused_imports)]
+pub use self::inverse_kinematics::*;
 pub mod seek;
 #[allow(unused_imports)]
 pub use self::seek::*;

crates/beet_examples/src/scenes/spatial/seek_3d.rs

@@ -1,5 +1,5 @@
-use crate::prelude::*;
 use crate::beet::prelude::*;
+use crate::prelude::*;
 use beetmash::core::scenes::Foxie;
 use beetmash::prelude::*;
 use bevy::prelude::*;
@@ -10,15 +10,17 @@ pub fn seek_3d(mut commands: Commands) {
 	// camera
 	commands.spawn((
 		Name::new("Camera"),
-		BundlePlaceholder::Camera3d, CameraDistance {
-		width: 80.,
-		offset: Vec3::new(0., 20., 40.),
-	}));
+		BundlePlaceholder::Camera3d,
+		CameraDistance {
+			width: 80.,
+			offset: Vec3::new(0., 20., 40.),
+		},
+	));
 
 	// cheese
 	let target = commands
 		.spawn((
-			FollowCursor3d,
+			FollowCursor3d::default(),
 			Transform::from_xyz(20., 0., 40.).with_scale(Vec3::splat(3.)),
 			BundlePlaceholder::Scene("kaykit/cheese.glb#Scene0".into()),
 		))