body.hpp

Go to the documentation of this file.

#ifndef BODY_HPP
#define BODY_HPP
 
/**
 * \file body.hpp
 * \brief SoA container for simulation bodies and proxies for AoS-like access.
 * \author Le Bars, Yoann
 * \ingroup PhysicsCore
 *
 * This file is part of the pure C++ benchmark.
 */
 
#include <cassert>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <vector>
 
namespace Model {
 
    class Bodies;          // Forward declaration
    class ConstBodyProxy;  // Forward declaration
    class BodiesAdaptor;   // Forward declaration for kdtree adaptor
 
    using Eigen::Quaterniond;
    using Eigen::Vector3d;
    using Eigen::Vector4d;
 
    /* We need to declare these types as metatypes so they can be used in
       queued signal/slot connections across threads. */
    using Vector3dVec =
        std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>;
    using QuaterniondVec =
        std::vector<Eigen::Quaterniond,
                    Eigen::aligned_allocator<Eigen::Quaterniond>>;
 
    /**
     * \brief A template base class for body proxies to reduce code
     *        duplication.
     * \tparam T Either `Bodies` or `const Bodies`.
     * \ingroup PhysicsCore
     */
    template <typename T>
    class BodyProxyBase {
       public:
        /**
         * \brief Construct a new Body Proxy Base object.
         *
         * \param in_bodies The `Bodies` container to be referenced.
         * \param in_index The index of the body this proxy will access.
         */
        BodyProxyBase(T& in_bodies, std::size_t in_index)
            : bodies_(in_bodies), index_(in_index) {}
 
        // --- Common Getters ---
        /**
         * \brief Access to body masses.
         *
         * \return Mass of the current body.
         */
        [[nodiscard]] double m() const noexcept { return bodies_.m_[index_]; }
 
        /**
         * \brief Access to the inverse of the body masses.
         *
         * \return The inverse of the current body's mass.
         */
        [[nodiscard]] double getInverseMass() const {
            return bodies_.invM_[index_];  // NOLINT
        }
 
        /**
         * \brief Access to body radii.
         *
         * \return The radius of the current body.
         */
        [[nodiscard]] double r() const noexcept { return bodies_.r_[index_]; }
 
        /**
         * \brief Access to the inverse of the body's moment of inertia.
         *
         * \return The inverse of the current body's moment of inertia.
         */
        [[nodiscard]] double getInverseInertia() const {
            return bodies_.iInv_[index_];  // NOLINT
        }
 
        /**
         * \brief Access to body positions.
         *
         * \return A const reference to the current body's position vector.
         */
        [[nodiscard]] const Vector3d& x() const noexcept {
            return bodies_.x_[index_];
        }
 
        /**
         * \brief Access to body velocities.
         *
         * \return A const reference to the current body's velocity vector.
         */
        [[nodiscard]] const Vector3d& v() const noexcept {
            return bodies_.v_[index_];
        }
 
        /**
         * \brief Access to body orientation.
         *
         * \return A const reference to the current body's orientation
         * quaternion.
         */
        [[nodiscard]] const Quaterniond& q() const noexcept {
            return bodies_.q_[index_];
        }
 
        /**
         * \brief Access to body angular velocities.
         *
         * \return A const reference to the current body's angular velocity
         * vector.
         */
        [[nodiscard]] const Vector3d& omega() const noexcept {
            return bodies_.omega_[index_];
        }
 
       protected:
        /// \brief Reference to the main SoA container.
        T& bodies_;
 
        /// \brief The index of the body this proxy refers to.
        std::size_t index_;
    };
 
    /**
     * \brief A proxy object that provides an AoS-like interface to a body
     *        stored in the `Bodies` SoA container.
     */
    /**
     * \brief Mutable proxy for accessing individual bodies in the Bodies container.
     * \ingroup PhysicsCore
     */
    class BodyProxy : public BodyProxyBase<Bodies> {
       public:
        /**
         * \brief Construct a new Body Proxy object.
         *
         * \param in_bodies The `Bodies` container to be accessed.
         * \param in_index The index of the body this proxy will access.
         */
        BodyProxy(Bodies& in_bodies, std::size_t in_index);
 
        /**
         * \brief Implicit conversion operator to a const proxy.
         *
         * \return A const proxy to the same body.
         */
        // NOLINTNEXTLINE(google-explicit-constructor)
        operator ConstBodyProxy() const;
 
