diff --git a/src/engine/geom/closest-point.hpp b/src/engine/geom/closest-point.hpp
new file mode 100644
index 0000000..324d9a3
--- /dev/null
+++ b/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 .
+ */
+
+#ifndef ANTKEEPER_GEOM_CLOSEST_POINT_HPP
+#define ANTKEEPER_GEOM_CLOSEST_POINT_HPP
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+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
+[[nodiscard]] point closest_point(const ray& a, const point& b) noexcept
+{
+ return a.extrapolate(std::max(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
+[[nodiscard]] point closest_point(const line_segment& ab, const point& 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
+[[nodiscard]] std::tuple, point> closest_point(const line_segment& ab, const line_segment& 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(std::max(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(std::max(-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(std::max((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(std::max(-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(std::max((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
+[[nodiscard]] point closest_point(const hyperplane& a, const point& 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
+[[nodiscard]] point closest_point(const hypersphere& a, const point& 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
+[[nodiscard]] point closest_point(const hypercapsule& a, const point& 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
+[[nodiscard]] point closest_point(const hyperrectangle& a, const point& 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(math::max(d), T{0});
+
+ point 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
diff --git a/src/engine/geom/primitives/capsule.hpp b/src/engine/geom/primitives/capsule.hpp
new file mode 100644
index 0000000..c8e595d
--- /dev/null
+++ b/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 .
+ */
+
+#ifndef ANTKEEPER_GEOM_PRIMITIVES_CAPSULE_HPP
+#define ANTKEEPER_GEOM_PRIMITIVES_CAPSULE_HPP
+
+#include
+
+namespace geom {
+namespace primitives {
+
+/**
+ * 3-dimensional capsule.
+ *
+ * @tparam T Real type.
+ */
+template
+using capsule = hypercapsule;
+
+} // namespace primitives
+} // namespace geom
+
+#endif // ANTKEEPER_GEOM_PRIMITIVES_CAPSULE_HPP
diff --git a/src/engine/geom/primitives/hypercapsule.hpp b/src/engine/geom/primitives/hypercapsule.hpp
new file mode 100644
index 0000000..ad71366
--- /dev/null
+++ b/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 .
+ */
+
+#ifndef ANTKEEPER_GEOM_PRIMITIVES_HYPERCAPSULE_HPP
+#define ANTKEEPER_GEOM_PRIMITIVES_HYPERCAPSULE_HPP
+
+#include
+#include
+
+namespace geom {
+namespace primitives {
+
+/**
+ * *n*-dimensional capsule.
+ *
+ * @tparam T Real type.
+ * @tparam N Number of dimensions.
+ */
+template
+struct hypercapsule
+{
+ /// Vector type.
+ using vector_type = math::vector;
+
+ /// Line segment type.
+ using segment_type = geom::line_segment;
+
+ /// 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
diff --git a/src/engine/geom/primitives/hyperplane.hpp b/src/engine/geom/primitives/hyperplane.hpp
index eb07615..c349cf6 100644
--- a/src/engine/geom/primitives/hyperplane.hpp
+++ b/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
diff --git a/src/engine/geom/primitives/hyperrectangle.hpp b/src/engine/geom/primitives/hyperrectangle.hpp
index 64364ac..0644ab9 100644
--- a/src/engine/geom/primitives/hyperrectangle.hpp
+++ b/src/engine/geom/primitives/hyperrectangle.hpp
@@ -102,29 +102,6 @@ struct hyperrectangle
return math::length(math::max(vector_type::zero(), d)) + std::min(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{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.
*
diff --git a/src/engine/geom/primitives/hypersphere.hpp b/src/engine/geom/primitives/hypersphere.hpp
index b7de49b..3602776 100644
--- a/src/engine/geom/primitives/hypersphere.hpp
+++ b/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.
*
diff --git a/src/engine/geom/primitives/line-segment.hpp b/src/engine/geom/primitives/line-segment.hpp
index 508d856..33eff7a 100644
--- a/src/engine/geom/primitives/line-segment.hpp
+++ b/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;
}
};
diff --git a/src/engine/geom/primitives/point.hpp b/src/engine/geom/primitives/point.hpp
index 978731c..1813ea5 100644
--- a/src/engine/geom/primitives/point.hpp
+++ b/src/engine/geom/primitives/point.hpp
@@ -32,7 +32,7 @@ namespace primitives {
* @tparam N Number of dimensions.
*/
template
-using point = vector;
+using point = math::vector;
} // namespace primitives
diff --git a/src/engine/geom/primitives/ray.hpp b/src/engine/geom/primitives/ray.hpp
index 51ea2d2..6e7f338 100644
--- a/src/engine/geom/primitives/ray.hpp
+++ b/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{0}, math::dot(point - origin, direction)));
- }
-
/**
* Calculates the square distance from the ray to a point.
*
diff --git a/src/engine/geom/primitives/stadium.hpp b/src/engine/geom/primitives/stadium.hpp
new file mode 100644
index 0000000..d9457be
--- /dev/null
+++ b/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 .
+ */
+
+#ifndef ANTKEEPER_GEOM_PRIMITIVES_STADIUM_HPP
+#define ANTKEEPER_GEOM_PRIMITIVES_STADIUM_HPP
+
+#include
+
+namespace geom {
+namespace primitives {
+
+/**
+ * 2-dimensional capsule.
+ *
+ * @tparam T Real type.
+ */
+template
+using stadium = hypercapsule;
+
+} // namespace primitives
+} // namespace geom
+
+#endif // ANTKEEPER_GEOM_PRIMITIVES_STADIUM_HPP
diff --git a/src/engine/physics/kinematics/collider-type.hpp b/src/engine/physics/kinematics/collider-type.hpp
index c47bd8c..1507c6e 100644
--- a/src/engine/physics/kinematics/collider-type.hpp
+++ b/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
diff --git a/src/engine/physics/kinematics/colliders/capsule-collider.hpp b/src/engine/physics/kinematics/colliders/capsule-collider.hpp
new file mode 100644
index 0000000..9eb1ae2
--- /dev/null
+++ b/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 .
+ */
+
+#ifndef ANTKEEPER_PHYSICS_CAPSULE_COLLIDER_HPP
+#define ANTKEEPER_PHYSICS_CAPSULE_COLLIDER_HPP
+
+#include
+#include
+
+namespace physics {
+
+/**
+ * Capsule collision object.
+ */
+class capsule_collider: public collider
+{
+public:
+ /// Capule type.
+ using capsule_type = geom::capsule;
+
+ [[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
diff --git a/src/game/states/nest-selection-state.cpp b/src/game/states/nest-selection-state.cpp
index c9ab974..0519bc5 100644
--- a/src/game/states/nest-selection-state.cpp
+++ b/src/game/states/nest-selection-state.cpp
@@ -66,6 +66,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -360,7 +361,7 @@ nest_selection_state::nest_selection_state(::game& ctx):
auto worker_skeletal_mesh = std::make_unique(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(worker_model);
- projectile_mesh->get_pose() = *worker_model->get_skeleton().get_pose("pupal");
- projectile_scene.object = projectile_mesh;
- //projectile_scene.object = std::make_shared(ctx.resource_manager->load("sphere.mdl"));
- //projectile_scene.object = std::make_shared(ctx.resource_manager->load("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(float3{-1.0f, -1.0f, -1.0f}, float3{1.0f, 1.0f, 1.0f});
- //auto projectile_collider = std::make_shared(1.0f);
-
-
- projectile_collider->set_material(std::make_shared(0.4f, 0.1f, 0.2f));
+ if (ctx.mouse_look_action.is_active())
+ {
+ auto projectile_collider = std::make_shared(1.0f);
+ //auto projectile_collider = std::make_shared(float3{-1.0f, -1.0f, -1.0f}, float3{1.0f, 1.0f, 1.0f});
+ projectile_collider->set_material(std::make_shared(0.4f, 0.1f, 0.2f));
+ projectile_body->set_collider(std::move(projectile_collider));
+
+ projectile_scene.object = std::make_shared(ctx.resource_manager->load("sphere.mdl"));
+ //projectile_scene.object = std::make_shared(ctx.resource_manager->load("cube.mdl"));
+ }
+ else
+ {
+ auto projectile_collider = std::make_shared(geom::capsule{{{0.0f, 0.0f, 1.0f}, {0.0f, 0.0f, -1.0f}}, 0.6f});
+ projectile_collider->set_material(std::make_shared(0.4f, 0.1f, 0.2f));
+ projectile_body->set_collider(std::move(projectile_collider));
+
+
+ //projectile_scene.object = std::make_shared(ctx.resource_manager->load("capsule.mdl"));
+
+ auto projectile_mesh = std::make_shared(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});
diff --git a/src/game/systems/physics-system.cpp b/src/game/systems/physics-system.cpp
index bcccd09..52bffa8 100644
--- a/src/game/systems/physics-system.cpp
+++ b/src/game/systems/physics-system.cpp
@@ -28,6 +28,8 @@
#include
#include
#include
+#include
+#include
#include
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(*body_a.get_collider());
const auto& sphere_b = static_cast(*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(*body_a.get_collider());
const auto& box_b = static_cast(*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(*body_a.get_collider());
+ const auto& capsule_b = static_cast(*body_b.get_collider());
+
+ // Transform plane into world-space
+ geom::plane 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 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(*body_a.get_collider());
- const auto& sphere_b = static_cast(*body_b.get_collider());
+ const auto& collider_a = static_cast(*body_a.get_collider());
+ const auto& collider_b = static_cast(*body_b.get_collider());
+
+ // Transform spheres into world-space
+ const math::vector center_a = body_a.get_transform() * collider_a.get_center();
+ const math::vector 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 difference = center_b - center_a;
- const math::vector 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(*body_a.get_collider());
+ const auto& collider_b = static_cast(*body_b.get_collider());
+
+ // Transform sphere into world-space
+ const math::vector 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 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 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(*body_a.get_collider());
+ const auto& collider_b = static_cast(*body_b.get_collider());
+
+ // Transform capsules into world-space
+ const geom::capsule 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 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 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));
+}
diff --git a/src/game/systems/physics-system.hpp b/src/game/systems/physics-system.hpp
index 9f9c39c..c6de5a3 100644
--- a/src/game/systems/physics-system.hpp
+++ b/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, 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, 4>, 4> narrow_phase_table;
math::vector gravity{0.0f, -9.80665f, 0.0f};