From a268405a2f5f6fcc0443724c0bd17c553014bc18 Mon Sep 17 00:00:00 2001 From: "C. J. Howard" Date: Sat, 25 Mar 2023 05:56:46 +0800 Subject: [PATCH] Add capsule collision --- src/engine/geom/closest-point.hpp | 262 +++++++++++++++++ src/engine/geom/primitives/capsule.hpp | 39 +++ src/engine/geom/primitives/hypercapsule.hpp | 82 ++++++ src/engine/geom/primitives/hyperplane.hpp | 12 - src/engine/geom/primitives/hyperrectangle.hpp | 23 -- src/engine/geom/primitives/hypersphere.hpp | 12 - src/engine/geom/primitives/line-segment.hpp | 53 +++- src/engine/geom/primitives/point.hpp | 2 +- src/engine/geom/primitives/ray.hpp | 12 - src/engine/geom/primitives/stadium.hpp | 39 +++ .../physics/kinematics/collider-type.hpp | 5 +- .../kinematics/colliders/capsule-collider.hpp | 118 ++++++++ src/game/states/nest-selection-state.cpp | 37 ++- src/game/systems/physics-system.cpp | 265 ++++++++++++++++-- src/game/systems/physics-system.hpp | 14 +- 15 files changed, 871 insertions(+), 104 deletions(-) create mode 100644 src/engine/geom/closest-point.hpp create mode 100644 src/engine/geom/primitives/capsule.hpp create mode 100644 src/engine/geom/primitives/hypercapsule.hpp create mode 100644 src/engine/geom/primitives/stadium.hpp create mode 100644 src/engine/physics/kinematics/colliders/capsule-collider.hpp 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};