core/ship.hh

#ifndef VTESTBED_CORE_SHIP_HH
#define VTESTBED_CORE_SHIP_HH

#include <iosfwd>

#include <vtestbed/base/blitz.hh>
#include <vtestbed/base/bstream.hh>
#include <vtestbed/base/constants.hh>
#include <vtestbed/core/compartment.hh>
#include <vtestbed/core/grid.hh>
#include <vtestbed/core/hull.hh>
#include <vtestbed/core/macros.hh>
#include <vtestbed/core/mass.hh>
#include <vtestbed/core/math.hh>
#include <vtestbed/core/propulsor.hh>
#include <vtestbed/core/types.hh>
#include <vtestbed/geometry/basis.hh>
#include <vtestbed/geometry/math.hh>
#include <vtestbed/geometry/quaternion.hh>
#include <vtestbed/geometry/types.hh>

namespace vtb {

    namespace core {

        template <class T>
        inline vtb::geometry::Basis<T,3>
        angular_velocity_matrix_zyx(const Vector<T,3>& sin, const Vector<T,3>& cos) {
            static constexpr const int theta=1, psi=2;
            return {{cos(theta)*cos(psi), -sin(psi), 0,
                cos(theta)*sin(psi), cos(psi), 0,
                -sin(theta), 0, 1}};
        }

        template <class T>
        inline vtb::geometry::Basis<T,3>
        angular_velocity_matrix_zyx(const Vector<T,3>& euler_angles) {
            return angular_velocity_matrix_zyx<T>(sin(euler_angles), cos(euler_angles));
        }

        template <class T>
        inline vtb::geometry::Basis<T,3>
        angular_velocity_matrix_xyz(const Vector<T,3>& sin, const Vector<T,3>& cos) {
            static constexpr const int phi=0, theta=1;
            return {{1, 0, sin(theta),
                0, cos(phi), -sin(phi)*cos(theta),
                0, sin(phi),  cos(phi)*cos(theta)}};
        }

        template <class T>
        inline vtb::geometry::Basis<T,3>
        angular_velocity_matrix_xyz(const Vector<T,3>& euler_angles) {
            return angular_velocity_matrix_xyz<T>(sin(euler_angles), cos(euler_angles));
        }

        template <class T>
        inline blitz::TinyMatrix<T,3,4>
        angular_velocity_matrix_quaternion(const vtb::geometry::Quaternion<T>& q) {
            blitz::TinyMatrix<T,3,4> m;
            m(0,0) =  q(1); m(0,1) = -q(0); m(0,2) = -q(3); m(0,3) =  q(2);
            m(1,0) =  q(2); m(1,1) =  q(3); m(1,2) = -q(0); m(1,3) = -q(1);
            m(2,0) =  q(3); m(2,1) = -q(2); m(2,2) =  q(1); m(2,3) = -q(0);
            m *= 2;
            return m;
        }

        template <class T>
        inline vtb::geometry::Basis<T,3>
        euler_angles_matrix_zyx(
            const Vector<T,3>& sin,
            const Vector<T,3>& cos,
            const T tan_theta
        ) {
            static constexpr const int theta=1, psi=2;
            return {{cos(psi)/cos(theta), sin(psi)/cos(theta), 0,
                    -sin(psi), cos(psi), 0,
                    cos(psi)*tan_theta, sin(psi)*tan_theta, 1}};
        }

        template <class T>
        inline vtb::geometry::Basis<T,3>
        euler_angles_matrix_zyx(const Vector<T,3>& euler_angles) {
            return euler_angles_matrix_zyx<T>(
                sin(euler_angles),
                cos(euler_angles),
                sin(1)/cos(1)
            );
        }

        template <class T>
        inline vtb::geometry::Basis<T,3>
        euler_angles_matrix_xyz(
            const Vector<T,3>& sin,
            const Vector<T,3>& cos,
            const T tan_theta
        ) {
            static constexpr const int phi=0, theta=1;
            return {{1, sin(phi)*tan_theta, -cos(phi)*tan_theta,
                0, cos(phi), sin(phi),
                0, -sin(phi)/cos(theta), cos(phi)/cos(theta)}};
        }

        template <class T>
        inline vtb::geometry::Basis<T,3>
        euler_angles_matrix_xyz(const Vector<T,3>& euler_angles) {
            return euler_angles_matrix_zyx<T>(
                sin(euler_angles),
                cos(euler_angles),
                sin(1)/cos(1)
            );
        }

