space.hpp

Go to the documentation of this file.

#ifndef SPACE_HPP
#define SPACE_HPP
 
/**
 * \file space.hpp
 * \brief N-body simulation space with gravitational interaction and collision
 *        response.
 * \author Le Bars, Yoann
 * \ingroup PhysicsCore
 *
 * This file defines the `Space` class, which manages the state and dynamics of
 * all bodies in the simulation. The physics model includes:
 *
 * - **Gravitational Interaction**: All bodies exert a gravitational force on
 *   each other, calculated using Newton's law of universal gravitation.
 *
 * - **Collision Response**: Collisions between spherical bodies are handled
 *   using an impulse-based method that accounts for both linear and
 *   rotational motion, including friction.
 *
 * - **Integration**: The simulation state (position and velocity) is
 *   advanced using the **Velocity Verlet** algorithm, a time-reversible and
 *   energy-conserving semi-implicit Euler integration scheme.
 *
 * - **Broad-Phase Detection**: A k-d tree is used to efficiently find
 *   potentially colliding pairs of bodies, avoiding an O(N²) check.
 *
 * This file is part of the pure C++ benchmark.
 *
 * This program 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.
 *
 * This program 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
 * this program. If not, see <https://www.gnu.org/licenses/>.
 */
 
#include <omp.h>
 
#include <chrono>
#include <cstddef>
#include <cstdint>
#include <memory>
#include <optional>
#include <stdexcept>
#include <vector>
 
#include "body.hpp"
#include "config.hpp"
#include "constants.hpp"
#include "kdtree.hpp"
 
// Forward-declare the test fixture class from the global namespace.
class SpaceTest;
 
namespace Model {
 
    /**
     * \brief A data container to hold all relevant information for a single
     *        collision event.
     */
    struct CollisionContext {
        /// \brief A proxy to the first body in the collision.
        BodyProxy& b1;
        /// \brief A proxy to the second body in the collision.
        BodyProxy& b2;
        /// \brief The normalised vector pointing from body 1 to body 2.
        Vector3d nVec_;
        /// \brief The vector from the centre of body 1 to the contact point.
        Vector3d r1Vec_;
        /// \brief The vector from the centre of body 2 to the contact point.
        Vector3d r2Vec_;
        /// \brief The relative velocity between the two bodies at the contact
        /// point.
        Vector3d vRel_;
        /// \brief The penetration depth of the two bodies.
        double overlap_;
    };
 
    /**
     * \brief Class describing a space in which move several bodies.
     * \ingroup PhysicsCore
     */
    class Space {
       public:
        /* Grant the test fixture from the global namespace access to protected
           and private members. */
        friend class ::SpaceTest;
 
        /// \brief Default constructor.
        Space() : dt_(0.), G_(G), epsilon_(EPSILON), kdTree_(bodies_) {}
 
        /**
         * \brief Initialise the space.
         *
         * \param config The simulation configuration object containing all
         * simulation parameters.
         */
        explicit Space(const Configuration::SimulationConfig& config);
 
        /// \brief Destructor to clean up OpenMP locks.
        ~Space();
 
        /**
         * \brief Number of bodies getter.
         *
         * \return The number of bodies in the space.
         */
        std::size_t n() const { return bodies_.size(); }
 
        /**
         * \brief Get a proxy to the i-th body.
         *
         * \param i Body index.
         *
         * \returns A mutable proxy object for the i-th body.
         */
        BodyProxy body(std::size_t i) { return bodies_[i]; }
 
        /**
         * \brief Get a const proxy to the i-th body.
         *
         * \param i Body index.
         *
         * \returns A const proxy object for the i-th body.
         */
        ConstBodyProxy body(std::size_t i) const { return bodies_[i]; }
 
        /**
         * \brief Get the time step used in the last completed simulation
         *        frame.
         *
         * \return The previous time step value.
         */
        [[nodiscard]] double getPreviousTimeStep() const {
            return previous_dt_;
        }
 
        /**
         * \brief Computes one full step of the simulation.
         *
         * This method orchestrates the main simulation loop, including:
         * 1. Advancing positions and velocities (Velocity Verlet).
         * 2. Detecting and resolving collisions.
         * 3. Calculating new forces (gravity).
         * \param iteration The current simulation iteration number.
         */
        void computeDynamics(std::size_t iteration);
 
