Add traverse_navmesh method

src/engine/ai/navmesh.cpp

@@ -0,0 +1,111 @@
+/*
+ * Copyright (C) 2023  Christopher J. Howard
+ *
+ * This file is part of Antkeeper source code.
+ *
+ * Antkeeper source code is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Antkeeper source code is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Antkeeper source code.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <engine/ai/navmesh.hpp>
+#include <engine/geom/closest-feature.hpp>
+#include <engine/geom/coordinates.hpp>
+#include <engine/math/quaternion.hpp>
+#include <iterator>
+
+namespace ai {
+
+navmesh_traversal traverse_navmesh(const geom::brep_mesh& mesh, geom::brep_face* face, geom::ray<float, 3> ray, float distance)
+{
+	// Get vertex positions and face normals
+	const auto& vertex_positions = mesh.vertices().attributes().at<math::fvec3>("position");
+	const auto& face_normals = mesh.faces().attributes().at<math::fvec3>("normal");
+	
+	// Init traversal result
+	navmesh_traversal traversal;
+	traversal.edge = nullptr;
+	
+	// Save initial ray origin
+	auto initial_origin = ray.origin;
+	
+	float segment_length = 0.0f;
+	int edge_index;
+	do
+	{
+		// Get triangle vertices
+		auto loop_it = face->loops().begin();
+		const auto& a = vertex_positions[loop_it->vertex()->index()];
+		const auto& b = vertex_positions[(++loop_it)->vertex()->index()];
+		const auto& c = vertex_positions[(++loop_it)->vertex()->index()];
+		
+		// Find closest feature on triangle to segment endpoint
+		std::tie(traversal.barycentric, edge_index) = geom::closest_feature(a, b, c, ray.extrapolate(distance));
+		
+		// Convert barycentric coordinates of closest point to Cartesian coordinates
+		traversal.cartesian = geom::barycentric_to_cartesian(traversal.barycentric, a, b, c);
+		
+		// Subtract length of projected segment from remaining distance
+		segment_length = math::length(traversal.cartesian - ray.origin);
+		distance -= segment_length;
+		
+		// Advance ray origin
+		ray.origin = traversal.cartesian;
+		
+		// If no edge reached or no remaining distance, traversal is complete
+		if (edge_index < 0 || distance <= 0.0f)
+		{
+			break;
+		}
+		
+		// Find loop and edge on which the closest point lies
+		auto closest_loop_it = face->loops().begin();
+		std::advance(closest_loop_it, edge_index);
+		geom::brep_loop* closest_loop = *closest_loop_it;
+		geom::brep_edge* closest_edge = closest_loop->edge();
+		
+		// Abort if a boundary edge was reached
+		if (closest_edge->loops().size() == 1)
+		{
+			traversal.edge = closest_edge;
+			break;
+		}
+		
+		// Find a loop and face that shares the closest edge
+		auto symmetric_loop_it = closest_edge->loops().begin();
+		if (*symmetric_loop_it == closest_loop)
+		{
+			++symmetric_loop_it;
+		}
+		geom::brep_loop* symmetric_loop = *symmetric_loop_it;
+		geom::brep_face* symmetric_face = symmetric_loop->face();
+		
+		// Find quaternion representing rotation from normal of first face to normal of second face
+		const auto& n0 = face_normals[face->index()];
+		const auto& n1 = face_normals[symmetric_face->index()];
+		const auto rotation = math::rotation(n0, n1);
+		
+		// Rotate ray direction
+		ray.direction = rotation * ray.direction;
+		
+		// Move to next face
+		face = symmetric_face;
+	}
+	while (segment_length > 0.0f);
+	
+	traversal.face = face;
+	traversal.remaining_distance = distance;
+	
+	return traversal;
+}
+
+} // namespace ai

src/engine/geom/spherical.hpp → src/engine/ai/navmesh.hpp

@@ -17,42 +17,32 @@
  * along with Antkeeper source code.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef ANTKEEPER_GEOM_SPHERICAL_HPP
-#define ANTKEEPER_GEOM_SPHERICAL_HPP
+#ifndef ANTKEEPER_AI_NAVMESH_HPP
+#define ANTKEEPER_AI_NAVMESH_HPP
 
 #include <engine/math/vector.hpp>
-#include <cmath>
+#include <engine/math/se3.hpp>
+#include <engine/geom/brep/brep-mesh.hpp>
+#include <engine/geom/primitives/point.hpp>
+#include <engine/geom/primitives/ray.hpp>
+#include <vector>
 
-namespace geom {
+namespace ai {
 
-/// Functions which operate on spherical coordinates.
-namespace spherical {
+struct navmesh_traversal
+{
+	geom::brep_face* face;
+	geom::brep_edge* edge;
+	geom::point<float, 3> barycentric;
+	geom::point<float, 3> cartesian;
+	float remaining_distance;
+};
 
 /**
- * Converts spherical coordinates to Cartesian (rectangular) coordinates.
- *
- * @param v Spherical coordinates, in the ISO order of radial distance, polar angle (radians), and azimuthal angle (radians).
- * @return Cartesian coordinates.
- *
- * @see geom::coordinates::cartesian
+ * 
  */
-template <class T>
-math::vec3<T> to_cartesian(const math::vec3<T>& v);
-
-template <class T>
-math::vec3<T> to_cartesian(const math::vec3<T>& v)
-{
-	const T x = v[0] * std::cos(v[1]);
-	
-	return math::vec3<T>
-	{
-		x * std::cos(v[2]),
-		x * std::sin(v[2]),
-		v[0] * std::sin(v[1])
-	};
-}
+[[nodiscard]] navmesh_traversal traverse_navmesh(const geom::brep_mesh& mesh, geom::brep_face* face, geom::ray<float, 3> ray, float distance);
 
-} // namespace spherical
-} // namespace geom
+} // namespace ai
 
-#endif // ANTKEEPER_GEOM_SPHERICAL_HPP
+#endif // ANTKEEPER_AI_NAVMESH_HPP

src/engine/geom/cartesian.hpp

@@ -1,58 +0,0 @@
-/*
- * Copyright (C) 2023  Christopher J. Howard
- *
- * This file is part of Antkeeper source code.
- *
- * Antkeeper source code is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Antkeeper source code is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Antkeeper source code.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef ANTKEEPER_GEOM_CARTESIAN_HPP
-#define ANTKEEPER_GEOM_CARTESIAN_HPP
-
-#include <engine/math/vector.hpp>
-#include <cmath>
-
-namespace geom {
-
-/// Functions which operate on Cartesian (rectangular) coordinates.
-namespace cartesian {
-
-/**
- * Converts Cartesian (rectangular) coordinates to spherical coordinates.
- *
- * @param v Cartesian coordinates.
- * @return Spherical coordinates, in the ISO order of radial distance, polar angle (radians), and azimuthal angle (radians).
- *
- * @see geom::coordinates::spherical
- */
-template <class T>
-math::vec3<T> to_spherical(const math::vec3<T>& v);
-
-template <class T>
-math::vec3<T> to_spherical(const math::vec3<T>& v)
-{
-	const T xx_yy = v.x() * v.x() + v.y() * v.y();
-	
-	return math::vec3<T>
-	{
-		std::sqrt(xx_yy + v.z() * v.z()),
-		std::atan2(v.z(), std::sqrt(xx_yy)),
-		std::atan2(v.y(), v.x())
-	};
-}
-
-} // namespace cartesian
-} // namespace geom
-
-#endif // ANTKEEPER_GEOM_CARTESIAN_HPP

src/engine/geom/closest-feature.hpp

@@ -0,0 +1,90 @@
+/*
+ * Copyright (C) 2023  Christopher J. Howard
+ *
+ * This file is part of Antkeeper source code.
+ *
+ * Antkeeper source code is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Antkeeper source code is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Antkeeper source code.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef ANTKEEPER_GEOM_CLOSEST_FEATURE_HPP
+#define ANTKEEPER_GEOM_CLOSEST_FEATURE_HPP
+
+#include <engine/geom/primitives/point.hpp>
+#include <engine/geom/primitives/triangle.hpp>
+#include <algorithm>
+#include <tuple>
+
+namespace geom {
+
+/**
+ * Calculates the closest features on a triangle to a point.
+ *
+ * @tparam T Real type.
+ *
+ * @param tri Triangle.
+ * @param a First point of triangle.
+ * @param b Second point of triangle.
+ * @param c Third point of triangle.
+ * @param p Point.
+ *
+ * @return Tuple containing the Barycentric coordinates of the closest point on the triangle to point @p p, followed by the index of the edge on which the point lies (`-1` if not on an edge).
+ */
+/// @{
+template <class T>
+[[nodiscard]] constexpr std::tuple<point<T, 3>, int> closest_feature(const point<T, 3>& a, const point<T, 3>& b, const point<T, 3>& c, const point<T, 3>& p) noexcept
+{
+	const auto ab = b - a;
+	const auto ca = a - c;
+	const auto ap = p - a;
+	const auto n = math::cross(ab, ca);
+	const auto d = math::sqr_length(n);
+	const auto q = math::cross(n, ap);
+	
+	point<T, 3> uvw;
+	if ((uvw.z() = math::dot(q, ab) / d) < T{0})
+	{
+		uvw.z() = T{0};
+		uvw.y() = std::min<T>(std::max<T>(math::dot(ab, ap) / math::sqr_length(ab), T{0}), T{1});
+		uvw.x() = T{1} - uvw.y();
+		return {uvw, 0};
+	}
+	else if ((uvw.y() = math::dot(q, ca) / d) < T{0})
+	{
+		uvw.y() = T{0};
+		uvw.x() = std::min<T>(std::max<T>(math::dot(ca, p - c) / math::sqr_length(ca), T{0}), T{1});
+		uvw.z() = T{1} - uvw.x();
+		return {uvw, 2};
+	}
+	else if ((uvw.x() = T{1} - uvw.y() - uvw.z()) < T{0})
+	{
+		uvw.x() = T{0};
+		const auto bc = c - b;
+		uvw.z() = std::min<T>(std::max<T>(math::dot(bc, p - b) / math::sqr_length(bc), T{0}), T{1});
+		uvw.y() = T{1} - uvw.z();
+		return {uvw, 1};
+	}
+	
+	return {uvw, -1};
+}
+
+template <class T>
+[[nodiscard]] inline constexpr std::tuple<point<T, 3>, int> closest_feature(const triangle<T, 3>& tri, const point<T, 3>& p) noexcept
+{
+	return closest_feature(tri.a, tri.b, tri.c, p);
+}
+/// @}
+
+} // namespace geom
+
+#endif // ANTKEEPER_GEOM_CLOSEST_FEATURE_HPP

src/engine/geom/closest-point.hpp

