Add capsule collision

src/engine/geom/closest-point.hpp

@@ -0,0 +1,262 @@
+/*
+ * 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_POINT_HPP
+#define ANTKEEPER_GEOM_CLOSEST_POINT_HPP
+
+#include <engine/geom/primitives/hypercapsule.hpp>
+#include <engine/geom/primitives/hyperplane.hpp>
+#include <engine/geom/primitives/hyperrectangle.hpp>
+#include <engine/geom/primitives/hypersphere.hpp>
+#include <engine/geom/primitives/line-segment.hpp>
+#include <engine/geom/primitives/point.hpp>
+#include <engine/geom/primitives/ray.hpp>
+#include <algorithm>
+#include <cmath>
+#include <tuple>
+
+namespace geom {
+
+/**
+ * Calculates the closest point on a ray to a point.
+ *
+ * @tparam T Real type.
+ * @tparam N Number of dimensions.
+ *
+ * @param a Ray a.
+ * @param b Point b.
+ *
+ * @return Closest point on ray a to point b.
+ */
+template <class T, std::size_t N>
+[[nodiscard]] point<T, N> closest_point(const ray<T, N>& a, const point<T, N>& b) noexcept
+{
+	return a.extrapolate(std::max<T>(T{0}, math::dot(b - a.origin, a.direction)));
+}
+
+/**
+ * Calculates the closest point on a line segment to a point.
+ *
+ * @tparam T Real type.
+ * @tparam N Number of dimensions.
+ *
+ * @param ab Line segment ab.
+ * @param c Point c.
+ *
+ * @return Closest point on line segment ab to point c.
+ */
+template <class T, std::size_t N>
+[[nodiscard]] point<T, N> closest_point(const line_segment<T, N>& ab, const point<T, N>& c) noexcept
+{
+	const auto direction_ab = ab.b - ab.a;
+	
+	const auto distance_ab = math::dot(c - ab.a, direction_ab);
+	if (distance_ab <= T{0})
+	{
+		return ab.a;
+	}
+	else
+	{
+		const auto sqr_length_ab = math::sqr_length(direction_ab);
+		if (distance_ab >= sqr_length_ab)
+		{
+			return ab.b;
+		}
+		else
+		{
+			return ab.a + direction_ab * (distance_ab / sqr_length_ab);
+		}
+	}
+}
+
+/**
+ * Calculates the closest points on two line segments.
+ *
+ * @tparam T Real type.
+ * @tparam N Number of dimensions.
+ *
+ * @param ab Line segment ab.
+ * @param cd Line segment cd.
+ *
+ * @return Tuple containing the closest point on segment ab to segment cd, followed by the closest point on segment cd to segment ab.
+ */
+/// @{
+template <class T, std::size_t N>
+[[nodiscard]] std::tuple<point<T, N>, point<T, N>> closest_point(const line_segment<T, N>& ab, const line_segment<T, N>& cd) noexcept
+{
+	const auto direction_ab = ab.b - ab.a;
+	const auto direction_cd = cd.b - cd.a;
+	const auto direction_ca = ab.a - cd.a;
+	
+	const auto sqr_length_ab = math::sqr_length(direction_ab);
+	const auto sqr_length_cd = math::sqr_length(direction_cd);
+	const auto cd_dot_ca = math::dot(direction_cd, direction_ca);
+	
+	if (sqr_length_ab <= T{0})
+	{
+		if (sqr_length_cd <= T{0})
+		{
+			// Both segments are points
+			return {ab.a, cd.a};
+		}
+		else
+		{
+			// Segment ab is a point
+			return
+			{
+				ab.a,
+				cd.a + direction_cd * std::min<T>(std::max<T>(cd_dot_ca / sqr_length_cd, T{0}), T{1})
+			};
+		}
+	}
+	else
+	{
+		const auto ab_dot_ca = math::dot(direction_ab, direction_ca);
+		
+		if (sqr_length_cd <= T{0})
+		{
+			// Segment cd is a point
+			return
+			{
+				ab.a + direction_ab * std::min<T>(std::max<T>(-ab_dot_ca / sqr_length_ab, T{0}), T{1}),
+				cd.a
+			};
+		}
+		else
+		{
+			const auto ab_dot_cd = math::dot(direction_ab, direction_cd);
+			
+			const auto den = sqr_length_ab * sqr_length_cd - ab_dot_cd * ab_dot_cd;
+			
+			auto distance_ab = (den) ? std::min<T>(std::max<T>((ab_dot_cd * cd_dot_ca - ab_dot_ca * sqr_length_cd) / den, T{0}), T{1}) : T{0};
+			auto distance_cd = (ab_dot_cd * distance_ab + cd_dot_ca) / sqr_length_cd;
+			
+			if (distance_cd < T{0})
+			{
+				return
+				{
+					ab.a + direction_ab * std::min<T>(std::max<T>(-ab_dot_ca / sqr_length_ab, T{0}), T{1}),
+					cd.a
+				};
+			}
+			else if (distance_cd > T{1})
+			{
+				return
+				{
+					ab.a + direction_ab * std::min<T>(std::max<T>((ab_dot_cd - ab_dot_ca) / sqr_length_ab, T{0}), T{1}),
+					cd.b
+				};
+			}
+			
+			return
+			{
+				ab.a + direction_ab * distance_ab,
+				cd.a + direction_cd * distance_cd
+			};
+		}
+	}
+}
+
+/**
+ * Calculates the closest point on a hyperplane to a point.
+ *
+ * @tparam T Real type.
+ * @tparam N Number of dimensions.
+ *
+ * @param a Hyperplane a.
+ * @param b Point b.
+ *
+ * @return Closest point on hyperplane a to point b.
+ */
+template <class T, std::size_t N>
+[[nodiscard]] point<T, N> closest_point(const hyperplane<T, N>& a, const point<T, N>& b) noexcept
+{
+	return b - a.normal * (math::dot(a.normal, b) + a.constant);
+}
+
+/**
+ * Calculates the closest point on a hypersphere to a point.
+ *
+ * @tparam T Real type.
+ * @tparam N Number of dimensions.
+ *
+ * @param a Hypersphere a.
+ * @param b Point b.
+ *
+ * @return Closest point on hypersphere a to point b.
+ */
+template <class T, std::size_t N>
+[[nodiscard]] point<T, N> closest_point(const hypersphere<T, N>& a, const point<T, N>& b)
+{
+	const auto ab = b - a.center;
+	const auto d = math::sqr_length(ab);
+	return a.center + ab * (d ? a.radius / std::sqrt(d) : d);
+}
+
+/**
+ * Calculates the closest point on a hypercapsule to a point.
+ *
+ * @tparam T Real type.
+ * @tparam N Number of dimensions.
+ *
+ * @param a Hypercapsule a.
+ * @param b Point b.
+ *
+ * @return Closest point on hypercapsule a to point b.
+ */
+template <class T, std::size_t N>
+[[nodiscard]] point<T, N> closest_point(const hypercapsule<T, N>& a, const point<T, N>& b)
+{
+	const auto c = closest_point(a.segment, b);
+	const auto cb = b - c;
+	const auto d = math::sqr_length(cb);
+	return c + cb * (d ? a.radius / std::sqrt(d) : d);
+}
+
+/**
+ * Calculates the closest point on a hyperrectangle to a point.
+ *
+ * @tparam T Real type.
+ * @tparam N Number of dimensions.
+ *
+ * @param a Hyperrectangle a.
+ * @param b Point b.
+ *
+ * @return Closest point on hyperrectangle a to point b.
+ */
+template <class T, std::size_t N>
+[[nodiscard]] point<T, N> closest_point(const hyperrectangle<T, N>& a, const point<T, N>& b) noexcept
+{
+	const auto c = a.center();
+	const auto p = b - c;
+	const auto d = math::abs(p) - a.extents();
+	const auto m = std::min<T>(math::max(d), T{0});
+	
+	point<T, N> r;
+	for (std::size_t i = 0; i < N; ++i)
+	{
+		r[i] = c[i] + std::copysign(d[i] >= m ? d[i] : T{0}, p[i]);
+	}
+	
+	return r;
+}
+
+} // namespace geom
+
+#endif // ANTKEEPER_GEOM_CLOSEST_POINT_HPP