        /**
         * \brief Calculates the kinetic and potential energy of the system.
         * \return A tuple containing:
         *         1. Translational Kinetic Energy
         *         2. Rotational Kinetic Energy
         *         3. Potential Energy
         */
        [[nodiscard]]
        std::tuple<double, double, double> calculateSystemEnergy() const;
 
        /**
         * \brief Retrieves the current positions and orientations of all
         * bodies.
         * \return A tuple containing a vector of positions and a vector of
         * quaternions.
         */
        [[nodiscard]] std::tuple<Vector3dVec, QuaterniondVec>
        getAllBodyTransforms() const;
 
        /**
         * \brief Retrieves all data required for display for all bodies.
         *
         * \param out_positions Output vector for body positions.
         * \param out_quaternions Output vector for body orientations.
         * \param out_torques Output vector for body torques.
         * \param out_alphas Output vector for body angular accelerations.
         */
        void getDataForDisplay(Vector3dVec& out_positions,
                               QuaterniondVec& out_quaternions,
                               Vector3dVec& out_torques,
                               Vector3dVec& out_alphas) const;
        void printProfilingReport() const;
 
        // --- Configuration Getters/Setters ---
        /**
         * \brief Get the universal gravitational constant.
         * \return The gravitational constant G.
         */
        [[nodiscard]] double getG() const { return G_; }
 
        /**
         * \brief Set the universal gravitational constant.
         * \param g The new gravitational constant value.
         */
        void setG(double g) { G_ = g; }
 
        /**
         * \brief Get the numerical precision threshold.
         * \return The epsilon value.
         */
        [[nodiscard]] double getEpsilon() const { return epsilon_; }
 
        /**
         * \brief Set the numerical precision threshold.
         * \param eps The new epsilon value.
         */
        void setEpsilon(double eps) { epsilon_ = eps; }
 
        /**
         * \brief Get the coefficient of restitution.
         * \return The coefficient of restitution.
         */
        [[nodiscard]] double getCoeffRestitution() const {
            return coeffRestitution_;
        }
 
        /**
         * \brief Set the coefficient of restitution.
         * \param e The new coefficient of restitution (typically in [0, 1]).
         */
        void setCoeffRestitution(double e) { coeffRestitution_ = e; }
 
        /**
         * \brief Get the coefficient of kinetic friction.
         * \return The coefficient of kinetic friction.
         */
        [[nodiscard]] double getCoeffFriction() const { return coeffFriction_; }
 
        /**
         * \brief Set the coefficient of kinetic friction.
         * \param mu The new coefficient of kinetic friction.
         */
        void setCoeffFriction(double mu) { coeffFriction_ = mu; }
 
        /**
         * \brief Get the coefficient of static friction.
         * \return The coefficient of static friction.
         */
        [[nodiscard]] double getCoeffStaticFriction() const {
            return coeffStaticFriction_;
        }
 
        /**
         * \brief Set the coefficient of static friction.
         * \param mu_s The new coefficient of static friction.
         */
        void setCoeffStaticFriction(double mu_s) {
            coeffStaticFriction_ = mu_s;
        }
 
        /**
         * \brief Get the linear damping factor.
         * \return The linear damping factor.
         */
        [[nodiscard]] double getLinearDamping() const { return linearDamping_; }
 
        /**
         * \brief Set the linear damping factor.
         * \param damping The new linear damping factor (typically in [0, 1]).
         */
        void setLinearDamping(double damping) { linearDamping_ = damping; }
 
        /**
         * \brief Get the angular damping factor.
         * \return The angular damping factor.
         */
        [[nodiscard]] double getAngularDamping() const {
            return angularDamping_;
        }
 
        /**
         * \brief Set the angular damping factor.
         * \param damping The new angular damping factor (typically in [0, 1]).
         */
        void setAngularDamping(double damping) { angularDamping_ = damping; }
 
        /**
         * \brief Get the current time step.
         * \return The current time step value.
         */
        [[nodiscard]] double getTimeStep() const { return dt_; }
 
        /**
         * \brief Set the current time step.
         * \param dt The new time step value.
         */
        void setTimeStep(double dt) { dt_ = dt; }
 
        /**
         * \brief Get the positional correction factor.
         * \return The positional correction factor.
         */
        [[nodiscard]] double getPositionalCorrectionFactor() const {
            return positionalCorrectionFactor_;
        }
 