        // --- Getters ---
        /**
         * \brief Access to body accelerations.
         * \return A mutable reference to the body's linear acceleration
         * vector.
         */
        [[nodiscard]] Vector3d& a();
 
        /**
         * \brief Access to body angular accelerations.
         *
         * \return A mutable reference to the body's angular acceleration
         * vector.
         */
        [[nodiscard]] Vector3d& alpha();
 
        /**
         * \brief Access to body angular velocities.
         * \return A mutable reference to the body's angular velocity vector.
         */
        [[nodiscard]] Vector3d& omega();
 
        // --- Modifiers ---
        /**
         * \brief Add a displacement to a position.
         *
         * \param correction Correction to be applied.
         */
        void addToX(const Vector3d& correction);
 
        /**
         * \brief Apply an impulse to a body.
         *
         * \param in_J Impulse to be applied.
         * \param in_r_vec Vector from the centre of mass to the contact point.
         * \return The torque vector generated by the impulse on this body.
         */
        Vector3d applyImpulse(const Vector3d& in_J, const Vector3d& in_r_vec);
 
        /**
         * \brief Set the acceleration of a body.
         *
         * \param new_a The new linear acceleration vector.
         */
        void setAcceleration(const Vector3d& new_a);
 
        /**
         * \brief Set the angular acceleration of a body.
         *
         * \param new_alpha The new angular acceleration vector.
         */
        void setAngularAcceleration(const Vector3d& new_alpha);
 
        /**
         * \brief Accumulate an acceleration to a body.
         *
         * \param accel The linear acceleration vector to add.
         */
        void accumulateAcceleration(const Vector3d& accel);
 
        /**
         * \brief Accumulate an angular acceleration to a body.
         *
         * \param in_alpha The angular acceleration vector to add.
         */
        void accumulateAngularAcceleration(const Vector3d& in_alpha);
 
        /// \brief Resets linear and angular accelerations to zero.
        void resetAccelerations();
 
        /// \brief Resets the accumulated torque to zero.
        void resetTorque();
 
        /// \brief Accumulates a torque vector to the body's total torque.
        void accumulateTorque(const Vector3d& torque);
 
        /**
         * \brief Applies damping to linear and angular velocities.
         *
         * \param linear_damping The damping factor for linear velocity.
         * \param angular_damping The damping factor for angular velocity.
         */
        void dampenVelocity(double linear_damping, double angular_damping);
 
        /// \brief Updates angular acceleration based on the accumulated torque.
        void updateAlphaFromTorque();
    };
 
    /**
     * \brief A const proxy object that provides a read-only AoS-like interface
     *        to a body stored in the `Bodies` SoA container.
     * \ingroup PhysicsCore
     */
    class ConstBodyProxy : public BodyProxyBase<const Bodies> {
       public:
        /**
         * \brief Construct a new Const Body Proxy object.
         *
         * \param in_bodies The `Bodies` container to be accessed.
         * \param in_index The index of the body this proxy will access.
         */
        ConstBodyProxy(const Bodies& in_bodies, std::size_t in_index);
    };
 
    /**
     * \brief Structure-of-Arrays (SoA) container for all bodies in the
     *        simulation.
     *
     * This layout improves cache performance by storing related data
     * contiguously.
     */
    /**
     * \brief Structure of Arrays container for all simulation bodies.
     * \ingroup PhysicsCore
     */
    class Bodies {
       public:
        // Grant access to the proxy base class template.
        template <typename T>
        friend class BodyProxyBase;
        friend class BodyProxy;
        friend class Space;
        friend class ConstBodyProxy;
        friend class BodiesAdaptor;
 
        /**
         * \brief Reserve memory space for the data structure.
         *
         * \param n Number of bodies in the simulation.
         */
        void reserve(std::size_t n) {
            m_.reserve(n);
            invM_.reserve(n);
            r_.reserve(n);
            iInv_.reserve(n);
            x_.reserve(n);
            v_.reserve(n);
            a_.reserve(n);
            q_.reserve(n);
            omega_.reserve(n);
            alpha_.reserve(n);
            torque_.reserve(n);
        }
 