src/engine/geom/primitives/capsule.hpp

@@ -0,0 +1,39 @@
+/*
+ * 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_PRIMITIVES_CAPSULE_HPP
+#define ANTKEEPER_GEOM_PRIMITIVES_CAPSULE_HPP
+
+#include <engine/geom/primitives/hypercapsule.hpp>
+
+namespace geom {
+namespace primitives {
+
+/**
+ * 3-dimensional capsule.
+ *
+ * @tparam T Real type.
+ */
+template <class T>
+using capsule = hypercapsule<T, 3>;
+
+} // namespace primitives
+} // namespace geom
+
+#endif // ANTKEEPER_GEOM_PRIMITIVES_CAPSULE_HPP

src/engine/geom/primitives/hypercapsule.hpp

@@ -0,0 +1,82 @@
+/*
+ * 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_PRIMITIVES_HYPERCAPSULE_HPP
+#define ANTKEEPER_GEOM_PRIMITIVES_HYPERCAPSULE_HPP
+
+#include <engine/geom/primitives/line-segment.hpp>
+#include <engine/math/vector.hpp>
+
+namespace geom {
+namespace primitives {
+
+/**
+ * *n*-dimensional capsule.
+ *
+ * @tparam T Real type.
+ * @tparam N Number of dimensions.
+ */
+template <class T, std::size_t N>
+struct hypercapsule
+{
+	/// Vector type.
+	using vector_type = math::vector<T, N>;
+	
+	/// Line segment type.
+	using segment_type = geom::line_segment<T, N>;
+	
+	/// Medial line segment.
+	segment_type segment;
+	
+	/// Radius of the hemi-hyperspheres.
+	T radius;
+	
+	/**
+	 * Tests whether a point is contained within this hypercapsule.
+	 *
+	 * @param point Point to test for containment.
+	 *
+	 * @return `true` if the point is contained within this hypercapsule, `false` otherwise.
+	 */
+	[[nodiscard]] inline constexpr bool contains(const vector_type& point) const noexcept
+	{
+		return segment.sqr_distance(point) <= radius * radius;
+	}
+	
+	/**
+	 * Calculates the signed distance from the hypercapsule to a point.
+	 *
+	 * @param point Input point.
+	 *
+	 * @return Signed distance from the hypercapsule to @p point.
+	 */
+	[[nodiscard]] inline T distance(const vector_type& point) const noexcept
+	{
+		const T d = segment.sqr_distance(point);
+		return (d ? std::sqrt(d) : d) - radius;
+	}
+};
+
+} // namespace primitives
+
+using namespace primitives;
+
+} // namespace geom
+
+#endif // ANTKEEPER_GEOM_PRIMITIVES_HYPERCAPSULE_HPP