        /**
         * \brief Set the positional correction factor.
         * \param factor The new positional correction factor.
         */
        void setPositionalCorrectionFactor(double factor) {
            positionalCorrectionFactor_ = factor;
        }
 
        /**
         * \brief Check if diagnostics are enabled.
         * \return True if diagnostics are enabled, false otherwise.
         */
        [[nodiscard]] bool isDiagnosticsEnabled() const {
            return diagnosticsEnabled_;
        }
 
        /**
         * \brief Enable or disable diagnostics.
         * \param enabled True to enable diagnostics, false to disable.
         */
        void setDiagnosticsEnabled(bool enabled) {
            diagnosticsEnabled_ = enabled;
        }
 
        // --- Test API (protected but accessible via friend) ---
        /**
         * \brief Provides direct access to the Bodies container for testing.
         * \return A reference to the internal Bodies object.
         * \note This method is public but intended for testing only.
         *       Access is controlled via the friend declaration.
         */
        [[nodiscard]] Bodies& getBodiesForTest() { return bodies_; }
 
        /**
         * \brief Factory method to create a CollisionContext if two bodies are
         * colliding.
         *
         * This method is part of the internal collision handling API and is
         * exposed to tests for unit testing collision logic.
         *
         * \param b1 First body.
         * \param b2 Second body.
         * \return A CollisionContext object if a collision occurs, otherwise
         * `std::nullopt`.
         * \note This method is public but intended for testing only.
         */
        [[nodiscard]] std::optional<CollisionContext> createCollisionContext(
            BodyProxy& b1, BodyProxy& b2);
 
        /**
         * \brief Calculates and applies the normal impulse (restitution) for a
         * collision.
         *
         * This method is part of the internal collision handling API and is
         * exposed to tests for unit testing collision logic.
         *
         * \param ctx The collision context.
         * \return A tuple containing the total impulse vector, its magnitude,
         * and the magnitude of the bias impulse component.
         * \note This method is public but intended for testing only.
         */
        [[nodiscard]] std::tuple<Vector3d, double, double>
        applyRestitutionImpulse(CollisionContext& ctx);
 
        /**
         * \brief Calculates the tangential (frictional) impulse for a
         * collision.
         *
         * This method is part of the internal collision handling API and is
         * exposed to tests for unit testing collision logic.
         *
         * \param ctx The collision context.
         * \param jn The magnitude of the normal impulse, used for the friction
         * limit.
         * \param impulseN The normal impulse vector.
         * \return The friction impulse vector.
         * \note This method is public but intended for testing only.
         */
        [[nodiscard]] Vector3d applyFrictionImpulse(const CollisionContext& ctx,
                                                    double jn,
                                                    const Vector3d& impulseN);
 
        /**
         * \brief A greedy graph colouring algorithm to assign a colour
         * (integer) to each body such that no two adjacent bodies (i.e.,
         * colliding bodies) share the same colour.
         *
         * This method implements a parallel greedy graph colouring algorithm
         * used to partition collision pairs into independent sets. Bodies with
         * the same colour can have their collisions resolved in parallel
         * without conflicts.
         *
         * \par Algorithm Overview
         * The algorithm uses an iterative approach with two passes per
         * iteration:
         * - <b>Pass 1</b>: Each body (in parallel) assigns itself the
         *   smallest available colour not used by any of its neighbours.
         * - <b>Pass 2</b>: Detect conflicts where adjacent bodies share the
         *   same colour (can occur due to race conditions in parallel
         *   execution).
         *
         * The algorithm iterates until no conflicts remain.
         *
         * \par Example
         * Consider a collision graph with 4 bodies:
         * \code
         * graph[0] = {1, 2}  // Body 0 collides with bodies 1 and 2
         * graph[1] = {0, 3}  // Body 1 collides with bodies 0 and 3
         * graph[2] = {0}     // Body 2 collides with body 0
         * graph[3] = {1}     // Body 3 collides with body 1
         * \endcode
         *
         * After colouring, a possible result is:
         * \code
         * colors[0] = 0  // Bodies 0 and 3 can be processed in parallel
         * colors[1] = 1  // Body 1 can be processed alone
         * colors[2] = 1  // Bodies 1 and 2 can be processed in parallel
         * colors[3] = 0  // (same as body 0)
         * \endcode
         *
         * \par Thread Safety
         * This method uses OpenMP locks to synchronize access to the shared
         * `colors_` vector. Reads of neighbour colours are protected by locks
         * to avoid data races, while writes are minimal and lock-protected.
         *
         * \param graph The collision graph where `graph[i]` contains
         *              indices of bodies colliding with `i`.
         *              The graph should be undirected (if body i collides with
         *              j, then j should appear in graph[i] and i in graph[j]).
         *
         * \note This method is public but intended for testing only.
         * \note The algorithm is guaranteed to terminate as conflicts decrease
         *       with each iteration (worst-case: O(N) iterations for N bodies).
         */
        void colorGraph(const std::vector<std::vector<std::size_t>>& graph);
 