@@ -27,6 +27,7 @@
 #include <engine/geom/primitives/line-segment.hpp>
 #include <engine/geom/primitives/point.hpp>
 #include <engine/geom/primitives/ray.hpp>
+#include <engine/geom/primitives/triangle.hpp>
 #include <algorithm>
 #include <cmath>
 #include <tuple>
@@ -189,6 +190,58 @@ template
 	return b - a.normal * (math::dot(a.normal, b) + a.constant);
 }
 
+/**
+ * Calculates the closest point on a triangle to a point.
+ *
+ * @tparam T Real type.
+ *
+ * @param tri Triangle.
+ * @param a First point of triangle.
+ * @param b Second point of triangle.
+ * @param c Third point of triangle.
+ * @param p Point.
+ *
+ * @return Closest point on the triangle to point @p p, followed by the index of the edge on which the point lies (`-1` if not on an edge).
+ */
+/// @{
+template <class T>
+[[nodiscard]] constexpr point<T, 3> closest_point(const point<T, 3>& a, const point<T, 3>& b, const point<T, 3>& c, const point<T, 3>& p) noexcept
+{
+	const auto ab = b - a;
+	const auto ca = a - c;
+	const auto ap = p - a;
+	const auto n = math::cross(ab, ca);
+	const auto d = math::sqr_length(n);
+	const auto q = math::cross(n, ap);
+	
+	T u, v, w;
+	if ((w = math::dot(q, ab) / d) < T{0})
+	{
+		v = std::min<T>(std::max<T>(math::dot(ab, ap) / math::sqr_length(ab), T{0}), T{1});
+		return {a * (T{1} - v) + b * v};
+	}
+	else if ((v = math::dot(q, ca) / d) < T{0})
+	{
+		u = std::min<T>(std::max<T>(math::dot(ca, p - c) / math::sqr_length(ca), T{0}), T{1});
+		return {a * u + c * (T{1} - u)};
+	}
+	else if ((u = T{1} - v - w) < T{0})
+	{
+		const auto bc = c - b;
+		w = std::min<T>(std::max<T>(math::dot(bc, p - b) / math::sqr_length(bc), T{0}), T{1});
+		return {b * (T{1} - w) + c * w};
+	}
+	
+	return {a * u + b * v + c * w};
+}
+
+template <class T>
+[[nodiscard]] inline constexpr point<T, 3> closest_point(const triangle<T, 3>& tri, const point<T, 3>& p) noexcept
+{
+	return closest_point(tri.a, tri.b, tri.c, p);
+}
+/// @}
+
 /**
  * Calculates the closest point on or in a hypersphere to a point.
  *

src/engine/geom/coordinates.hpp

@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) 2023  Christopher J. Howard
+ *
+ * This file is part of Antkeeper source code.
+ *
+ * Antkeeper source code is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Antkeeper source code is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Antkeeper source code.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef ANTKEEPER_GEOM_COORDINATES_HPP
+#define ANTKEEPER_GEOM_COORDINATES_HPP
+
+#include <engine/geom/primitives/point.hpp>
+#include <cmath>
+
+namespace geom {
+
+/**
+ * Converts Cartesian coordinates to barycentric coordinates.
+ *
+ * @tparam T Real type.
+ *
+ * @param p Barycentric coordinates of point to convert.
+ * @param a Cartesian coordinates of first point of triangle.
+ * @param b Cartesian coordinates of second point of triangle.
+ * @param c Cartesian coordinates of third point of triangle.
+ *
+ * @return Cartesian coordinates of point @p p.
+ */
+template <class T>
+[[nodiscard]] inline constexpr point<T, 3> barycentric_to_cartesian(const point<T, 3>& p, const point<T, 3>& a, const point<T, 3>& b, const point<T, 3>& c) noexcept
+{
+	return a * p.x() + b * p.y() + c * p.z();
+}
+
+/**
+ * Converts Cartesian coordinates to barycentric coordinates.
+ *
+ * @tparam T Real type.
+ *
+ * @param p Cartesian coordinates of point to convert.
+ * @param a Cartesian coordinates of first point of triangle.
+ * @param b Cartesian coordinates of second point of triangle.
+ * @param c Cartesian coordinates of third point of triangle.
+ *
+ * @return Barycentric coordinates of point @p p.
+ */
+template <class T>
+[[nodiscard]] constexpr point<T, 3> cartesian_to_barycentric(const point<T, 3>& p, const point<T, 3>& a, const point<T, 3>& b, const point<T, 3>& c) noexcept
+{
+	const auto ab = b - a;
+	const auto ca = a - c;
+	const auto n = math::cross(ab, ca);
+	const auto d = math::sqr_length(n);
+	const auto q = math::cross(n, p - a);
+	
+	point<T, 3> uvw;
+	uvw.z() = math::dot(q, ab) / d;
+	uvw.y() = math::dot(q, ca) / d;
+	uvw.x() = T{1} - uvw.y() - uvw.z();
+	
+	return uvw;
+}
+
+/**
+ * Converts Cartesian (rectangular) coordinates to spherical coordinates.
+ *
+ * @tparam T Real type.
+ *
+ * @param p Cartesian coordinates of point to convert.
+ *
+ * @return Spherical coordinates of point @p p, in the ISO order of radial distance, polar angle (radians), and azimuthal angle (radians).
+ */
+template <class T>
+[[nodiscard]] point<T, 3> cartesian_to_spherical(const point<T, 3>& p)
+{
+	const T xx_yy = p.x() * p.x() + p.y() * p.y();
+	
+	return
+	{
+		std::sqrt(xx_yy + p.z() * p.z()),
+		std::atan2(p.z(), std::sqrt(xx_yy)),
+		std::atan2(p.y(), p.x())
+	};
+}
+
+/**
+ * Converts spherical coordinates to Cartesian (rectangular) coordinates.
+ *
+ * @tparam T Real type.
+ *
+ * @param p Spherical coordinates to convert, in the ISO order of radial distance, polar angle (radians), and azimuthal angle (radians).
+ *
+ * @return Cartesian coordinates of point @p p.
+ */
+template <class T>
+[[nodiscard]] point<T, 3> spherical_to_cartesian(const point<T, 3>& p)
+{
+	const T x = p.x() * std::cos(p.y());
+	
+	return
+	{
+		x * std::cos(p.z()),
+		x * std::sin(p.z()),
+		p.x() * std::sin(p.y())
+	};
+}
+
+} // namespace geom
+
+#endif // ANTKEEPER_GEOM_COORDINATES_HPP

src/engine/math/linear-algebra.hpp → src/engine/geom/primitives/triangle.hpp

@@ -17,11 +17,40 @@
  * along with Antkeeper source code.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef ANTKEEPER_MATH_LINEAR_ALGEBRA_HPP
-#define ANTKEEPER_MATH_LINEAR_ALGEBRA_HPP
+#ifndef ANTKEEPER_GEOM_PRIMITIVES_TRIANGLE_HPP
+#define ANTKEEPER_GEOM_PRIMITIVES_TRIANGLE_HPP
 
-#include <engine/math/matrix.hpp>
-#include <engine/math/quaternion.hpp>
 #include <engine/math/vector.hpp>
 
-#endif // ANTKEEPER_MATH_LINEAR_ALGEBRA_HPP
+namespace geom {
+namespace primitives {
+
+/**
+ * *n*-dimensional triangle.
+ *
+ * @tparam T Real type.
+ * @tparam N Number of dimensions.
+ */
+template <class T, std::size_t N>
+struct triangle
+{
+	/// Vector type.
+	using vector_type = math::vector<T, N>;
+	
+	/// First point.
+	vector_type a;
+	
+	/// Second point.
+	vector_type b;
+	
+	/// Third point.
+	vector_type c;
+};
+
+} // namespace primitives
+
+using namespace primitives;
+
+} // namespace geom
+
+#endif // ANTKEEPER_GEOM_PRIMITIVES_TRIANGLE_HPP

src/engine/math/se3.hpp

@@ -24,10 +24,12 @@
 #include <engine/math/quaternion.hpp>
 
 namespace math {
-namespace transformation {
+
+/// Transformation types
+namespace transformation_types {
 
 /**
- * 3-dimensional Euclidean proper rigid transformation in SE(3).
+ * SE(3) proper rigid transformation (rototranslation).
  *
  * @tparam T Scalar type.
  */
@@ -36,101 +38,116 @@ struct se3
 {
 public:
 	/// Scalar type.
-	typedef T scalar_type;
+	using scalar_type = T;
 	
 	/// Vector type.
-	typedef math::vec3<T> vector_type;
+	using vector_type = vec3<T>;
 	
 	/// Quaternion type.
-	typedef math::quaternion<T> quaternion_type;
+	using quaternion_type = quat<T>;
 	
 	/// Transformation matrix type.
-	typedef math::mat4<T> matrix_type;
+	using matrix_type = mat4<T>;
 	
-	/// Vector representing the translation component of this SE(3) transformation.
+	/// Vector representing the translation component of the transformation.
 	vector_type t;
 	
-	/// Quaternion representing the rotation component of this SE(3) transformation.
+	/// Quaternion representing the rotation component of the transformation.
 	quaternion_type r;
 	
-	/// Returns the inverse of this SE(3) transformation.
-	[[nodiscard]] se3 inverse() const;
+	/// Returns the inverse of this transformation.
+	[[nodiscard]] constexpr se3 inverse() const noexcept
+	{
+		const quaternion_type inverse_r = conjugate(r);
+		const vector_type inverse_t = -(inverse_r * t);
+		return {inverse_t, inverse_r};
+	}
 	
-	/// Returns a matrix representation of the SE(3) transformation.
-	[[nodiscard]] matrix_type matrix() const;
+	/// Returns a matrix representation of this transformation.
+	/// @{
+	[[nodiscard]] constexpr matrix_type matrix() const noexcept
+	{
+		matrix_type m = mat4<T>(mat3<T>(r));
+		
+		m[3].x() = t.x();
+		m[3].y() = t.y();
+		m[3].z() = t.z();
+		
+		return m;
+	}
+	
+	[[nodiscard]] inline constexpr explicit operator matrix_type() const noexcept
+	{
+		return matrix();
+	}
+	/// @}
 	
 	/**
-	 * Transforms a vector by this SE(3) transformation.
+	 * Transforms a vector by this transformation.
+	 *
+	 * @param v Untransformed vector.
 	 *
-	 * @param x Untransformed vector.
 	 * @return Transformed vector.
 	 */
-	[[nodiscard]] vector_type transform(const vector_type& x) const;
+	/// @{
+	[[nodiscard]] inline constexpr vector_type transform(const vector_type& v) const noexcept
+	{
+		return r * v + t;
+	}
+	
+	[[nodiscard]] inline constexpr vector_type operator*(const vector_type& v) const noexcept
+	{
+		return transform(v);
+	}
+	/// @}
 	
 	/**
-	 * Transforms an SE(3) transformation by this SE(3) transformation.
+	 * Transforms an SE(3) transformation by this transformation.
+	 *
+	 * @param xf SE(3) transformation.
 	 *
-	 * @param x Other SE(3) transformation.
 	 * @return Frame in this se3's space.
 	 */
-	[[nodiscard]] se3 transform(const se3& x) const;
-	
-	/// @copydoc se3::transform(const vector_type&) const
-	[[nodiscard]] vector_type operator*(const vector_type& x) const;
+	/// @{
+	[[nodiscard]] constexpr se3 transform(const se3& xf) const noexcept
+	{
+		return {xf.transform(t), normalize(xf.r * r)};
+	}
 	
-	/// @copydoc se3::transform(const se3&) const
-	[[nodiscard]] se3 operator*(const se3& x) const;
-};
-
-template <class T>
-se3<T> se3<T>::inverse() const
-{
-	const quaternion_type inverse_r = math::conjugate(r);
-	const vector_type inverse_t = -(inverse_r * t);
-	return se3{inverse_t, inverse_r};
-}
-
-template <class T>
-typename se3<T>::matrix_type se3<T>::matrix() const
-{
-	matrix_type m = math::mat4<T>(math::mat3<T>(r));
+	[[nodiscard]] inline constexpr se3 operator*(const se3& xf) const noexcept
+	{
+		return transform(xf);
+	}
+	/// @}
 	
-	m[3].x() = t.x();
-	m[3].y() = t.y();
-	m[3].z() = t.z();
+	/*
+	 * Type-casts the transform scalars using `static_cast`.
+	 *
+	 * @tparam U Target scalar type.
+	 *
+	 * @return Type-casted transform.
+	 */
+	template <class U>
+	[[nodiscard]] inline constexpr explicit operator se3<U>() const noexcept
+	{
+		return {vec3<U>(t), quat<U>(r)};
+	}
 	
-	return m;
-}
-
-template <class T>
-typename se3<T>::vector_type se3<T>::transform(const vector_type& x) const
-{
-	return r * x + t;
-}
-
-template <class T>
-se3<T> se3<T>::transform(const se3& x) const
-{
-	return se3
+	/// Returns an identity transformation.
+	[[nodiscard]] static inline constexpr se3 identity() noexcept
 	{
-		x.transform(t),
-		math::normalize(x.r * r)
-	};
-}
+		return {vector_type::zero(), quaternion_type::identity()};
+	}
+};
 
-template <class T>
-typename se3<T>::vector_type se3<T>::operator*(const vector_type& x) const
-{
-	return transform(x);
-}
+} // namespace transformation_types
 