src/engine/geom/primitives/hyperplane.hpp

@@ -79,18 +79,6 @@ struct hyperplane
 	{
 		return math::dot(normal, point) + constant;
 	}
-	
-	/**
-	 * Calculates the closest point on the hyperplane to a point.
-	 *
-	 * @param point Input point.
-	 *
-	 * @return Closest point on the hyperplane to @p point.
-	 */
-	[[nodiscard]] inline constexpr vector_type closest_point(const vector_type& point) const noexcept
-	{
-		return point - normal * distance(point);
-	}
 };
 
 } // namespace primitives

src/engine/geom/primitives/hyperrectangle.hpp

@@ -102,29 +102,6 @@ struct hyperrectangle
 		return math::length(math::max(vector_type::zero(), d)) + std::min<T>(T{0}, math::max(d));
 	}
 	
-	/**
-	 * Calculates the closest point on the hyperrectangle to a point.
-	 *
-	 * @param point Input point.
-	 *
-	 * @return Closest point on the hyperrectangle to @p point.
-	 */
-	[[nodiscard]] constexpr vector_type closest_point(const vector_type& point) const noexcept
-	{
-		const vector_type c = center();		
-		const vector_type p = point - c;
-		const vector_type d = math::abs(p) - extents();
-		const T m = std::min<T>(T{0}, math::max(d));
-		
-		vector_type r;
-		for (std::size_t i = 0; i < N; ++i)
-		{
-			r[i] = c[i] + std::copysign(d[i] >= m ? d[i] : T{0}, p[i]);
-		}
-		
-		return r;
-	}
-	
 	/**
 	 * Extends the hyperrectangle to include a point.
 	 *

src/engine/geom/primitives/hypersphere.hpp

@@ -87,18 +87,6 @@ struct hypersphere
 		return (d ? std::sqrt(d) : d) - radius;
 	}
 	
-	/**
-	 * Calculates the closest point on the hypersphere to a point.
-	 *
-	 * @param point Input point.
-	 *
-	 * @return Closest point on the hypersphere to @p point.
-	 */
-	[[nodiscard]] inline vector_type closest_point(const vector_type& point) const noexcept
-	{
-		return center + math::normalize(point - center) * radius;
-	}
-	
 	/**
 	 * Tests whether another hypersphere intersects this hypersphere.
 	 *

src/engine/geom/primitives/line-segment.hpp

@@ -43,6 +43,16 @@ struct line_segment
 	/// Second endpoint.
 	vector_type b;
 	
+	/**
+	 * Calculates the square length of the line segment.
+	 *
+	 * @return Square length of the line segment.
+	 */
+	[[nodiscard]] inline T sqr_length() const noexcept
+	{
+		return math::sqr_distance(a, b);
+	}
+	
 	/**
 	 * Calculates the length of the line segment.
 	 *
@@ -50,7 +60,48 @@ struct line_segment
 	 */
 	[[nodiscard]] inline T length() const noexcept
 	{
-		return math::distance(a, b);
+		const T d = sqr_length();
+		return d ? std::sqrt(d) : d;
+	}
+	
+	/**
+	 * Calculates the square distance from the line segment to a point.
+	 *
+	 * @param point Input point.
+	 *
+	 * @return Square distance from the line segment to @p point.
+	 */
+	[[nodiscard]] T sqr_distance(const vector_type& point) const noexcept
+	{
+		const vector_type ab = b - a;
+		const vector_type ap = point - a;
+		
+		const T t = math::dot(ap, ab);
+		if (t <= T{0})
+		{
+			return math::sqr_length(ap);
+		}
+		
+		const T ab_sqr_length = math::sqr_length(ab);
+		if (t >= ab_sqr_length)
+		{
+			return math::sqr_length(point - b);
+		}
+		
+		return math::sqr_length(ap) - (t * t) / ab_sqr_length;
+	}
+	
+	/**
+	 * Calculates the distance from the line segment to a point.
+	 *
+	 * @param point Input point.
+	 *
+	 * @return Distance from the line segment to @p point.
+	 */
+	[[nodiscard]] inline T distance(const vector_type& point) const
+	{
+		const T d = sqr_distance(point);
+		return d ? std::sqrt(d) : d;
 	}
 };
 

src/engine/geom/primitives/point.hpp

@@ -32,7 +32,7 @@ namespace primitives {
  * @tparam N Number of dimensions.
  */
 template <class T, std::size_t N>
-using point = vector<T, N>;
+using point = math::vector<T, N>;
 
 } // namespace primitives
 

src/engine/geom/primitives/ray.hpp

@@ -57,18 +57,6 @@ struct ray
 		return origin + direction * distance;
 	}
 	