       private:
        /**
         * \brief Handles collision detection and response between two
         * bodies.
         *
         * \param b1 A proxy to the first body in the collision.
         * \param b2 A proxy to the second body in the collision.
         */
        void handleCollision(BodyProxy& b1, BodyProxy& b2);
 
        /**
         * \brief Initialises the bodies in the simulation with random
         *        properties.
         *
         * \param n The number of bodies to create.
         * \param dens The density of the bodies.
         * \param seed The seed for random number generation.
         */
        void initializeBodies(std::size_t n, double dens, unsigned int seed);
 
        /**
         * \brief Resolves interpenetration by moving bodies apart along the
         *        collision normal.
         */
        void resolveInterpenetration(CollisionContext& ctx);
 
        /**
         * \brief Calculates the effective mass of two colliding bodies in a
         * given direction.
         *
         * \param ctx The collision context.
         * \param direction_vec The direction vector (e.g., normal or
         * tangent). \return The scalar effective mass.
         */
        [[nodiscard]] double getEffectiveMass(const CollisionContext& ctx,
                                              const Vector3d& direction_vec);
 
        // --- Member Variables ---
        // --- Simulation Stages ---
        /**
         * \brief Logs system energy and checks for instability if diagnostics
         * are enabled.
         * \param iteration The current simulation iteration number.
         */
        void logSystemEnergy(std::size_t iteration);
 
        /**
         * \brief Builds the collision graph using broad-phase and
         *        narrow-phase detection.
         *
         * This method implements a two-phase collision detection algorithm
         * to efficiently identify all colliding body pairs in the simulation.
         * The result is stored in `collisionGraph_`, where `collisionGraph_[i]`
         * contains the indices of all bodies colliding with body `i`.
         *
         * \par Algorithm Overview
         * The algorithm uses a two-phase approach:
         *
         * <b>Phase 1: Broad-Phase Detection (Spatial Acceleration)</b>
         * - Rebuilds the k-d tree with current body positions
         * - For each body, performs a radius search to find all bodies within
         *   a safe distance (body radius + maximum body radius in system)
         * - Uses thread-local storage to collect candidate pairs in parallel
         * - Complexity: O(N log N) where N is the number of bodies
         *
         * <b>Phase 2: Narrow-Phase Detection (Exact Collision Test)</b>
         * - For each candidate pair from Phase 1, performs an exact collision
         *   test using distance and radius comparison
         * - Only pairs that actually overlap are added to the collision graph
         * - Complexity: O(C) where C is the number of candidate pairs
         *
         * \par Example
         * Consider a system with 3 bodies:
         * \code
         * Body 0: position (0, 0, 0), radius 1.0
         * Body 1: position (1.5, 0, 0), radius 1.0  // Overlaps with body 0
         * Body 2: position (5, 0, 0), radius 1.0     // No collision
         * \endcode
         *
         * After execution:
         * \code
         * collisionGraph_[0] = {1}  // Body 0 collides with body 1
         * collisionGraph_[1] = {0}  // Body 1 collides with body 0
         * collisionGraph_[2] = {}    // Body 2 has no collisions
         * \endcode
         *
         * \par Thread Safety
         * This method is fully parallelized using OpenMP:
         * - Broad-phase searches are performed in parallel using thread-local
         *   storage to avoid race conditions
         * - Candidate pairs are collected per-thread and merged sequentially
         *   after parallel execution
         * - The final collision graph is built sequentially from thread-local
         *   results
         *
         * \par Performance Optimizations
         * - Maximum radius is cached and only recalculated when bodies are
         *   added/removed (not every frame)
         * - Results vectors are pre-allocated to avoid repeated allocations
         * - k-d tree is rebuilt each frame to reflect position changes
         * - Thread-local storage minimizes lock contention
         *
         * \note This method is called once per simulation step in
         *       `computeDynamics()`.
         * \note The collision graph is undirected: if body i collides with j,
         *       then j appears in `collisionGraph_[i]` and i appears in
         *       `collisionGraph_[j]`.
         */
        void buildCollisionGraph();
 