        /**
         * \brief Add a new body at the back of the data structure.
         *
         * \param in_m New body mass.
         * \param in_r New body radius.
         * \param in_x0 New body initial position.
         * \param in_v0 New body initial velocity.
         */
        void emplaceBack(double in_m, double in_r, const Vector3d& in_x0,
                         const Vector3d& in_v0, const Vector3d& in_omega0) {
            m_.push_back(in_m);
            invM_.push_back(in_m > 0 ? 1.0 / in_m : 0.0);
            r_.push_back(in_r);
            if (invM_.back() > 0 && in_r > 0) {
                iInv_.push_back(1.0 / (0.4 * in_m * in_r * in_r));
            } else {
                iInv_.push_back(0.0);
            }
            x_.push_back(in_x0);
            v_.push_back(in_v0);
            a_.push_back(Vector3d::Zero());
            q_.push_back(Quaterniond::Identity());
            omega_.push_back(in_omega0);
            alpha_.push_back(Vector3d::Zero());
            torque_.push_back(Vector3d::Zero());
        }
 
        /**
         * \brief Vectorized Velocity Verlet: Part 1.
         *
         * \param dt The time step for the current frame's velocity update.
         * \param dt_pos The time step from the previous frame for the position
         *               update.
         */
        void integratePart1(double dt, double dt_pos) {
#pragma omp parallel for schedule(static)
            for (std::size_t i = 0; i < size(); ++i) {
                /* In Velocity Verlet, positions are updated using the full
                   time step from the *previous* frame's dynamics. */
                x_[i] += v_[i] * dt_pos + 0.5 * a_[i] * dt_pos * dt_pos;
 
                /* Velocities and rotations are half-updated using the
                 *current* frame’s time step, `dt`. */
                v_[i] += 0.5 * a_[i] * dt;
                omega_[i] += 0.5 * alpha_[i] * dt;
            }
            // Update all rotations in a single vectorized pass.
            updateRotationsRK2(dt);
        }
 
        /**
         * \brief Vectorized Velocity Verlet: Part 2.
         * \param dt The time step for the current frame.
         */
        void integratePart2(double dt) {
#pragma omp parallel for
            for (std::size_t i = 0; i < size(); ++i) {
                // Complete the velocity update for this frame.
                omega_[i] += 0.5 * alpha_[i] * dt;
                v_[i] += 0.5 * a_[i] * dt;
            }
        }
 
        /**
         * \brief Get number of bodies in the simulation.
         *
         * \return The total number of bodies.
         */
        [[nodiscard]] std::size_t size() const { return m_.size(); }
 
        /**
         * \brief A proxy to access a given body.
         *
         * \param index Index of the body to be accessed.
         *
         * \return A mutable proxy to the specified body.
         */
        BodyProxy operator[](std::size_t index) {
            assert(index < size() && "Index out of bounds");
            return BodyProxy(*this, index);
        }
 
        /**
         * \brief Array like access to given body.
         *
         * \param index Index of the body to be accessed.
         *
         * \return A const proxy to the specified body.
         */
        ConstBodyProxy operator[](std::size_t index) const {
            assert(index < size() && "Index out of bounds");
            return ConstBodyProxy(*this, index);
        }
 
        // --- Data Arrays (SoA) ---
       private:
        /**
         * \brief Calculates the time derivative of a quaternion.
         *
         * \param omega The angular velocity vector.
         * \param q_coeffs The quaternion coefficients [x, y, z, w].
         * \return The quaternion derivative multiplied by 2.
         */
        [[nodiscard]] static Vector4d getQuaternionDerivative(  // NOLINT
            const Vector3d& omega, const Vector4d& q_coeffs) {
            const Vector3d q_vec = q_coeffs.head<3>();
            const double q_w = q_coeffs.w();
 
            const Vector3d derivative_vec = q_w * omega + omega.cross(q_vec);
            const double derivative_w = -omega.dot(q_vec);
 
            return {derivative_vec.x(), derivative_vec.y(), derivative_vec.z(),
                    derivative_w};
        }
 
        /**
         * \brief Vectorized update of orientations for all bodies using a
         * 2nd-order Runge-Kutta method (RK2).
         *
         * This method mirrors the efficient, vectorized approach of the Python
         * implementation, operating on all bodies at once.
         *
         * \param dt The time step for the integration.
         */
        void updateRotationsRK2(double dt) {
#pragma omp parallel for schedule(static)
            for (std::size_t i = 0; i < size(); ++i) {
                // As an optimisation, skip bodies that are not rotating.
                if (omega_[i].squaredNorm() < 1e-12) {
                    continue;
                }
 
                // RK2 Midpoint Method for quaternion integration
                const Vector4d q_coeffs = q_[i].coeffs();
 
                /* k1 = f(q_old)
                   The derivative returns 2*dq/dt, so we use 0.25*dt instead of
                   0.5*dt. */
                const Vector4d k1 =
                    getQuaternionDerivative(omega_[i], q_coeffs);
                // q_mid = q_old + 0.5 * dt * (0.5 * k1)
                const Vector4d q_mid = (q_coeffs + 0.25 * dt * k1).normalized();
 
                /* k2 = f(q_mid)
                   The derivative returns 2*dq/dt, so we use 0.5*dt instead of
                   dt. */
                const Vector4d k2 = getQuaternionDerivative(omega_[i], q_mid);
 
                // q_new = q_old + dt * (0.5 * k2)
                q_[i].coeffs() += 0.5 * dt * k2;
                q_[i].normalize();
            }
        }
 