-	/**
-	 * Calculates the closest point on the ray to a point.
-	 *
-	 * @param point Input point.
-	 *
-	 * @return Closest point on the ray to @p point.
-	 */
-	[[nodiscard]] inline constexpr vector_type closest_point(const vector_type& point) const noexcept
-	{
-		return extrapolate(std::max<T>(T{0}, math::dot(point - origin, direction)));
-	}
-	
 	/**
 	 * Calculates the square distance from the ray to a point.
 	 *

src/engine/geom/primitives/stadium.hpp

@@ -0,0 +1,39 @@
+/*
+ * 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_PRIMITIVES_STADIUM_HPP
+#define ANTKEEPER_GEOM_PRIMITIVES_STADIUM_HPP
+
+#include <engine/geom/primitives/hypercapsule.hpp>
+
+namespace geom {
+namespace primitives {
+
+/**
+ * 2-dimensional capsule.
+ *
+ * @tparam T Real type.
+ */
+template <class T>
+using stadium = hypercapsule<T, 2>;
+
+} // namespace primitives
+} // namespace geom
+
+#endif // ANTKEEPER_GEOM_PRIMITIVES_STADIUM_HPP

src/engine/physics/kinematics/collider-type.hpp

@@ -36,7 +36,10 @@ enum class collider_type: std::uint8_t
 	sphere,
 	
 	/// Box collider.
-	box
+	box,
+	
+	/// Capsule collider.
+	capsule
 };
 
 } // namespace physics

src/engine/physics/kinematics/colliders/capsule-collider.hpp

@@ -0,0 +1,118 @@
+/*
+ * 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_PHYSICS_CAPSULE_COLLIDER_HPP
+#define ANTKEEPER_PHYSICS_CAPSULE_COLLIDER_HPP
+
+#include <engine/physics/kinematics/collider.hpp>
+#include <engine/geom/primitives/capsule.hpp>
+
+namespace physics {
+
+/**
+ * Capsule collision object.
+ */
+class capsule_collider: public collider
+{
+public:
+	/// Capule type.
+	using capsule_type = geom::capsule<float>;
+	
+	[[nodiscard]] inline constexpr collider_type type() const noexcept override
+	{
+		return collider_type::capsule;
+	}
+	
+	/**
+	 * Constructs a capsule collider from a capsule.
+	 *
+	 * @param capsule Capsule shape.
+	 */
+	inline explicit capsule_collider(const capsule_type& capsule) noexcept:
+		m_capsule{capsule}
+	{}
+	
+	/**
+	 * Constructs a capsule collider.
+	 *
+	 * @param segment Capsule line segment.
+	 * @param radius Capsule hemisphere radius.
+	 */
+	/// @{
+	inline capsule_collider(const capsule_type::segment_type& segment, float radius) noexcept:
+		m_capsule{segment, radius}
+	{}
+	capsule_collider() noexcept = default;
+	/// @}
+	
+	/**
+	 * Sets the collider's capsule.
+	 *
+	 * @param capsule Capsule shape.
+	 */
+	inline void set_capsule(const capsule_type& capsule) noexcept
+	{
+		m_capsule = capsule;
+	}
+	
+	/**
+	 * Sets the segment of the capsule.
+	 *
+	 * @param segment Capsule segment, in object space.
+	 */
+	inline void set_segment(const capsule_type::segment_type& segment) noexcept
+	{
+		m_capsule.segment = segment;
+	}
+	
+	/**
+	 * Sets the radius of the capsule hemispheres.
+	 *
+	 * @param radius Capsule hemisphere radius.
+	 */
+	inline void set_radius(float radius) noexcept
+	{
+		m_capsule.radius = radius;
+	}
+	
+	/// Returns the capsule shape.
+	[[nodiscard]] inline const capsule_type& get_capsule() const noexcept
+	{
+		return m_capsule;
+	}
+	
+	/// Returns the segment of the capsule, in object space.
+	[[nodiscard]] inline const capsule_type::segment_type& get_segment() const noexcept
+	{
+		return m_capsule.segment;
+	}
+	
+	/// Returns the radius of the capsule hemispheres.
+	[[nodiscard]] inline float get_radius() const noexcept
+	{
+		return m_capsule.radius;
+	}
+	
+private:
+	capsule_type m_capsule{{{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}}, 0.0f};
+};
+
+} // namespace physics
+
+#endif // ANTKEEPER_PHYSICS_CAPSULE_COLLIDER_HPP

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

@@ -66,6 +66,7 @@
 #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/resources/resource-manager.hpp>
@@ -360,7 +361,7 @@ nest_selection_state::nest_selection_state(::game& ctx):
 	
 	
 	auto worker_skeletal_mesh = std::make_unique<scene::skeletal_mesh>(worker_model);
-	worker_skeletal_mesh->get_pose() = pupal_pose;
+	//worker_skeletal_mesh->get_pose() = pupal_pose;
 	
 	
 	worker_ant_eid = ctx.entity_registry->create();
@@ -917,11 +918,6 @@ void nest_selection_state::setup_controls()
 				
 				scene_component projectile_scene;
 				
-				auto projectile_mesh = std::make_shared<scene::skeletal_mesh>(worker_model);
-				projectile_mesh->get_pose() = *worker_model->get_skeleton().get_pose("pupal");
-				projectile_scene.object = projectile_mesh;
-				//projectile_scene.object = std::make_shared<scene::static_mesh>(ctx.resource_manager->load<render::model>("sphere.mdl"));
-				//projectile_scene.object = std::make_shared<scene::static_mesh>(ctx.resource_manager->load<render::model>("cube.mdl"));
 				
 				transform_component projectile_transform;
 				projectile_transform.local = camera_transform.world;