-template <class T>
-se3<T> se3<T>::operator*(const se3& x) const
-{
-	return transform(x);
-}
+// Bring transformation types into math namespace
+using namespace transformation_types;
+
+// Bring transformation types into math::types namespace
+namespace types { using namespace math::transformation_types; }
 
-} // namespace transformation
 } // namespace math
 
 #endif // ANTKEEPER_MATH_TRANSFORMATION_SE3_HPP

src/engine/math/transform.hpp

@@ -27,9 +27,7 @@
 namespace math {
 
 /**
- * 3D transformation.
- *
- * Transformations are applied in the following order: scale, rotation, translation.
+ * SRT transformation.
  *
  * @tparam T Scalar type.
  */
@@ -40,13 +38,13 @@ struct transform
 	using scalar_type = T;
 	
 	/// Vector type.
-	using vector_type = vector<T, 3>;
+	using vector_type = vec3<T>;
 	
 	/// Quaternion type.
-	using quaternion_type = quaternion<T>;
+	using quaternion_type = quat<T>;
 	
 	/// Transformation matrix type.
-	using matrix_type = matrix<T, 4, 4>;
+	using matrix_type = mat4<T>;
 	
 	/// Translation vector.
 	vector_type translation;
@@ -102,7 +100,7 @@ struct transform
 	template <class U>
 	[[nodiscard]] inline constexpr explicit operator transform<U>() const noexcept
 	{
-		return {math::vector<U, 3>(translation), math::quaternion<U>(rotation), math::vector<U, 3>(scale)};
+		return {vec3<U>(translation), quat<U>(rotation), vec3<U>(scale)};
 	}
 	
 	/// Returns an identity transform.

src/engine/physics/orbit/frame.hpp

@@ -143,7 +143,7 @@ namespace pqw {
 	 * @return PQW to BCI transformation.
 	 */
 	template <typename T>
-	math::transformation::se3<T> to_bci(T om, T in, T w)
+	math::se3<T> to_bci(T om, T in, T w)
 	{
 		const math::quaternion<T> r = math::normalize
 		(
@@ -152,7 +152,7 @@ namespace pqw {
 				math::quaternion<T>::rotate_z(w)
 		);
 		
-		return math::transformation::se3<T>{{T(0), T(0), T(0)}, r};
+		return math::se3<T>{{T(0), T(0), T(0)}, r};
 	}
 
 } // namespace pqw
@@ -210,7 +210,7 @@ namespace bci {
 	 * @see Archinal, B.A., A’Hearn, M.F., Bowell, E. et al. Report of the IAU Working Group on Cartographic Coordinates and Rotational Elements: 2009. Celest Mech Dyn Astr 109, 101–135 (2011). https://doi.org/10.1007/s10569-010-9320-4
 	 */
 	template <typename T>
-	math::transformation::se3<T> to_bcbf(T ra, T dec, T w)
+	math::se3<T> to_bcbf(T ra, T dec, T w)
 	{
 		const math::quaternion<T> r = math::normalize
 		(
@@ -219,7 +219,7 @@ namespace bci {
 				math::quaternion<T>::rotate_z(-w)
 		);
 		
-		return math::transformation::se3<T>{{T(0), T(0), T(0)}, r};
+		return math::se3<T>{{T(0), T(0), T(0)}, r};
 	}
 	
 	/**
@@ -231,7 +231,7 @@ namespace bci {
 	 * @return BCI to PQW transformation.
 	 */
 	template <typename T>
-	math::transformation::se3<T> to_pqw(T om, T in, T w)
+	math::se3<T> to_pqw(T om, T in, T w)
 	{
 		const math::quaternion<T> r = math::normalize
 		(
@@ -240,7 +240,7 @@ namespace bci {
 				math::quaternion<T>::rotate_z(-om)
 		);
 		
-		return math::transformation::se3<T>{{T(0), T(0), T(0)}, r};
+		return math::se3<T>{{T(0), T(0), T(0)}, r};
 	}
 	
 } // namespace bci
@@ -298,7 +298,7 @@ namespace bcbf {
 	 * @see Archinal, B.A., A’Hearn, M.F., Bowell, E. et al. Report of the IAU Working Group on Cartographic Coordinates and Rotational Elements: 2009. Celest Mech Dyn Astr 109, 101–135 (2011). https://doi.org/10.1007/s10569-010-9320-4
 	 */
 	template <typename T>
-	math::transformation::se3<T> to_bci(T ra, T dec, T w)
+	math::se3<T> to_bci(T ra, T dec, T w)
 	{
 		const math::quaternion<T> r = math::normalize
 		(
@@ -308,7 +308,7 @@ namespace bcbf {
 
 		);
 		
-		return math::transformation::se3<T>{{T(0), T(0), T(0)}, r};
+		return math::se3<T>{{T(0), T(0), T(0)}, r};
 	}
 	
 	/**
@@ -320,7 +320,7 @@ namespace bcbf {
 	 * @return BCBF to ENU transformation.
 	 */
 	template <typename T>
-	math::transformation::se3<T> to_enu(T distance, T latitude, T longitude)
+	math::se3<T> to_enu(T distance, T latitude, T longitude)
 	{
 		const math::vec3<T> t = {T(0), T(0), -distance};
 		const math::quaternion<T> r = math::normalize
@@ -329,7 +329,7 @@ namespace bcbf {
 				math::quaternion<T>::rotate_z(-longitude - math::half_pi<T>)
 		);
 		
-		return math::transformation::se3<T>{t, r};
+		return math::se3<T>{t, r};
 	}
 
 } // namespace bcbf
@@ -385,7 +385,7 @@ namespace enu {
 	 * @return ENU to BCBF transformation.
 	 */
 	template <typename T>
-	math::transformation::se3<T> to_bcbf(T distance, T latitude, T longitude)
+	math::se3<T> to_bcbf(T distance, T latitude, T longitude)
 	{
 		const math::vec3<T> t = {T(0), T(0), distance};
 		const math::quaternion<T> r = math::normalize
@@ -394,7 +394,7 @@ namespace enu {
 				math::quaternion<T>::rotate_x(math::half_pi<T> - latitude)
 		);
 		
-		return math::transformation::se3<T>{r * t, r};
+		return math::se3<T>{r * t, r};
 	}
 	
 } // namespace enu

src/engine/render/passes/sky-pass.cpp

@@ -38,8 +38,6 @@
 #include <engine/math/vector.hpp>
 #include <engine/color/color.hpp>
 #include <engine/math/interpolation.hpp>
-#include <engine/geom/cartesian.hpp>
-#include <engine/geom/spherical.hpp>
 #include <engine/physics/orbit/orbit.hpp>
 #include <engine/physics/light/photometry.hpp>
 #include <bit>
@@ -233,7 +231,7 @@ void sky_pass::render(render::context& ctx)
 	observer_position = observer_position_tween.interpolate(ctx.alpha);
 	
 	// Construct tweened ICRF to EUS transformation
-	math::transformation::se3<float> icrf_to_eus =
+	math::se3<float> icrf_to_eus =
 	{
 		icrf_to_eus_translation.interpolate(ctx.alpha),
 		icrf_to_eus_rotation.interpolate(ctx.alpha)
@@ -672,7 +670,7 @@ void sky_pass::set_magnification(float magnification)
 	this->magnification = magnification;
 }
 
-void sky_pass::set_icrf_to_eus(const math::transformation::se3<float>& transformation)
+void sky_pass::set_icrf_to_eus(const math::se3<float>& transformation)
 {
 	icrf_to_eus_translation[1] = transformation.t;
 	icrf_to_eus_rotation[1] = transformation.r;

src/engine/render/passes/sky-pass.hpp

@@ -184,7 +184,7 @@ public:
 	void set_moon_model(std::shared_ptr<render::model> model);
 	void set_stars_model(std::shared_ptr<render::model> model);
 	
-	void set_icrf_to_eus(const math::transformation::se3<float>& transformation);
+	void set_icrf_to_eus(const math::se3<float>& transformation);
 	
 	void set_sun_position(const math::fvec3& position);
 	void set_sun_luminance(const math::fvec3& luminance);

src/game/states/experiments/treadmill-experiment-state.cpp

@@ -0,0 +1,904 @@
+/*
+ * Copyright (C) 2023  Christopher J. Howard
+ *
+ * This file is part of Antkeeper source code.
+ *
+ * Antkeeper source code is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Antkeeper source code is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Antkeeper source code.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "game/states/experiments/treadmill-experiment-state.hpp"
+
+#include "game/ant/ant-cladogenesis.hpp"
+#include "game/ant/ant-genome.hpp"
+#include "game/ant/ant-morphogenesis.hpp"
+#include "game/ant/ant-phenome.hpp"
+#include "game/commands/commands.hpp"
+#include "game/components/constraint-stack-component.hpp"
+#include "game/components/scene-component.hpp"
+#include "game/components/picking-component.hpp"
+#include "game/components/spring-component.hpp"
+#include "game/components/rigid-body-component.hpp"
+#include "game/components/rigid-body-constraint-component.hpp"
+#include "game/components/steering-component.hpp"
+#include "game/components/terrain-component.hpp"
+#include "game/components/legged-locomotion-component.hpp"
+#include "game/components/winged-locomotion-component.hpp"
+#include "game/components/ik-component.hpp"
+#include "game/components/transform-component.hpp"
+#include "game/constraints/child-of-constraint.hpp"
+#include "game/constraints/copy-rotation-constraint.hpp"
+#include "game/constraints/copy-scale-constraint.hpp"
+#include "game/constraints/copy-transform-constraint.hpp"
+#include "game/constraints/copy-translation-constraint.hpp"
+#include "game/constraints/ease-to-constraint.hpp"
+#include "game/constraints/pivot-constraint.hpp"
+#include "game/constraints/spring-rotation-constraint.hpp"
+#include "game/constraints/spring-to-constraint.hpp"
+#include "game/constraints/spring-translation-constraint.hpp"
+#include "game/constraints/three-dof-constraint.hpp"
+#include "game/constraints/track-to-constraint.hpp"
+#include "game/controls.hpp"
+#include "game/spawn.hpp"
+#include "game/states/pause-menu-state.hpp"
+#include "game/systems/astronomy-system.hpp"
+#include "game/systems/atmosphere-system.hpp"
+#include "game/systems/camera-system.hpp"
+#include "game/systems/collision-system.hpp"
+#include "game/world.hpp"
+#include <engine/animation/ease.hpp>
+#include <engine/animation/screen-transition.hpp>
+#include <engine/config.hpp>
+#include <engine/entity/archetype.hpp>
+#include <engine/input/mouse.hpp>
+#include <engine/math/interpolation.hpp>
+#include <engine/math/projection.hpp>
+#include <engine/physics/light/exposure.hpp>
+#include <engine/physics/kinematics/constraints/spring-constraint.hpp>
+#include <engine/physics/kinematics/colliders/sphere-collider.hpp>
+#include <engine/physics/kinematics/colliders/plane-collider.hpp>
+#include <engine/physics/kinematics/colliders/box-collider.hpp>
+#include <engine/physics/kinematics/colliders/capsule-collider.hpp>
+#include <engine/render/passes/clear-pass.hpp>
+#include <engine/render/passes/ground-pass.hpp>
+#include <engine/render/passes/material-pass.hpp>
+#include <engine/resources/resource-manager.hpp>
+#include <engine/utility/state-machine.hpp>
+#include <engine/scene/static-mesh.hpp>
+#include <engine/scene/skeletal-mesh.hpp>
+#include <engine/scene/rectangle-light.hpp>
+#include <engine/scene/point-light.hpp>
+#include <engine/geom/intersection.hpp>
+#include <engine/animation/ease.hpp>
+#include <engine/color/color.hpp>
+#include <engine/geom/brep/brep-operations.hpp>
+#include <engine/geom/coordinates.hpp>
+#include <engine/ai/navmesh.hpp>
+
+treadmill_experiment_state::treadmill_experiment_state(::game& ctx):
+	game_state(ctx)
+{
+	debug::log::trace("Entering nest view state...");	
+	
+	// Create world if not yet created
+	if (ctx.entities.find("earth") == ctx.entities.end())
+	{
+		// Create cosmos
+		::world::cosmogenesis(ctx);
+		
+		// Create observer
+		::world::create_observer(ctx);
+	}
+	
+	ctx.active_ecoregion = ctx.resource_manager->load<::ecoregion>("seedy-scrub.eco");
+	::world::enter_ecoregion(ctx, *ctx.active_ecoregion);
+	
+	debug::log::trace("Generating genome...");
+	std::unique_ptr<ant_genome> genome = ant_cladogenesis(ctx.active_ecoregion->gene_pools[0], ctx.rng);
+	debug::log::trace("Generated genome");
+	
+	debug::log::trace("Building worker phenome...");
+	worker_phenome = ant_phenome(*genome, ant_caste_type::worker);
+	debug::log::trace("Built worker phenome...");
+	
+	debug::log::trace("Generating worker model...");
+	std::shared_ptr<render::model> worker_model = ant_morphogenesis(worker_phenome);
+	debug::log::trace("Generated worker model");
+	
+	// Create directional light
+	// ctx.underground_directional_light = std::make_unique<scene::directional_light>();
+	// ctx.underground_directional_light->set_color({1.0f, 1.0f, 1.0f});
+	// ctx.underground_directional_light->set_illuminance(2.0f);
+	// ctx.underground_directional_light->set_direction(math::normalize(math::fvec3{0, -1, 0}));
+	// ctx.underground_directional_light->set_shadow_caster(true);
+	// ctx.underground_directional_light->set_shadow_framebuffer(ctx.shadow_map_framebuffer);
+	// ctx.underground_directional_light->set_shadow_bias(0.005f);
+	// ctx.underground_directional_light->set_shadow_cascade_count(4);
+	// ctx.underground_directional_light->set_shadow_cascade_coverage(0.15f);
+	// ctx.underground_directional_light->set_shadow_cascade_distribution(0.8f);
+	// ctx.underground_scene->add_object(*ctx.underground_directional_light);
+	
+	// ctx.underground_clear_pass->set_clear_color({0.214f, 0.214f, 0.214f, 1.0f});
+	// ctx.underground_clear_pass->set_clear_color({});
+	// light_probe = std::make_shared<scene::light_probe>();
+	// light_probe->set_luminance_texture(ctx.resource_manager->load<gl::texture_cube>("grey-furnace.tex"));
+	// ctx.underground_scene->add_object(*light_probe);
+	
+	//const float color_temperature = 5000.0f;
+	//const math::fvec3 light_color = color::aces::ap1<float>.from_xyz * color::cat::matrix(color::illuminant::deg2::d50<float>, color::aces::white_point<float>) * color::cct::to_xyz(color_temperature);
+	const math::fvec3 light_color{1.0f, 1.0f, 1.0f}; 
+	
+	// Create rectangle light
+	// ctx.underground_rectangle_light = std::make_unique<scene::rectangle_light>();
+	// ctx.underground_rectangle_light->set_color(light_color);
+	// ctx.underground_rectangle_light->set_luminous_flux(1500.0f);
+	// ctx.underground_rectangle_light->set_translation({0.0f, 10.0f, 0.0f});
+	// ctx.underground_rectangle_light->set_rotation(math::fquat::rotate_x(math::radians(90.0f)));
+	// ctx.underground_rectangle_light->set_scale(7.0f);
+	// ctx.underground_scene->add_object(*ctx.underground_rectangle_light);
+	
+	// Create light rectangle
+	// auto light_rectangle_model = ctx.resource_manager->load<render::model>("light-rectangle.mdl");
+	// auto light_rectangle_material = std::make_shared<render::material>(*light_rectangle_model->get_groups().front().material);
+	// light_rectangle_emissive = std::static_pointer_cast<render::matvar_fvec3>(light_rectangle_material->get_variable("emissive"));
+	// light_rectangle_emissive->set(ctx.underground_rectangle_light->get_colored_luminance());
+	// auto light_rectangle_static_mesh = std::make_shared<scene::static_mesh>(light_rectangle_model);
+	// light_rectangle_static_mesh->set_material(0, light_rectangle_material);
+	
+	// auto light_rectangle_eid = ctx.entity_registry->create();
+	// ctx.entity_registry->emplace<scene_component>(light_rectangle_eid, std::move(light_rectangle_static_mesh), std::uint8_t{1});
+	// ctx.entity_registry->patch<scene_component>
+	// (
+		// light_rectangle_eid,
+		// [&](auto& component)
+		// {
+			// component.object->set_transform(ctx.underground_rectangle_light->get_transform());
+		// }
+	// );
+	
+	// Create treadmill
+	auto treadmill_eid = ctx.entity_registry->create();
+	scene_component treadmill_scene_component;
+	treadmill_scene_component.object = std::make_shared<scene::static_mesh>(ctx.resource_manager->load<render::model>("cube-15cm.mdl"));
+	treadmill_scene_component.layer_mask = 1;
+	ctx.entity_registry->emplace<scene_component>(treadmill_eid, std::move(treadmill_scene_component));
+	
+	// Create worker
+	auto worker_skeletal_mesh = std::make_unique<scene::skeletal_mesh>(worker_model);
+	worker_eid = ctx.entity_registry->create();
+	transform_component worker_transform_component;
+	worker_transform_component.local = math::transform<float>::identity();
+	worker_transform_component.local.translation = {0, 0.1f, 0};
+	worker_transform_component.local.scale = math::fvec3::one() * worker_phenome.body_size->mean_mesosoma_length;
+	worker_transform_component.world = worker_transform_component.local;
+	ctx.entity_registry->emplace<transform_component>(worker_eid, worker_transform_component);
+	ctx.entity_registry->emplace<scene_component>(worker_eid, std::move(worker_skeletal_mesh), std::uint8_t{1});
+	
+	// Create color checker
+	auto color_checker_eid = ctx.entity_registry->create();
+	scene_component color_checker_scene_component;
+	color_checker_scene_component.object = std::make_shared<scene::static_mesh>(ctx.resource_manager->load<render::model>("color-checker.mdl"));
+	color_checker_scene_component.object->set_translation({0, 0, 4});
+	color_checker_scene_component.layer_mask = 1;
+	ctx.entity_registry->emplace<scene_component>(color_checker_eid, std::move(color_checker_scene_component));
+	
+	// Disable UI color clear
+	ctx.ui_clear_pass->set_cleared_buffers(false, true, false);
+	
+	// Set world time
+	::world::set_time(ctx, 2022, 6, 21, 12, 0, 0.0);
+	
+	// Init time scale
+	double time_scale = 60.0;
+	
+	// Set time scale
+	::world::set_time_scale(ctx, time_scale);
+	
+	// Setup and enable sky and ground passes
+	ctx.sky_pass->set_enabled(true);
+	ctx.ground_pass->set_enabled(true);
+	
+	sky_probe = std::make_shared<scene::light_probe>();
+	sky_probe->set_luminance_texture(std::make_shared<gl::texture_cube>(384, 512, gl::pixel_type::float_16, gl::pixel_format::rgb));
+	ctx.sky_pass->set_sky_probe(sky_probe);
+	ctx.surface_scene->add_object(*sky_probe);
+	
+	// Set camera exposure
+	const float ev100_sunny16 = physics::light::ev::from_settings(16.0f, 1.0f / 100.0f, 100.0f);
+	ctx.surface_camera->set_exposure_value(ev100_sunny16);
+	
+	const auto& viewport_size = ctx.window->get_viewport_size();
+	const float aspect_ratio = static_cast<float>(viewport_size[0]) / static_cast<float>(viewport_size[1]);
+	
+	// Create third person camera rig
+	create_third_person_camera_rig();
+	
+	// Setup controls
+	setup_controls();
+	
+	// Queue enable game controls
+	ctx.function_queue.push
+	(
+		[&ctx]()
+		{
+			::enable_game_controls(ctx);
+			::enable_keeper_controls(ctx);
+		}
+	);
+	
+	// Queue fade in
+	ctx.fade_transition_color->set({0, 0, 0});
+	ctx.function_queue.push(std::bind(&screen_transition::transition, ctx.fade_transition.get(), 1.0f, true, ease<float>::out_sine, true, nullptr));
+	
+	// Refresh frame scheduler
+	ctx.frame_scheduler.refresh();
+	
+	// Load navmesh
+	navmesh = ctx.resource_manager->load<geom::brep_mesh>("cube-15cm.msh");
+	
+	// Generate navmesh attributes
+	geom::generate_face_normals(*navmesh);
+	geom::generate_vertex_normals(*navmesh);
+	
+	// Build navmesh BVH
+	debug::log::info("building bvh");
+	navmesh_bvh = std::make_unique<geom::bvh>(*navmesh);
+	debug::log::info("building bvh done");
+	
+	debug::log::trace("Entered nest view state");
+}
+
+treadmill_experiment_state::~treadmill_experiment_state()
+{
+	debug::log::trace("Exiting nest view state...");
+	
+	// Disable game controls
+	::disable_game_controls(ctx);
+	::disable_keeper_controls(ctx);
+	
+	destroy_third_person_camera_rig();
+	
+	debug::log::trace("Exited nest view state");
+}
+
+void treadmill_experiment_state::create_third_person_camera_rig()
+{
+	// Construct third person camera rig scene component
+	scene_component third_person_camera_rig_camera;
+	third_person_camera_rig_camera.object = ctx.surface_camera;
+	third_person_camera_rig_camera.layer_mask = 1;
+	
+	// Construct third person camera rig entity
+	third_person_camera_rig_eid = ctx.entity_registry->create();
+	ctx.entity_registry->emplace<scene_component>(third_person_camera_rig_eid, third_person_camera_rig_camera);
+	
+	set_third_person_camera_zoom(third_person_camera_zoom);
+	set_third_person_camera_rotation(third_person_camera_yaw, third_person_camera_pitch);
+	update_third_person_camera();
+}
+
+void treadmill_experiment_state::destroy_third_person_camera_rig()
+{
+	ctx.entity_registry->destroy(third_person_camera_rig_eid);
+}
+
+void treadmill_experiment_state::set_third_person_camera_zoom(double zoom)
+{
+	// Clamp zoom
+	third_person_camera_zoom = std::min<double>(std::max<double>(zoom, 0.0), 1.0);
+	
+	// Update FoV
+	third_person_camera_hfov = ease<double, double>::out_sine(third_person_camera_far_hfov, third_person_camera_near_hfov, third_person_camera_zoom);
+	third_person_camera_vfov = math::vertical_fov(third_person_camera_hfov, static_cast<double>(ctx.surface_camera->get_aspect_ratio()));
+	
+	// Update focal plane size
+	third_person_camera_focal_plane_height = ease<double, double>::out_sine(third_person_camera_far_focal_plane_height, third_person_camera_near_focal_plane_height, third_person_camera_zoom);
+	third_person_camera_focal_plane_width = third_person_camera_focal_plane_height * ctx.surface_camera->get_aspect_ratio();
+	
+	// Update focal distance
+	third_person_camera_focal_distance = third_person_camera_focal_plane_height * 0.5 / std::tan(third_person_camera_vfov * 0.5);
+	
+	// update_third_person_camera();
+}
+
+void treadmill_experiment_state::set_third_person_camera_rotation(double yaw, double pitch)
+{
+	third_person_camera_yaw = yaw;
+	third_person_camera_pitch = std::min(math::half_pi<double>, std::max(-math::half_pi<double>, pitch));
+	
+	third_person_camera_yaw_rotation = math::angle_axis(third_person_camera_yaw, {0.0, 1.0, 0.0});
+	third_person_camera_pitch_rotation = math::angle_axis(third_person_camera_pitch, {-1.0, 0.0, 0.0});
+	third_person_camera_orientation = math::normalize(third_person_camera_yaw_rotation * third_person_camera_pitch_rotation);
+}
+
+void treadmill_experiment_state::zoom_third_person_camera(double zoom)
+{
+	set_third_person_camera_zoom(third_person_camera_zoom + zoom);
+}
+
+void treadmill_experiment_state::translate_third_person_camera(const math::dvec3& direction, double magnitude)
+{
+	// Scale translation magnitude by factor of focal plane height
+	magnitude *= third_person_camera_focal_plane_height * third_person_camera_speed;
+	
+	// Rotate translation direction according to camera yaw
+	const math::dvec3 rotated_direction = third_person_camera_yaw_rotation * direction;
+	
+	third_person_camera_focal_point += rotated_direction * magnitude;
+	
+	// update_third_person_camera();
+}
+
+void treadmill_experiment_state::rotate_third_person_camera(const input::mouse_moved_event& event)
+{
+	const double yaw = third_person_camera_yaw - ctx.mouse_pan_factor * static_cast<double>(event.difference.x());
+	const double pitch = third_person_camera_pitch + ctx.mouse_tilt_factor * static_cast<double>(event.difference.y());
+	
+	set_third_person_camera_rotation(yaw, pitch);
+}
+
+void treadmill_experiment_state::handle_mouse_motion(const input::mouse_moved_event& event)
+{
+	ctx.surface_material_pass->set_mouse_position(math::fvec2(event.position));
+	
+	if (!mouse_look && !mouse_grip && !mouse_zoom)
+	{
+		return;
+	}
+	
+	if (mouse_grip)
+	{
+		const math::dvec2 viewport_size = math::dvec2(ctx.window->get_viewport_size());
+		
+		math::dvec3 translation
+		{
+			third_person_camera_focal_plane_width * (static_cast<double>(-event.difference.x()) / (viewport_size.x() - 1.0)),
+			0.0,
+			third_person_camera_focal_plane_height * (static_cast<double>(-event.difference.y()) / (viewport_size.y() - 1.0)),
+		};
+		
+		if (third_person_camera_pitch < 0.0)
+		{
+			translation.z() *= -1.0;
+		}
+		
+		third_person_camera_focal_point += third_person_camera_yaw_rotation * translation;
+	}
+	
+	if (mouse_look)
+	{
+		rotate_third_person_camera(event);
+	}
+	
+	if (mouse_zoom)
+	{
+		const double zoom_speed = -1.0 / static_cast<double>(ctx.window->get_viewport_size().y());
+		zoom_third_person_camera(static_cast<double>(event.difference.y()) * zoom_speed);
+	}
+	
+	update_third_person_camera();
+}
+
+void treadmill_experiment_state::update_third_person_camera()
+{
+	const auto worker_rotation = ctx.entity_registry->get<::scene_component>(worker_eid).object->get_rotation();
+	
+	const auto camera_rotation = math::normalize(math::dquat(worker_rotation) * third_person_camera_orientation);
+	const math::dvec3 camera_position = third_person_camera_focal_point + camera_rotation * math::dvec3{0.0f, 0.0f, third_person_camera_focal_distance};
+	
+	ctx.entity_registry->patch<scene_component>
+	(
+		third_person_camera_rig_eid,
+		[&](auto& component)
+		{
+			auto& camera = static_cast<scene::camera&>(*component.object);
+			
+			camera.set_translation(math::fvec3(camera_position));
+			camera.set_rotation(math::fquat(camera_rotation));
+			camera.set_perspective(static_cast<float>(third_person_camera_vfov), camera.get_aspect_ratio(), camera.get_clip_near(), camera.get_clip_far());
+		}
+	);
+}
+
+void treadmill_experiment_state::load_camera_preset(std::uint8_t index)
+{
+	if (!camera_presets[index])
+	{
+		return;
+	}
+	
+	const auto& preset = *camera_presets[index];
+	
+	third_person_camera_yaw = preset.yaw;
+	third_person_camera_pitch = preset.pitch;
+	third_person_camera_focal_point = preset.focal_point;
+	zoom_third_person_camera(preset.zoom - third_person_camera_zoom);
+	
+	third_person_camera_yaw_rotation = math::angle_axis(third_person_camera_yaw, {0.0, 1.0, 0.0});
+	third_person_camera_pitch_rotation = math::angle_axis(third_person_camera_pitch, {-1.0, 0.0, 0.0});
+	third_person_camera_orientation = math::normalize(third_person_camera_yaw_rotation * third_person_camera_pitch_rotation);
+	
+	update_third_person_camera();
+}
+
+void treadmill_experiment_state::save_camera_preset(std::uint8_t index)
+{
+	camera_presets[index] =
+	{
+		third_person_camera_yaw,
+		third_person_camera_pitch,
+		third_person_camera_focal_point,
+		third_person_camera_zoom
+	};
+}
+
+void treadmill_experiment_state::load_or_save_camera_preset(std::uint8_t index)
+{
+	if (ctx.save_camera_action.is_active())
+	{
+		save_camera_preset(index);
+	}
+	else
+	{
+		load_camera_preset(index);
+	}
+}
+
+geom::ray<float, 3> treadmill_experiment_state::get_mouse_ray(const math::vec2<std::int32_t>& mouse_position) const
+{
+	// Get window viewport size
+	const auto& viewport_size = ctx.window->get_viewport_size();
+	
+	// Transform mouse coordinates from window space to NDC space
+	const math::fvec2 mouse_ndc =
+	{
+		static_cast<float>(mouse_position.x()) / static_cast<float>(viewport_size.x() - 1) * 2.0f - 1.0f,
+		(1.0f - static_cast<float>(mouse_position.y()) / static_cast<float>(viewport_size.y() - 1)) * 2.0f - 1.0f
+	};
+	
+	const auto& scene_component = ctx.entity_registry->get<::scene_component>(third_person_camera_rig_eid);
+	const auto& camera = static_cast<const scene::camera&>(*scene_component.object);
+	
+	return camera.pick(mouse_ndc);
+}
+
+void treadmill_experiment_state::setup_controls()
+{
+	// Enable/toggle mouse look
+	action_subscriptions.emplace_back
+	(
+		ctx.mouse_look_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event)
+			{
+				mouse_look = ctx.toggle_mouse_look ? !mouse_look : true;
+				
+				//ctx.input_manager->set_cursor_visible(!mouse_look);
+				ctx.input_manager->set_relative_mouse_mode(mouse_look || mouse_grip || mouse_zoom);
+			}
+		)
+	);
+	
+	// Disable mouse look
+	action_subscriptions.emplace_back
+	(
+		ctx.mouse_look_action.get_deactivated_channel().subscribe
+		(
+			[&](const auto& event)
+			{
+				if (!ctx.toggle_mouse_look && mouse_look)
+				{
+					mouse_look = false;
+					//ctx.input_manager->set_cursor_visible(true);
+					ctx.input_manager->set_relative_mouse_mode(mouse_look || mouse_grip || mouse_zoom);
+				}
+			}
+		)
+	);
+	
+	// Enable/toggle mouse grip
+	action_subscriptions.emplace_back
+	(
+		ctx.mouse_grip_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event)
+			{
+				mouse_grip = ctx.toggle_mouse_grip ? !mouse_grip : true;
+				
+				if (mouse_grip)
+				{
+					const auto& mouse_position = (*ctx.input_manager->get_mice().begin())->get_position();
+					
+					// Cast ray to plane
+					const auto mouse_ray = get_mouse_ray(mouse_position);
+					const auto intersection = geom::intersection(mouse_ray, mouse_grip_plane);
+					if (intersection)
+					{
+						mouse_grip_point = mouse_ray.origin + mouse_ray.direction * (*intersection);
+					}
+				}
+				
+				ctx.input_manager->set_relative_mouse_mode(mouse_look || mouse_grip || mouse_zoom);
+				
+				// BVH picking test
+				const auto& mouse_position = (*ctx.input_manager->get_mice().begin())->get_position();
+				const auto mouse_ray = get_mouse_ray(mouse_position);
+				
+				debug::log::info("pick:");
+				float nearest_hit = std::numeric_limits<float>::infinity();
+				bool hit = false;
+				std::uint32_t hit_index;
+				const auto& vertex_positions = navmesh->vertices().attributes().at<math::fvec3>("position");
+				std::size_t test_count = 0;
+				
+				
+				int box_test_passed = 0;
+				navmesh_bvh->visit
+				(
+					mouse_ray,
+					[&](std::uint32_t index)
+					{
+						++box_test_passed;
+						
+						geom::brep_face* face = navmesh->faces()[index];
+						auto loop = face->loops().begin();
+						const auto& a = vertex_positions[loop->vertex()->index()];
+						const auto& b = vertex_positions[(++loop)->vertex()->index()];
+						const auto& c = vertex_positions[(++loop)->vertex()->index()];
+						
+						if (auto intersection = geom::intersection(mouse_ray, a, b, c))
+						{
+							++test_count;
+							float t = std::get<0>(*intersection);
+							if (t < nearest_hit)
+							{
+								hit = true;
+								nearest_hit = t;
+								hit_index = index;
+							}
+						}
+						
+						
+					}
+				);
+				
+				debug::log::info("box tests passed: {}", box_test_passed);
+				
+				if (hit)
+				{
+					const auto& navmesh_face_normals = navmesh->faces().attributes().at<math::fvec3>("normal");
+					
+					navmesh_agent_face = navmesh->faces()[hit_index];
+					navmesh_agent_position = mouse_ray.extrapolate(nearest_hit);
+					navmesh_agent_normal = navmesh_face_normals[hit_index];
+					
+					
+					const auto& hit_normal = navmesh_face_normals[hit_index];
+					
+					const float standing_height = worker_phenome.legs->standing_height * worker_phenome.body_size->mean_mesosoma_length;
+					const math::fvec3 translation = navmesh_agent_position + hit_normal * standing_height;
+					
+					const math::fquat rotation = math::rotation(math::fvec3{0, 1, 0}, hit_normal);
+					
+					ctx.entity_registry->patch<scene_component>
+					(
+						worker_eid,
+						[&](auto& component)
+						{
+							component.object->set_translation(translation);
+							component.object->set_rotation(rotation);
+						}
+					);
+					
+					debug::log::info("hit! test count: {}", test_count);
+				}
+				else
+				{
+					debug::log::info("no hit");
+				}
+			}
+		)
+	);
+	
+	// Disable mouse grip
+	action_subscriptions.emplace_back
+	(
+		ctx.mouse_grip_action.get_deactivated_channel().subscribe
+		(
+			[&](const auto& event)
+			{
+				mouse_grip = false;
+				ctx.input_manager->set_relative_mouse_mode(mouse_look || mouse_grip || mouse_zoom);
+			}
+		)
+	);
+	
+	
+	// Enable/toggle mouse zoom
+	action_subscriptions.emplace_back
+	(
+		ctx.mouse_zoom_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event)
+			{
+				mouse_zoom = ctx.toggle_mouse_zoom ? !mouse_zoom : true;
+				ctx.input_manager->set_relative_mouse_mode(mouse_look || mouse_grip || mouse_zoom);
+			}
+		)
+	);
+	
+	// Disable mouse zoom
+	action_subscriptions.emplace_back
+	(
+		ctx.mouse_zoom_action.get_deactivated_channel().subscribe
+		(
+			[&](const auto& event)
+			{
+				mouse_zoom = false;
+				ctx.input_manager->set_relative_mouse_mode(mouse_look || mouse_grip || mouse_zoom);
+			}
+		)
+	);
+	
+	// Mouse look
+	mouse_motion_subscription = ctx.input_manager->get_event_dispatcher().subscribe<input::mouse_moved_event>
+	(
+		std::bind_front(&treadmill_experiment_state::handle_mouse_motion, this)
+	);
+	
+	// Move forward
+	action_subscriptions.emplace_back
+	(
+		ctx.move_forward_action.get_active_channel().subscribe
+		(
+			[&](const auto& event)
+			{
+				// translate_third_person_camera({0.0, 0.0, -1.0}, event.input_value / ctx.fixed_update_rate);
+				// update_third_person_camera();
+				
+				if (navmesh_agent_face)
+				{
+					const auto& scene_component = ctx.entity_registry->get<::scene_component>(worker_eid);
+					geom::ray<float, 3> ray;
+					ray.origin = navmesh_agent_position;
+					ray.direction = scene_component.object->get_rotation() * math::fvec3{0, 0, 1};
+					
+					auto traversal = ai::traverse_navmesh(*navmesh, navmesh_agent_face, ray, event.input_value / ctx.fixed_update_rate * 0.75f);
+					navmesh_agent_face = traversal.face;
+					navmesh_agent_position = traversal.cartesian;
+					
+					const float standing_height = worker_phenome.legs->standing_height * worker_phenome.body_size->mean_mesosoma_length;
+					
+					// Interpolate vertex normals
+					const auto& vertex_positions = navmesh->vertices().attributes().at<math::fvec3>("position");
+					const auto& vertex_normals = navmesh->vertices().attributes().at<math::fvec3>("normal");
+					
+					auto loop = traversal.face->loops().begin();
+					const auto& na = vertex_normals[loop->vertex()->index()];
+					const auto& nb = vertex_normals[(++loop)->vertex()->index()];
+					const auto& nc = vertex_normals[(++loop)->vertex()->index()];
+					
+					const auto& uvw = traversal.barycentric;
+					const auto smooth_normal = math::normalize(na * uvw.x() + nb * uvw.y() + nc * uvw.z());
+					
+					navmesh_agent_normal = smooth_normal;
+					
+					
+					ctx.entity_registry->patch<::scene_component>
+					(
+						worker_eid,
+						[&](auto& component)
+						{
+							
+							auto transform = component.object->get_transform();
+							transform.translation = navmesh_agent_position + smooth_normal * standing_height;
+							
+							// Find rotation from local up vector to vertex-interpolated surface normal
+							auto rotation = math::rotation(transform.rotation * math::fvec3{0, 1, 0}, navmesh_agent_normal);
+							
+							// Rotate object
+							transform.rotation = math::normalize(rotation * transform.rotation);
+							
+							component.object->set_transform(transform);
+							
+							third_person_camera_focal_point = math::dvec3(transform.translation);
+							update_third_person_camera();
+						}
+					);
+				}
+			}
+		)
+	);
+	
+	// Move back
+	action_subscriptions.emplace_back
+	(
+		ctx.move_back_action.get_active_channel().subscribe
+		(
+			[&](const auto& event)
+			{
+				// translate_third_person_camera({0.0, 0.0, 1.0}, event.input_value / ctx.fixed_update_rate);
+				// update_third_person_camera();
+			}
+		)
+	);
+	
+	// Move left
+	action_subscriptions.emplace_back
+	(
+		ctx.move_left_action.get_active_channel().subscribe
+		(
+			[&](const auto& event)
+			{
+				// translate_third_person_camera({-1.0, 0.0, 0.0}, event.input_value / ctx.fixed_update_rate);
+				// update_third_person_camera();
+				
+				ctx.entity_registry->patch<::scene_component>
+				(
+					worker_eid,
+					[&](auto& component)
+					{
+						auto rotation = math::angle_axis(math::radians(30.0f) * event.input_value / ctx.fixed_update_rate, navmesh_agent_normal);
+						component.object->set_rotation(math::normalize(rotation * component.object->get_rotation()));
+					}
+				);
+				
+				update_third_person_camera();
+			}
+		)
+	);
+	
+	// Move right
+	action_subscriptions.emplace_back
+	(
+		ctx.move_right_action.get_active_channel().subscribe
+		(
+			[&](const auto& event)
+			{
+				// translate_third_person_camera({1.0, 0.0, 0.0}, event.input_value / ctx.fixed_update_rate);
+				// update_third_person_camera();
+				
+				ctx.entity_registry->patch<::scene_component>
+				(
+					worker_eid,
+					[&](auto& component)
+					{
+						auto rotation = math::angle_axis(math::radians(-30.0f) * event.input_value / ctx.fixed_update_rate, navmesh_agent_normal);
+						component.object->set_rotation(math::normalize(rotation * component.object->get_rotation()));
+					}
+				);
+				update_third_person_camera();
+			}
+		)
+	);
+	
+	// Zoom in
+	action_subscriptions.emplace_back
+	(
+		ctx.move_up_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event)
+			{
+				zoom_third_person_camera(1.0 / static_cast<double>(third_person_camera_zoom_step_count));
+				update_third_person_camera();
+			}
+		)
+	);
+	
+	// Zoom out
+	action_subscriptions.emplace_back
+	(
+		ctx.move_down_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event)
+			{
+				zoom_third_person_camera(-1.0 / static_cast<double>(third_person_camera_zoom_step_count));
+				update_third_person_camera();
+			}
+		)
+	);
+	
+	// Focus
+	action_subscriptions.emplace_back
+	(
+		ctx.focus_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event)
+			{
+				
+			}
+		)
+	);
+	action_subscriptions.emplace_back
+	(
+		ctx.focus_action.get_deactivated_channel().subscribe
+		(
+			[&](const auto& event)
+			{
+				
+			}
+		)
+	);
+	
+	// Camera presets
+	action_subscriptions.emplace_back
+	(
+		ctx.camera_1_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event) {load_or_save_camera_preset(0);}
+		)
+	);
+	action_subscriptions.emplace_back
+	(
+		ctx.camera_2_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event) {load_or_save_camera_preset(1);}
+		)
+	);
+	action_subscriptions.emplace_back
+	(
+		ctx.camera_3_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event) {load_or_save_camera_preset(2);}
+		)
+	);
+	action_subscriptions.emplace_back
+	(
+		ctx.camera_4_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event) {load_or_save_camera_preset(3);}
+		)
+	);
+	action_subscriptions.emplace_back
+	(
+		ctx.camera_5_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event) {load_or_save_camera_preset(4);}
+		)
+	);
+	action_subscriptions.emplace_back
+	(
+		ctx.camera_6_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event) {load_or_save_camera_preset(5);}
+		)
+	);
+	action_subscriptions.emplace_back
+	(
+		ctx.camera_7_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event) {load_or_save_camera_preset(6);}
+		)
+	);
+	action_subscriptions.emplace_back
+	(
+		ctx.camera_8_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event) {load_or_save_camera_preset(7);}
+		)
+	);
+	action_subscriptions.emplace_back
+	(
+		ctx.camera_9_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event) {load_or_save_camera_preset(8);}
+		)
+	);
+	action_subscriptions.emplace_back
+	(
+		ctx.camera_10_action.get_activated_channel().subscribe
+		(
+			[&](const auto& event) {load_or_save_camera_preset(9);}
+		)
+	);
+}

src/game/states/experiments/treadmill-experiment-state.hpp

@@ -0,0 +1,127 @@
+/*
+ * Copyright (C) 2023  Christopher J. Howard
+ *
+ * This file is part of Antkeeper source code.
+ *
+ * Antkeeper source code is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Antkeeper source code is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Antkeeper source code.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef ANTKEEPER_TREADMILL_EXPERIMENT_STATE_HPP
+#define ANTKEEPER_TREADMILL_EXPERIMENT_STATE_HPP
+
+#include "game/states/game-state.hpp"
+#include <engine/entity/id.hpp>
+#include <engine/math/vector.hpp>
+#include <engine/render/model.hpp>
+#include <engine/event/subscription.hpp>
+#include <engine/input/mouse-events.hpp>
+#include <engine/geom/primitives/ray.hpp>
+#include <engine/geom/primitives/plane.hpp>
+#include <engine/math/angles.hpp>
+#include <engine/scene/light-probe.hpp>
+#include <engine/geom/bvh/bvh.hpp>
+#include <engine/geom/brep/brep-mesh.hpp>
+#include "game/ant/ant-phenome.hpp"
+
+class treadmill_experiment_state: public game_state
+{
+public:
+	explicit treadmill_experiment_state(::game& ctx);
+	~treadmill_experiment_state() override;
+	
+private:
+	void create_third_person_camera_rig();
+	void destroy_third_person_camera_rig();
+	
+	void set_third_person_camera_zoom(double zoom);
+	void set_third_person_camera_rotation(double yaw, double pitch);
+	
+	void zoom_third_person_camera(double zoom);
+	void translate_third_person_camera(const math::dvec3& direction, double magnitude);
+	void rotate_third_person_camera(const input::mouse_moved_event& event);
+	
+	void handle_mouse_motion(const input::mouse_moved_event& event);
+	
+	void update_third_person_camera();
+	void load_camera_preset(std::uint8_t index);
+	void save_camera_preset(std::uint8_t index);
+	void load_or_save_camera_preset(std::uint8_t index);
+	
+	[[nodiscard]] geom::ray<float, 3> get_mouse_ray(const math::vec2<std::int32_t>& mouse_position) const;
+	
+	void setup_controls();
+	
+	std::vector<std::shared_ptr<::event::subscription>> action_subscriptions;
+	std::shared_ptr<::event::subscription> mouse_motion_subscription;
+	
+	bool mouse_look{false};
+	bool mouse_grip{false};
+	bool mouse_zoom{false};
+	geom::plane<float> mouse_grip_plane{{0.0, 1.0, 0.0}, 0.0};
+	math::fvec3 mouse_grip_point{};
+	
+	bool moving{false};
+	
+	entity::id third_person_camera_rig_eid{entt::null};
+	double third_person_camera_yaw{0.0};
+	double third_person_camera_pitch{math::radians(45.0)};
+	math::dvec3 third_person_camera_focal_point{0.0, 0.0, 0.0};
+	double third_person_camera_zoom{0.25};
+	std::uint32_t third_person_camera_zoom_step_count{6};
+	
+	double third_person_camera_near_focal_plane_height{2.0f};
+	double third_person_camera_far_focal_plane_height{50.0f};
+	
+	double third_person_camera_near_hfov{math::radians(90.0)};
+	double third_person_camera_far_hfov{math::radians(45.0)};
+	
+	/// In focal plane heights per second.
+	double third_person_camera_speed{1.0f};
+	
+	double third_person_camera_hfov{};
+	double third_person_camera_vfov{};
+	double third_person_camera_focal_plane_width{};
+	double third_person_camera_focal_plane_height{};
+	double third_person_camera_focal_distance{};
+	math::dquat third_person_camera_yaw_rotation{math::dquat::identity()};
+	math::dquat third_person_camera_pitch_rotation{math::dquat::identity()};
+	math::dquat third_person_camera_orientation{math::dquat::identity()};
+	
+	struct camera_preset
+	{
+		double yaw{};
+		double pitch{};
+		math::dvec3 focal_point{};
+		double zoom{0.25};
+	};
+	
+	std::vector<std::optional<camera_preset>> camera_presets{10};
+	
+	std::shared_ptr<render::matvar_fvec3> light_rectangle_emissive;
+	std::shared_ptr<scene::light_probe> light_probe;
+	
+	std::shared_ptr<geom::brep_mesh> navmesh;
+	std::unique_ptr<geom::bvh> navmesh_bvh;
+	entity::id larva_eid;
+	entity::id worker_eid;
+	
+	ant_phenome worker_phenome;
+	geom::brep_face* navmesh_agent_face{};
+	math::fvec3 navmesh_agent_position{};
+	math::fvec3 navmesh_agent_normal{};
+	
+	std::shared_ptr<scene::light_probe> sky_probe;
+};
+
+#endif // ANTKEEPER_TREADMILL_EXPERIMENT_STATE_HPP

src/game/states/main-menu-state.cpp

@@ -34,6 +34,7 @@
 #include "game/states/nest-selection-state.hpp"
 #include "game/states/nest-view-state.hpp"
 #include "game/states/options-menu-state.hpp"
+#include "game/states/experiments/treadmill-experiment-state.hpp"
 #include "game/strings.hpp"
 #include "game/world.hpp"
 #include <engine/math/vector.hpp>
@@ -130,7 +131,8 @@ main_menu_state::main_menu_state(::game& ctx, bool fade_in):
 					// ctx.state_machine.emplace(std::make_unique<nuptial_flight_state>(ctx));
 					// ctx.state_machine.emplace(std::make_unique<collection_menu_state>(ctx));
 					// ctx.state_machine.emplace(std::make_unique<nest_selection_state>(ctx));
-					ctx.state_machine.emplace(std::make_unique<nest_view_state>(ctx));
+					// ctx.state_machine.emplace(std::make_unique<nest_view_state>(ctx));
+					ctx.state_machine.emplace(std::make_unique<treadmill_experiment_state>(ctx));
 				}
 			);
 		};

src/game/states/nest-view-state.cpp

@@ -82,6 +82,8 @@
 #include <engine/animation/ease.hpp>
 #include <engine/color/color.hpp>
 #include <engine/geom/brep/brep-operations.hpp>
+#include <engine/geom/coordinates.hpp>
+#include <engine/ai/navmesh.hpp>
 
 nest_view_state::nest_view_state(::game& ctx):
 	game_state(ctx)
@@ -106,7 +108,7 @@ nest_view_state::nest_view_state(::game& ctx):
 	debug::log::trace("Generated genome");
 	
 	debug::log::trace("Building worker phenome...");
-	ant_phenome worker_phenome = ant_phenome(*genome, ant_caste_type::worker);
+	worker_phenome = ant_phenome(*genome, ant_caste_type::worker);
 	debug::log::trace("Built worker phenome...");
 	
 	debug::log::trace("Generating worker model...");
@@ -164,19 +166,12 @@ nest_view_state::nest_view_state(::game& ctx):
 		}
 	);
 	
-	// Create petri dish
-	auto petri_dish_eid = ctx.entity_registry->create();
-	scene_component petri_dish_scene_component;
-	petri_dish_scene_component.object = std::make_shared<scene::static_mesh>(ctx.resource_manager->load<render::model>("square-petri-dish.mdl"));
-	petri_dish_scene_component.layer_mask = 2;
-	ctx.entity_registry->emplace<scene_component>(petri_dish_eid, std::move(petri_dish_scene_component));
-	
-	// Create petri dish cement
-	auto petri_dish_cement_eid = ctx.entity_registry->create();
-	scene_component petri_dish_cement_scene_component;
-	petri_dish_cement_scene_component.object = std::make_shared<scene::static_mesh>(ctx.resource_manager->load<render::model>("square-petri-dish-cement.mdl"));
-	petri_dish_cement_scene_component.layer_mask = 2;
-	ctx.entity_registry->emplace<scene_component>(petri_dish_cement_eid, std::move(petri_dish_cement_scene_component));
+	// Create treadmill
+	auto treadmill_eid = ctx.entity_registry->create();
+	scene_component treadmill_scene_component;
+	treadmill_scene_component.object = std::make_shared<scene::static_mesh>(ctx.resource_manager->load<render::model>("treadmill-5cm.mdl"));
+	treadmill_scene_component.layer_mask = 2;
+	ctx.entity_registry->emplace<scene_component>(treadmill_eid, std::move(treadmill_scene_component));
 	
 	// Create worker
 	auto worker_skeletal_mesh = std::make_unique<scene::skeletal_mesh>(worker_model);
@@ -280,10 +275,11 @@ nest_view_state::nest_view_state(::game& ctx):
 	ctx.frame_scheduler.refresh();
 	
 	// Load navmesh
-	navmesh = ctx.resource_manager->load<geom::brep_mesh>("square-petri-dish-navmesh.msh");
+	navmesh = ctx.resource_manager->load<geom::brep_mesh>("treadmill-5cm.msh");
 	
 	// Generate navmesh attributes
 	geom::generate_face_normals(*navmesh);
+	geom::generate_vertex_normals(*navmesh);
 	
 	// Build navmesh BVH
 	debug::log::info("building bvh");
@@ -610,14 +606,21 @@ void nest_view_state::setup_controls()
 				{
 					const auto& navmesh_face_normals = navmesh->faces().attributes().at<math::fvec3>("normal");
 					
+					navmesh_agent_face = navmesh->faces()[hit_index];
+					navmesh_agent_position = mouse_ray.extrapolate(nearest_hit);
+					navmesh_agent_normal = navmesh_face_normals[hit_index];
+					
+					
 					const auto& hit_normal = navmesh_face_normals[hit_index];
 					
-					const math::fvec3 translation = mouse_ray.origin + mouse_ray.direction * nearest_hit;
+					const float standing_height = worker_phenome.legs->standing_height * worker_phenome.body_size->mean_mesosoma_length;
+					const math::fvec3 translation = navmesh_agent_position + hit_normal * standing_height;
+					
 					const math::fquat rotation = math::rotation(math::fvec3{0, 1, 0}, hit_normal);
 					
 					ctx.entity_registry->patch<scene_component>
 					(
-						larva_eid,
+						worker_eid,
 						[&](auto& component)
 						{
 							component.object->set_translation(translation);
@@ -688,8 +691,56 @@ void nest_view_state::setup_controls()
 		(
 			[&](const auto& event)
 			{
-				translate_third_person_camera({0.0, 0.0, -1.0}, event.input_value / ctx.fixed_update_rate);
-				update_third_person_camera();
+				// translate_third_person_camera({0.0, 0.0, -1.0}, event.input_value / ctx.fixed_update_rate);
+				// update_third_person_camera();
+				
+				if (navmesh_agent_face)
+				{
+					const auto& scene_component = ctx.entity_registry->get<::scene_component>(worker_eid);
+					geom::ray<float, 3> ray;
+					ray.origin = navmesh_agent_position;
+					ray.direction = scene_component.object->get_rotation() * math::fvec3{0, 0, 1};
+					
+					auto traversal = ai::traverse_navmesh(*navmesh, navmesh_agent_face, ray, event.input_value / ctx.fixed_update_rate);
+					navmesh_agent_face = traversal.face;
+					navmesh_agent_position = traversal.cartesian;
+					
+					const float standing_height = worker_phenome.legs->standing_height * worker_phenome.body_size->mean_mesosoma_length;
+					
+					// Interpolate vertex normals
+					const auto& vertex_positions = navmesh->vertices().attributes().at<math::fvec3>("position");
+					const auto& vertex_normals = navmesh->vertices().attributes().at<math::fvec3>("normal");
+					
+					auto loop = traversal.face->loops().begin();
+					const auto& na = vertex_normals[loop->vertex()->index()];
+					const auto& nb = vertex_normals[(++loop)->vertex()->index()];
+					const auto& nc = vertex_normals[(++loop)->vertex()->index()];
+					
+					const auto& uvw = traversal.barycentric;
+					const auto smooth_normal = math::normalize(na * uvw.x() + nb * uvw.y() + nc * uvw.z());
+					
+					navmesh_agent_normal = smooth_normal;
+					
+					
+					ctx.entity_registry->patch<::scene_component>
+					(
+						worker_eid,
+						[&](auto& component)
+						{
+							
+							auto transform = component.object->get_transform();
+							transform.translation = navmesh_agent_position + smooth_normal * standing_height;
+							
+							// Find rotation from local up vector to vertex-interpolated surface normal
+							auto rotation = math::rotation(transform.rotation * math::fvec3{0, 1, 0}, navmesh_agent_normal);
+							
+							// Rotate object
+							transform.rotation = math::normalize(rotation * transform.rotation);
+							
+							component.object->set_transform(transform);
+						}
+					);
+				}
 			}
 		)
 	);
@@ -714,8 +765,18 @@ void nest_view_state::setup_controls()
 		(
 			[&](const auto& event)
 			{
-				translate_third_person_camera({-1.0, 0.0, 0.0}, event.input_value / ctx.fixed_update_rate);
-				update_third_person_camera();
+				// translate_third_person_camera({-1.0, 0.0, 0.0}, event.input_value / ctx.fixed_update_rate);
+				// update_third_person_camera();
+				
+				ctx.entity_registry->patch<::scene_component>
+				(
+					worker_eid,
+					[&](auto& component)
+					{
+						auto rotation = math::angle_axis(math::radians(30.0f) * event.input_value / ctx.fixed_update_rate, navmesh_agent_normal);
+						component.object->set_rotation(math::normalize(rotation * component.object->get_rotation()));
+					}
+				);
 			}
 		)
 	);
@@ -727,8 +788,18 @@ void nest_view_state::setup_controls()
 		(
 			[&](const auto& event)
 			{
-				translate_third_person_camera({1.0, 0.0, 0.0}, event.input_value / ctx.fixed_update_rate);
-				update_third_person_camera();
+				// translate_third_person_camera({1.0, 0.0, 0.0}, event.input_value / ctx.fixed_update_rate);
+				// update_third_person_camera();
+				
+				ctx.entity_registry->patch<::scene_component>
+				(
+					worker_eid,
+					[&](auto& component)
+					{
+						auto rotation = math::angle_axis(math::radians(-30.0f) * event.input_value / ctx.fixed_update_rate, navmesh_agent_normal);
+						component.object->set_rotation(math::normalize(rotation * component.object->get_rotation()));
+					}
+				);
 			}
 		)
 	);

src/game/states/nest-view-state.hpp

@@ -32,6 +32,7 @@
 #include <engine/scene/light-probe.hpp>
 #include <engine/geom/bvh/bvh.hpp>
 #include <engine/geom/brep/brep-mesh.hpp>
+#include "game/ant/ant-phenome.hpp"
 
 class nest_view_state: public game_state
 {
@@ -114,6 +115,11 @@ private:
 	std::unique_ptr<geom::bvh> navmesh_bvh;
 	entity::id larva_eid;
 	entity::id worker_eid;
+	
+	ant_phenome worker_phenome;
+	geom::brep_face* navmesh_agent_face{};
+	math::fvec3 navmesh_agent_position{};
+	math::fvec3 navmesh_agent_normal{};
 };
 
 #endif // ANTKEEPER_NEST_VIEW_STATE_HPP

src/game/states/splash-state.cpp

@@ -26,7 +26,6 @@
 #include <engine/animation/screen-transition.hpp>
 #include <engine/debug/log.hpp>
 #include <engine/math/vector.hpp>
-#include <engine/math/linear-algebra.hpp>
 #include <engine/render/material-flags.hpp>
 #include <engine/render/passes/clear-pass.hpp>
 #include <engine/resources/resource-manager.hpp>

src/game/systems/astronomy-system.cpp

@@ -23,7 +23,6 @@
 #include "game/components/transform-component.hpp"
 #include "game/components/diffuse-reflector-component.hpp"
 #include <engine/geom/intersection.hpp>
-#include <engine/geom/cartesian.hpp>
 #include <engine/geom/primitives/sphere.hpp>
 #include <engine/color/color.hpp>
 #include <engine/physics/orbit/orbit.hpp>
@@ -33,7 +32,6 @@
 #include <engine/physics/light/luminosity.hpp>
 #include <engine/physics/light/refraction.hpp>
 #include <engine/physics/gas/atmosphere.hpp>
-#include <engine/geom/cartesian.hpp>
 #include <engine/astro/apparent-size.hpp>
 #include <engine/geom/solid-angle.hpp>
 #include <engine/math/polynomial.hpp>
@@ -52,7 +50,7 @@ astronomy_system::astronomy_system(entity::registry& registry):
 	starlight_illuminance{0, 0, 0}
 {
 	// Construct ENU to EUS transformation
-	enu_to_eus = math::transformation::se3<double>
+	enu_to_eus = math::se3<double>
 	{
 		{0, 0, 0},
 		math::dquat::rotate_x(-math::half_pi<double>)
@@ -535,7 +533,7 @@ void astronomy_system::update_icrf_to_eus(const ::celestial_body_component& body
 	const double body_prime_meridian = math::polynomial::horner(body.prime_meridian.begin(), body.prime_meridian.end(), time_days);
 	
 	// Construct ICRF frame to BCBF transformation
-	math::transformation::se3<double> icrf_to_bcbf = physics::orbit::frame::bci::to_bcbf
+	math::se3<double> icrf_to_bcbf = physics::orbit::frame::bci::to_bcbf
 	(
 		body_pole_ra,
 		body_pole_dec,
@@ -552,7 +550,7 @@ void astronomy_system::update_icrf_to_eus(const ::celestial_body_component& body
 		// Upload topocentric frame to sky pass
 		sky_pass->set_icrf_to_eus
 		(
-			math::transformation::se3<float>
+			math::se3<float>
 			{
 				math::fvec3(icrf_to_eus.t),
 				math::fquat(icrf_to_eus.r)

src/game/systems/astronomy-system.hpp

@@ -146,13 +146,13 @@ private:
 	entity::id reference_body_eid;
 	
 	/// ENU to EUS transformation.
-	math::transformation::se3<double> enu_to_eus;
+	math::se3<double> enu_to_eus;
 	
 	/// BCBF to EUS transformation.
-	math::transformation::se3<double> bcbf_to_eus;
+	math::se3<double> bcbf_to_eus;
 	
 	/// ICRF to EUS tranformation.
-	math::transformation::se3<double> icrf_to_eus;
+	math::se3<double> icrf_to_eus;
 	
 	scene::directional_light* sun_light;
 	scene::directional_light* moon_light;

src/game/world.cpp

@@ -35,7 +35,6 @@
 #include "game/systems/orbit-system.hpp"
 #include "game/systems/terrain-system.hpp"
 #include <engine/geom/solid-angle.hpp>
-#include <engine/geom/spherical.hpp>
 #include <engine/gl/drawing-mode.hpp>
 #include <engine/gl/texture-filter.hpp>
 #include <engine/gl/texture-wrapping.hpp>