        /// \brief Bodies’ masses.
        std::vector<double> m_;
 
        /// \brief Bodies inverse masses.
        std::vector<double> invM_;
 
        /// \brief Bodies radii.
        std::vector<double> r_;
 
        /// \brief Bodies inverse moments of inertia.
        std::vector<double> iInv_;
 
        /// \brief Bodies' positions.
        std::vector<Vector3d> x_;
 
        /// \brief Bodies' velocities.
        std::vector<Vector3d> v_;
 
        /// \brief Bodies' accelerations.
        std::vector<Vector3d> a_;
 
        /// \brief Bodies' orientations (quaternions).
        std::vector<Quaterniond> q_;
 
        /// \brief Bodies' angular velocities.
        std::vector<Vector3d> omega_;
 
        /// \brief Bodies' angular accelerations.
        std::vector<Vector3d> alpha_;
 
        /// \brief Net torque applied to each body in a frame.
        std::vector<Vector3d> torque_;
    };
 
    // --- Implementation of BodyProxy ---
 
    inline BodyProxy::BodyProxy(Bodies& in_bodies, std::size_t in_index)
        : BodyProxyBase<Bodies>(in_bodies, in_index) {}
 
    inline BodyProxy::operator ConstBodyProxy() const {
        return {bodies_, index_};  // Implicit conversion is fine here
    }
 
    inline Vector3d& BodyProxy::a() { return bodies_.a_[index_]; }
 
    inline Vector3d& BodyProxy::alpha() { return bodies_.alpha_[index_]; }
 
    inline Vector3d& BodyProxy::omega() {
        return bodies_.omega_[index_];  // Return a mutable reference
    }
 
    // --- Implementation of BodyProxy ---
 
    inline void BodyProxy::addToX(const Vector3d& correction) {
        bodies_.x_[index_] += correction;
    }
 
    inline Vector3d BodyProxy::applyImpulse(const Vector3d& in_J,
                                            const Vector3d& in_r_vec) {
        bodies_.v_[index_] += in_J * getInverseMass();
        return in_r_vec.cross(in_J);
    }
 
    inline void BodyProxy::setAcceleration(const Vector3d& new_a) {
        bodies_.a_[index_] = new_a;
    }
 
    inline void BodyProxy::setAngularAcceleration(const Vector3d& new_alpha) {
        bodies_.alpha_[index_] = new_alpha;
    }
 
    inline void BodyProxy::accumulateAcceleration(const Vector3d& accel) {
        bodies_.a_[index_] += accel;
    }
 
    inline void BodyProxy::accumulateAngularAcceleration(
        const Vector3d& in_alpha) {
        bodies_.alpha_[index_] += in_alpha;
    }
 
    inline void BodyProxy::resetAccelerations() {
        bodies_.a_[index_].setZero();
        bodies_.alpha_[index_].setZero();
    }
 
    inline void BodyProxy::resetTorque() { bodies_.torque_[index_].setZero(); }
 
    inline void BodyProxy::accumulateTorque(const Vector3d& torque) {
        bodies_.torque_[index_] += torque;
    }
 
    inline void BodyProxy::updateAlphaFromTorque() {
        bodies_.alpha_[index_] = bodies_.torque_[index_] * getInverseInertia();
    }
 
    inline void BodyProxy::dampenVelocity(double linear_damping,
                                          double angular_damping) {
        bodies_.v_[index_] *= (1.0 - linear_damping);
        bodies_.omega_[index_] *= (1.0 - angular_damping);
    }
 
    // --- Implementation of ConstBodyProxy ---
 
    inline ConstBodyProxy::ConstBodyProxy(const Bodies& in_bodies,
                                          std::size_t in_index)
        : BodyProxyBase<const Bodies>(in_bodies, in_index) {}
 
}  // namespace Model
#endif  // BODY_HPP