        template <class T>
        inline Vector<T,3>
        euler_angles_residual_vector_zyx(
            const Vector<T,3>& sin,
            const Vector<T,3>& cos,
            const T tan_theta,
            const Vector<T,3>& angvelocity
        ) {
            static constexpr const int theta=1, psi=2;
            // common subexpressions
            auto d_01 = angvelocity(0)*angvelocity(1);
            auto d_12 = angvelocity(1)*angvelocity(2);
            auto d_02 = angvelocity(0)*angvelocity(2);
            return {
                -sin(theta)*cos(psi)*d_01 - cos(psi)*d_12 - cos(theta)*sin(psi)*d_02,
                -sin(theta)*sin(psi)*d_01 - sin(psi)*d_12 + cos(theta)*cos(psi)*d_02,
                -cos(theta)*d_01
            };
        }

        template <class T>
        inline Vector<T,3>
        euler_angles_residual_vector_xyz(
            const Vector<T,3>& sin,
            const Vector<T,3>& cos,
            const T tan_theta,
            const Vector<T,3>& angvelocity
        ) {
            static constexpr const int phi=0, theta=1;
            // common subexpressions
            auto d_01 = angvelocity(0)*angvelocity(1);
            auto d_12 = angvelocity(1)*angvelocity(2);
            auto d_02 = angvelocity(0)*angvelocity(2);
            return {
                cos(theta)*d_12,
                -sin(phi)*d_01 + sin(phi)*sin(theta)*d_12 - cos(phi)*cos(theta)*d_02,
                 cos(phi)*d_01 - cos(phi)*sin(theta)*d_12 - sin(phi)*cos(theta)*d_02
            };
        }

        template <class T>
        inline blitz::TinyMatrix<T,4,3>
        quaternion_matrix(const vtb::geometry::Quaternion<T>& q) {
            blitz::TinyMatrix<T,4,3> m;
            m(0,0) =  q(1); m(0,1) =  q(2); m(0,2) =  q(3);
            m(1,0) = -q(0); m(1,1) =  q(3); m(1,2) = -q(2);
            m(2,0) = -q(3); m(2,1) = -q(0); m(2,2) =  q(1);
            m(3,0) =  q(2); m(3,1) = -q(1); m(3,2) = -q(0);
            m /= 2;
            return m;
        }

        template <class T>
        class Ship {

        public:
            using scalar_type = T;
            using vec3 = blitz::TinyVector<T,3>;
            using hull_type = Hull<T>;
            using room_type = Compartment<T>;
            using room_array = Compartment_array<T>;
            using propulsor_type = Propulsor<T>;
            using mass_type = Mass<T>;
            using mass_array_type = Mass_array<T>;
            using matrix_type = blitz::TinyMatrix<T,3,3>;
            using coordinate_system_type = vtb::geometry::Coordinate_system<T,3>;
            using quaternion_coordinate_system_type =
                vtb::geometry::Quaternion_coordinate_system<T>;
            using rotation_matrix_type = vtb::geometry::Rotation_matrix<T,3>;
            using basis_type = vtb::geometry::Basis<T,3>;
            using quaternion_type = vtb::geometry::Quaternion<T>;

        public:
            union state_vector_type {
                using variables_type = blitz::TinyVector<T,13>;
                variables_type variables;
                struct {
                    vec3 position;
                    vec3 velocity;
                    quaternion_type quaternion;
                    vec3 angmomentum;
                };
                state_vector_type(): variables{T{}} {}
                state_vector_type(const state_vector_type& rhs): variables(rhs.variables) {}
                state_vector_type(const variables_type& rhs): variables(rhs) {}
                ~state_vector_type() {}
                inline const variables_type& vars() const { return this->variables; }
                inline operator const variables_type&() const { return this->variables; }
                inline state_vector_type&
                operator=(const variables_type& rhs) { this->variables = rhs; return *this; }
            };
            using variables_type = typename state_vector_type::variables_type;

            static_assert(sizeof(variables_type) == 3*sizeof(vec3) + sizeof(quaternion_type),
                "bad state_vector");
            static_assert(sizeof(variables_type) == sizeof(state_vector_type),
                "bad state_vector");

        private:
            // Position coordinates and their first and second time derivatives
            // (velocity and acceleration).
            vec3 _position[3] {T{},T{},T{}};
            // Angular position and its first and second time derivatives.
            vec3 _angdisplacement[3] {T{},T{},T{}};
            // Angular momentum.
            vec3 _angmomentum{T{}};
            // Rotation quaternion and its first and second time derivatives.
            quaternion_type _quaternion{1,0,0,0};
            // Rotation matrix computed from the current Euler angles.
            rotation_matrix_type _rotation_matrix;
            rotation_matrix_type _inertia_matrix;
            hull_type _hull;
            room_array _rooms;
            propulsor_type _propulsor;
            scalar_type _mass{};