        /// \brief Resolves all detected collisions using graph colouring.
        void resolveCollisions();
 
        /// \brief Calculates and applies gravitational forces, and completes
        /// Velocity Verlet part 2.
        void applyGravity(double current_dt, double linear_damping,
                          double angular_damping);
 
        /**
         * \brief Determines the optimal time step for the next frame based on
         * current velocities and accelerations.
         */
        void updateAdaptiveTimeStep();
 
        /// \brief Time step (in s).
        double dt_;
 
        /// \brief Previous time step (in s).
        double previous_dt_;
 
        /// \brief Universal gravitational constant (in m³ / kg /s²).
        double G_;
 
        /// \brief Numerical precision threshold.
        double epsilon_;
 
        /// \brief Coefficient of restitution for collisions.
        double coeffRestitution_;
 
        /// \brief Coefficient of kinetic (sliding) friction.
        double coeffFriction_;
 
        /// \brief Coefficient of static friction.
        double coeffStaticFriction_;
 
        /// \brief The percentage of overlap to correct in each frame.
        double positionalCorrectionFactor_;
 
        /// \brief Damping factor for linear velocity.
        double linearDamping_;
 
        /// \brief Damping factor for angular velocity.
        double angularDamping_;
 
        /// \brief Flag indicating if diagnostic output is enabled.
        bool diagnosticsEnabled_;
 
        /// \brief Frequency of energy log output (every N iterations).
        std::size_t logFreq_;
 
        /// \brief SoA container for all bodies in the simulation.
        Bodies bodies_;  // NOLINT
 
        /// \brief k-d tree for broad-phase collision detection.
        KDTree kdTree_;
 
        /// \brief Adjacency list for the collision graph.
        std::vector<std::vector<std::size_t>> collisionGraph_;  // NOLINT
 
        /**
         * \brief Stores the colour of each body for parallel collision
         *        processing.
         */
        std::vector<int> colors_;  // NOLINT
 
        /// \brief OpenMP locks for thread-safe collision graph updates.
        std::vector<omp_lock_t> graphLocks_;
 
        /**
         * \brief Thread-local storage for collision pairs found during
         *        broad-phase. This reduces lock contention on the global
         *        `collisionGraph_` during parallel processing.
         */
        std::vector<std::vector<std::pair<std::size_t, std::size_t>>>
            thread_local_collision_pairs_;
 
        /// \brief Thread-local storage for broad-phase search results to avoid
        /// allocations.
#if defined(NANOFLANN_VERSION) && NANOFLANN_VERSION < 0x150
        std::vector<std::vector<std::pair<std::size_t, double>>>
            thread_local_search_results_;
#else
        std::vector<std::vector<nanoflann::ResultItem<std::size_t, double>>>
            thread_local_search_results_;
#endif
 
        /// \brief Thread-local storage for graph coloring used colours flags.
        std::vector<std::vector<uint8_t>> thread_local_used_colors_;
 
        /// \brief Thread-local storage for graph coloring neighbor colours.
        std::vector<std::vector<int>> thread_local_neighbour_colours_;
 
        /// \brief Persistent storage for graph coloring conflict flags.
        std::vector<uint8_t> conflicts_;
 
        /// \brief The total energy from the previous logged step.
        double lastTotalEnergy_ = std::numeric_limits<double>::infinity();
 
        /// \brief Cached maximum radius for collision detection optimization.
        /// This is recalculated only when bodies are added/removed, not every
        /// frame.
        mutable double cached_max_radius_ = 0.0;
 
        // --- Profiling Timers ---
        /* These members accumulate the time spent in different simulation
           stages. */
        /// \brief Time spent in integration steps.
        mutable double profIntegration_ = 0.0;
 
        /// \brief Time spent in collision graph building.
        mutable double profCollisionGraph_ = 0.0;
 
        /// \brief Time spent in collision response.
        mutable double profCollisionResponse_ = 0.0;
 
        /// \brief Time spent in force calculation.
        mutable double profForceCalculation_ = 0.0;
    };
}
 
#endif  // SPACE_HPP