@@ -934,13 +930,30 @@ void nest_selection_state::setup_controls()
 				projectile_body->set_inertia(0.05f);
 				projectile_body->set_angular_damping(0.5f);
 				
-				auto projectile_collider = std::make_shared<physics::box_collider>(float3{-1.0f, -1.0f, -1.0f}, float3{1.0f, 1.0f, 1.0f});
-				//auto projectile_collider = std::make_shared<physics::sphere_collider>(1.0f);
-				
-				
-				projectile_collider->set_material(std::make_shared<physics::collider_material>(0.4f, 0.1f, 0.2f));
+				if (ctx.mouse_look_action.is_active())
+				{
+					auto projectile_collider = std::make_shared<physics::sphere_collider>(1.0f);
+					//auto projectile_collider = std::make_shared<physics::box_collider>(float3{-1.0f, -1.0f, -1.0f}, float3{1.0f, 1.0f, 1.0f});
+					projectile_collider->set_material(std::make_shared<physics::collider_material>(0.4f, 0.1f, 0.2f));
+					projectile_body->set_collider(std::move(projectile_collider));
+					
+					projectile_scene.object = std::make_shared<scene::static_mesh>(ctx.resource_manager->load<render::model>("sphere.mdl"));
+					//projectile_scene.object = std::make_shared<scene::static_mesh>(ctx.resource_manager->load<render::model>("cube.mdl"));
+				}
+				else
+				{
+					auto projectile_collider = std::make_shared<physics::capsule_collider>(geom::capsule<float>{{{0.0f, 0.0f, 1.0f}, {0.0f, 0.0f, -1.0f}}, 0.6f});
+					projectile_collider->set_material(std::make_shared<physics::collider_material>(0.4f, 0.1f, 0.2f));
+					projectile_body->set_collider(std::move(projectile_collider));
+					
+					
+					//projectile_scene.object = std::make_shared<scene::static_mesh>(ctx.resource_manager->load<render::model>("capsule.mdl"));
+					
+					auto projectile_mesh = std::make_shared<scene::skeletal_mesh>(worker_model);
+					projectile_mesh->get_pose() = *worker_model->get_skeleton().get_pose("pupal");
+					projectile_scene.object = projectile_mesh;
+				}
 				
-				projectile_body->set_collider(std::move(projectile_collider));
 				projectile_body->apply_central_impulse(camera_transform.world.rotation * float3{0.0f, 0.0f, -10.0f});
 				
 				

src/game/systems/physics-system.cpp

@@ -28,6 +28,8 @@
 #include <engine/physics/kinematics/colliders/plane-collider.hpp>
 #include <engine/physics/kinematics/colliders/sphere-collider.hpp>
 #include <engine/physics/kinematics/colliders/box-collider.hpp>
+#include <engine/physics/kinematics/colliders/capsule-collider.hpp>
+#include <engine/geom/closest-point.hpp>
 #include <execution>
 
 namespace {
@@ -60,18 +62,27 @@ physics_system::physics_system(entity::registry& registry):
 	constexpr auto plane_i = std::to_underlying(physics::collider_type::plane);
 	constexpr auto sphere_i = std::to_underlying(physics::collider_type::sphere);
 	constexpr auto box_i = std::to_underlying(physics::collider_type::box);
+	constexpr auto capsule_i = std::to_underlying(physics::collider_type::capsule);
 	
 	narrow_phase_table[plane_i][plane_i] = std::bind_front(&physics_system::narrow_phase_plane_plane, this);
-	narrow_phase_table[plane_i][sphere_i] = std::bind(&physics_system::narrow_phase_plane_sphere, this, std::placeholders::_1, std::placeholders::_2, 1.0f);
-	narrow_phase_table[plane_i][box_i] = std::bind(&physics_system::narrow_phase_plane_box, this, std::placeholders::_1, std::placeholders::_2, 1.0f);
+	narrow_phase_table[plane_i][sphere_i] = std::bind_front(&physics_system::narrow_phase_plane_sphere, this);
+	narrow_phase_table[plane_i][box_i] = std::bind_front(&physics_system::narrow_phase_plane_box, this);
+	narrow_phase_table[plane_i][capsule_i] = std::bind_front(&physics_system::narrow_phase_plane_capsule, this);
 	
 	narrow_phase_table[sphere_i][plane_i] = std::bind_front(&physics_system::narrow_phase_sphere_plane, this);
 	narrow_phase_table[sphere_i][sphere_i] = std::bind_front(&physics_system::narrow_phase_sphere_sphere, this);
 	narrow_phase_table[sphere_i][box_i] = std::bind_front(&physics_system::narrow_phase_sphere_box, this);
+	narrow_phase_table[sphere_i][capsule_i] = std::bind_front(&physics_system::narrow_phase_sphere_capsule, this);
 	
 	narrow_phase_table[box_i][plane_i] = std::bind_front(&physics_system::narrow_phase_box_plane, this);
 	narrow_phase_table[box_i][sphere_i] = std::bind_front(&physics_system::narrow_phase_box_sphere, this);
 	narrow_phase_table[box_i][box_i] = std::bind_front(&physics_system::narrow_phase_box_box, this);
+	narrow_phase_table[box_i][capsule_i] = std::bind_front(&physics_system::narrow_phase_box_capsule, this);
+	
+	narrow_phase_table[capsule_i][plane_i] = std::bind_front(&physics_system::narrow_phase_capsule_plane, this);
+	narrow_phase_table[capsule_i][sphere_i] = std::bind_front(&physics_system::narrow_phase_capsule_sphere, this);
+	narrow_phase_table[capsule_i][box_i] = std::bind_front(&physics_system::narrow_phase_capsule_box, this);
+	narrow_phase_table[capsule_i][capsule_i] = std::bind_front(&physics_system::narrow_phase_capsule_capsule, this);
 }
 
 void physics_system::update(float t, float dt)