        public:

            inline const vec3& position(int i=0) const { return this->_position[i]; }

            inline void position(int i, const vec3& rhs) { this->_position[i] = rhs; }

            inline const vec3& velocity() const { return position(1); }

            inline void velocity(const vec3& rhs) { this->_position[1] = rhs; }

            inline const vec3& acceleration() const { return position(2); }

            inline const vec3&
            angular_displacement(int i=0) const {
                return this->_angdisplacement[i];
            }

            inline vec3 euler_angles() const { return to_euler_angles(this->_quaternion); }

            inline const quaternion_type& quaternion() const { return this->_quaternion; }

            void euler_angles(const vec3& rhs);

            void quaternion(const quaternion_type& q);

            inline T roll() const { return euler_angles()[0]; }

            inline T pitch() const { return euler_angles()[1]; }

            inline T yaw() const { return euler_angles()[2]; }

            inline T surge(int i=0) const { return this->_position[i][0]; }

            inline T sway(int i=0) const { return this->_position[i][1]; }

            inline T heave(int i=0) const { return this->_position[i][2]; }

            inline const vec3& angular_velocity() const { return angular_displacement(1); }
            void angular_velocity(const vec3& rhs);

            inline const vec3& angular_acceleration() const { return angular_displacement(2); }

            inline const vec3& angular_momentum() const { return this->_angmomentum; }
            inline void angular_momentum(const vec3& rhs) { this->_angmomentum = rhs; }

            void state_vector(const state_vector_type& v, const state_vector_type& dv, T dt);

            inline void
            state_vector(const variables_type& v, const variables_type& dv, T dt) {
                this->state_vector(
                    reinterpret_cast<const state_vector_type&>(v),
                    reinterpret_cast<const state_vector_type&>(dv),
                    dt
                );
            }

            state_vector_type state_vector() const;
            void reset();
            inline const hull_type& hull() const { return this->_hull; }
            void hull(hull_type&& rhs);
            void hull(const hull_type& rhs);

            inline void compartments(const room_array& rhs) { this->_rooms = rhs; }
            inline room_array& compartments() { return this->_rooms; }
            inline const room_array& compartments() const { return this->_rooms; }
            inline const propulsor_type& propulsor() const { return this->_propulsor; }
            inline void propulsor(const propulsor_type& rhs) { this->_propulsor = rhs; }

            inline scalar_type mass() const { return this->_mass; }

            T flooded_mass() const;

            inline void
            mass(scalar_type rhs) {
                if (rhs < 0) { throw std::invalid_argument("bad mass"); }
                this->_mass = rhs;
            }

            inline void
            displacement(scalar_type rhs) {
                this->mass(vtb::base::constants<T>::water_density()*rhs);
            }

            inline scalar_type
            displacement() const {
                return this->mass() / vtb::base::constants<T>::water_density();
            }

            inline void
            draught(scalar_type rhs) {
                if (rhs < 0) { throw std::invalid_argument("bad draught"); }
                auto level = hull().bounds(2).min() + rhs;
                this->displacement(hull().signed_volume_below(2, level));
            }

            inline coordinate_system_type
            earth_fixed_coordinate_system() const {
                return make_coordinate_system(rotation_matrix(), position(0));
            }

            inline quaternion_coordinate_system_type
            body_fixed_coordinate_system() const {
                return make_coordinate_system(conj(quaternion()), position(0));
            }

            inline const rotation_matrix_type&
            rotation_matrix() const {
                return this->_rotation_matrix;
            }

            inline const rotation_matrix_type&
            inertia_matrix() const {
                return this->_inertia_matrix;
            }

            void dump(std::ostream& out) const;

            template <class X>
            friend std::ostream&
            operator<<(std::ostream& out, const Ship<X>& rhs);

            template <class X>
            friend bstream&
            operator<<(bstream& out, const Ship<X>& rhs);

            template <class X>
            friend bstream&
            operator>>(bstream& in, Ship<X>& rhs);

        private:

            inline void
            angular_displacement(int i, const vec3& rhs) {
                this->_angdisplacement[i] = rhs;
            }

        };

        template <class T>
        std::ostream&
        operator<<(std::ostream& out, const Ship<T>& rhs);

        template <class T>
        bstream&
        operator<<(bstream& out, const Ship<T>& rhs);

        template <class T>
        bstream&
        operator>>(bstream& in, Ship<T>& rhs);

    }

}

#endif // vim:filetype=cpp