@@ -384,7 +395,7 @@ void physics_system::narrow_phase_plane_plane(physics::rigid_body& body_a, physi
 	return;
 }
 
-void physics_system::narrow_phase_plane_sphere(physics::rigid_body& body_a, physics::rigid_body& body_b, float normal_sign)
+void physics_system::narrow_phase_plane_sphere(physics::rigid_body& body_a, physics::rigid_body& body_b)
 {
 	const auto& plane_a = static_cast<const physics::plane_collider&>(*body_a.get_collider());
 	const auto& sphere_b = static_cast<const physics::sphere_collider&>(*body_b.get_collider());
@@ -408,13 +419,13 @@ void physics_system::narrow_phase_plane_sphere(physics::rigid_body& body_a, phys
 	// Generate collision contact
 	auto& contact = manifold.contacts[0];
 	contact.point = body_b.get_position() - plane_normal * sphere_b.get_radius();
-	contact.normal = plane_normal * -normal_sign;
+	contact.normal = plane_normal;
 	contact.depth = std::abs(signed_distance - sphere_b.get_radius());
 	
 	narrow_phase_manifolds.emplace_back(std::move(manifold));
 }
 
-void physics_system::narrow_phase_plane_box(physics::rigid_body& body_a, physics::rigid_body& body_b, float normal_sign)
+void physics_system::narrow_phase_plane_box(physics::rigid_body& body_a, physics::rigid_body& body_b)
 {
 	const auto& plane_a = static_cast<const physics::plane_collider&>(*body_a.get_collider());
 	const auto& box_b = static_cast<const physics::box_collider&>(*body_b.get_collider());
@@ -453,7 +464,7 @@ void physics_system::narrow_phase_plane_box(physics::rigid_body& body_a, physics
 		{
 			auto& contact = manifold.contacts[manifold.contact_count];
 			contact.point = point;
-			contact.normal = plane_normal * -normal_sign;
+			contact.normal = plane_normal;
 			contact.depth = std::abs(signed_distance);
 			
 			++manifold.contact_count;
@@ -473,23 +484,93 @@ void physics_system::narrow_phase_plane_box(physics::rigid_body& body_a, physics
 	}
 }
 
+void physics_system::narrow_phase_plane_capsule(physics::rigid_body& body_a, physics::rigid_body& body_b)
+{
+	const auto& plane_a = static_cast<const physics::plane_collider&>(*body_a.get_collider());
+	const auto& capsule_b = static_cast<const physics::capsule_collider&>(*body_b.get_collider());
+	
+	// Transform plane into world-space
+	geom::plane<float> plane;
+	plane.normal = body_a.get_orientation() * plane_a.get_normal();
+	plane.constant = plane_a.get_constant() - math::dot(plane.normal, body_a.get_position());
+	
+	// Transform capsule into world-space
+	const geom::capsule<float> capsule
+	{
+		{
+			body_b.get_transform() * capsule_b.get_segment().a,
+			body_b.get_transform() * capsule_b.get_segment().b
+		},
+		capsule_b.get_radius()
+	};
+	
+	// Calculate signed distances to capsule segment endpoints
+	const float distance_a = plane.distance(capsule.segment.a);
+	const float distance_b = plane.distance(capsule.segment.b);
+	
+	collision_manifold_type manifold;
+	manifold.contact_count = 0;
+	
+	if (distance_a <= capsule.radius)
+	{
+		auto& contact = manifold.contacts[manifold.contact_count];
+		
+		contact.point = capsule.segment.a - plane.normal * capsule.radius;
+		contact.normal = plane.normal;
+		contact.depth = std::abs(distance_a - capsule.radius);
+		
+		++manifold.contact_count;
+	}
+	
+	if (distance_b <= capsule.radius)
+	{
+		auto& contact = manifold.contacts[manifold.contact_count];
+		
+		contact.point = capsule.segment.b - plane.normal * capsule.radius;
+		contact.normal = plane.normal;
+		contact.depth = std::abs(distance_b - capsule.radius);
+		
+		++manifold.contact_count;
+	}
+	
+	if (manifold.contact_count)
+	{
+		manifold.body_a = &body_a;
+		manifold.body_b = &body_b;
+		narrow_phase_manifolds.emplace_back(std::move(manifold));
+	}
+}
+
 void physics_system::narrow_phase_sphere_plane(physics::rigid_body& body_a, physics::rigid_body& body_b)
 {
-	narrow_phase_plane_sphere(body_b, body_a, -1.0f);
+	narrow_phase_plane_sphere(body_b, body_a);
 }
 
 void physics_system::narrow_phase_sphere_sphere(physics::rigid_body& body_a, physics::rigid_body& body_b)
 {
-	const auto& sphere_a = static_cast<const physics::sphere_collider&>(*body_a.get_collider());
-	const auto& sphere_b = static_cast<const physics::sphere_collider&>(*body_b.get_collider());
+	const auto& collider_a = static_cast<const physics::sphere_collider&>(*body_a.get_collider());
+	const auto& collider_b = static_cast<const physics::sphere_collider&>(*body_b.get_collider());
+	
+	// Transform spheres into world-space
+	const math::vector<float, 3> center_a = body_a.get_transform() * collider_a.get_center();
+	const math::vector<float, 3> center_b = body_b.get_transform() * collider_b.get_center();
+	const float radius_a = collider_a.get_radius();
+	const float radius_b = collider_b.get_radius();
+	
+	// Sum sphere radii
+	const float sum_radii = radius_a + radius_b;
 	
-	const float sum_radii = sphere_a.get_radius() + sphere_b.get_radius();
-	const float sqr_sum_radii = sum_radii * sum_radii;
+	// Get vector from center a to center b
+	const math::vector<float, 3> difference = center_b - center_a;
 	
-	const math::vector<float, 3> difference = body_b.get_position() - body_a.get_position();
+	const float sqr_distance = math::sqr_length(difference);
+	if (sqr_distance > sum_radii * sum_radii)
+	{
+		return;
+	}
 	
-	const float sqr_distance = math::dot(difference, difference);
-	if (sqr_distance > sqr_sum_radii)
+	// Ignore degenerate case (sphere centers identical)
+	if (!sqr_distance)
 	{
 		return;
 	}
@@ -502,18 +583,12 @@ void physics_system::narrow_phase_sphere_sphere(physics::rigid_body& body_a, phy
 	
 	// Generate collision contact
 	auto& contact = manifold.contacts[0];
-	if (sqr_distance != 0.0f)
-	{
-		const float distance = std::sqrt(sqr_distance);
-		contact.normal = difference / distance;
-		contact.depth = sum_radii - distance;
-	}
-	else
-	{
-		contact.normal = {1.0f, 0.0f, 0.0f};
-		contact.depth = sum_radii;
-	}
-	contact.point = body_a.get_position() + contact.normal * sphere_a.get_radius();
+	
+	const float distance = std::sqrt(sqr_distance);
+	
+	contact.normal = difference / distance;
+	contact.depth = sum_radii - distance;
+	contact.point = center_a + contact.normal * (radius_a - contact.depth * 0.5f);
 	
 	narrow_phase_manifolds.emplace_back(std::move(manifold));
 }
@@ -523,9 +598,63 @@ void physics_system::narrow_phase_sphere_box(physics::rigid_body& body_a, physic
 	return;
 }
 
+void physics_system::narrow_phase_sphere_capsule(physics::rigid_body& body_a, physics::rigid_body& body_b)
+{
+	const auto& collider_a = static_cast<const physics::sphere_collider&>(*body_a.get_collider());
+	const auto& collider_b = static_cast<const physics::capsule_collider&>(*body_b.get_collider());
+	
+	// Transform sphere into world-space
+	const math::vector<float, 3> center_a = body_a.get_transform() * collider_a.get_center();
+	const float radius_a = collider_a.get_radius();
+	
+	// Transform capsule into world-space
+	const geom::line_segment<float, 3> segment_b
+	{
+		body_b.get_transform() * collider_b.get_segment().a,
+		body_b.get_transform() * collider_b.get_segment().b
+	};
+	const float radius_b = collider_b.get_radius();
+	
+	// Sum the two radii
+	const float sum_radii = radius_a + radius_b;
+	
+	// Find closest point on capsule segment to sphere center
+	const auto closest_point = geom::closest_point(segment_b, center_a);
+	
+	// Get vector from sphere center to point to on capsule segment
+	const math::vector<float, 3> difference = closest_point - center_a;
+	
+	const float sqr_distance = math::sqr_length(difference);
+	if (sqr_distance > sum_radii * sum_radii)
+	{
+		return;
+	}
+	
+	// Ignore degenerate case (sphere center on capsule segment)
+	if (!sqr_distance)
+	{
+		return;
+	}
+	
+	collision_manifold_type manifold;
+	manifold.contact_count = 1;
+	manifold.body_a = &body_a;
+	manifold.body_b = &body_b;
+	
+	auto& contact = manifold.contacts[0];
+	
+	const float distance = std::sqrt(sqr_distance);
+	
+	contact.depth = sum_radii - distance;
+	contact.normal = difference / distance;
+	contact.point = center_a + contact.normal * (radius_a - contact.depth * 0.5f);
+	
+	narrow_phase_manifolds.emplace_back(std::move(manifold));
+}
+
 void physics_system::narrow_phase_box_plane(physics::rigid_body& body_a, physics::rigid_body& body_b)
 {
-	narrow_phase_plane_box(body_b, body_a, -1.0f);
+	narrow_phase_plane_box(body_b, body_a);
 }
 
 void physics_system::narrow_phase_box_sphere(physics::rigid_body& body_a, physics::rigid_body& body_b)
@@ -537,3 +666,85 @@ void physics_system::narrow_phase_box_box(physics::rigid_body& body_a, physics::
 {
 	return;
 }
+
+void physics_system::narrow_phase_box_capsule(physics::rigid_body& body_a, physics::rigid_body& body_b)
+{
+	return;
+}
+
+void physics_system::narrow_phase_capsule_plane(physics::rigid_body& body_a, physics::rigid_body& body_b)
+{
+	narrow_phase_plane_capsule(body_b, body_a);
+}
+
+void physics_system::narrow_phase_capsule_sphere(physics::rigid_body& body_a, physics::rigid_body& body_b)
+{
+	narrow_phase_sphere_capsule(body_b, body_a);
+}
+
+void physics_system::narrow_phase_capsule_box(physics::rigid_body& body_a, physics::rigid_body& body_b)
+{
+	return;
+}
+
+void physics_system::narrow_phase_capsule_capsule(physics::rigid_body& body_a, physics::rigid_body& body_b)
+{
+	const auto& collider_a = static_cast<const physics::capsule_collider&>(*body_a.get_collider());
+	const auto& collider_b = static_cast<const physics::capsule_collider&>(*body_b.get_collider());
+	
+	// Transform capsules into world-space
+	const geom::capsule<float> capsule_a
+	{
+		{
+			body_a.get_transform() * collider_a.get_segment().a,
+			body_a.get_transform() * collider_a.get_segment().b
+		},
+		collider_a.get_radius()
+	};
+	const geom::capsule<float> capsule_b
+	{
+		{
+			body_b.get_transform() * collider_b.get_segment().a,
+			body_b.get_transform() * collider_b.get_segment().b
+		},
+		collider_b.get_radius()
+	};
+	
+	// Calculate closest points between capsule segments
+	const auto [closest_a, closest_b] = geom::closest_point(capsule_a.segment, capsule_b.segment);
+	
+	// Sum sphere radii
+	const float sum_radii = capsule_a.radius + capsule_b.radius;
+	
+	// Get vector from closest point on segment a to closest point on segment b
+	const math::vector<float, 3> difference = closest_b - closest_a;
+	
+	const float sqr_distance = math::sqr_length(difference);
+	if (sqr_distance > sum_radii * sum_radii)
+	{
+		return;
+	}
+	
+	// Ignore degenerate case (closest points identical)
+	if (!sqr_distance)
+	{
+		return;
+	}
+	
+	// Init collision manifold
+	collision_manifold_type manifold;
+	manifold.body_a = &body_a;
+	manifold.body_b = &body_b;
+	manifold.contact_count = 1;
+	
+	// Generate collision contact
+	auto& contact = manifold.contacts[0];
+	
+	const float distance = std::sqrt(sqr_distance);
+	
+	contact.normal = difference / distance;
+	contact.depth = sum_radii - distance;
+	contact.point = closest_a + contact.normal * (capsule_a.radius - contact.depth * 0.5f);
+	
+	narrow_phase_manifolds.emplace_back(std::move(manifold));
+}

src/game/systems/physics-system.hpp

@@ -63,18 +63,26 @@ private:
 	void correct_positions();
 	
 	void narrow_phase_plane_plane(physics::rigid_body& body_a, physics::rigid_body& body_b);
-	void narrow_phase_plane_sphere(physics::rigid_body& body_a, physics::rigid_body& body_b, float normal_sign);
-	void narrow_phase_plane_box(physics::rigid_body& body_a, physics::rigid_body& body_b, float normal_sign);
+	void narrow_phase_plane_sphere(physics::rigid_body& body_a, physics::rigid_body& body_b);
+	void narrow_phase_plane_box(physics::rigid_body& body_a, physics::rigid_body& body_b);
+	void narrow_phase_plane_capsule(physics::rigid_body& body_a, physics::rigid_body& body_b);
 	
 	void narrow_phase_sphere_plane(physics::rigid_body& body_a, physics::rigid_body& body_b);
 	void narrow_phase_sphere_sphere(physics::rigid_body& body_a, physics::rigid_body& body_b);
 	void narrow_phase_sphere_box(physics::rigid_body& body_a, physics::rigid_body& body_b);
+	void narrow_phase_sphere_capsule(physics::rigid_body& body_a, physics::rigid_body& body_b);
 	
 	void narrow_phase_box_plane(physics::rigid_body& body_a, physics::rigid_body& body_b);
 	void narrow_phase_box_sphere(physics::rigid_body& body_a, physics::rigid_body& body_b);
 	void narrow_phase_box_box(physics::rigid_body& body_a, physics::rigid_body& body_b);
+	void narrow_phase_box_capsule(physics::rigid_body& body_a, physics::rigid_body& body_b);
 	
-	std::array<std::array<std::function<void(physics::rigid_body&, physics::rigid_body&)>, 3>, 3> narrow_phase_table;
+	void narrow_phase_capsule_plane(physics::rigid_body& body_a, physics::rigid_body& body_b);
+	void narrow_phase_capsule_sphere(physics::rigid_body& body_a, physics::rigid_body& body_b);
+	void narrow_phase_capsule_box(physics::rigid_body& body_a, physics::rigid_body& body_b);
+	void narrow_phase_capsule_capsule(physics::rigid_body& body_a, physics::rigid_body& body_b);
+	
+	std::array<std::array<std::function<void(physics::rigid_body&, physics::rigid_body&)>, 4>, 4> narrow_phase_table;
 	
 	math::vector<float, 3> gravity{0.0f, -9.80665f, 0.0f};