core/ship.cc

#include <cmath>
#include <ostream>
#include <stdexcept>

#include <vtestbed/config/real_type.hh>
#include <vtestbed/core/ship.hh>
#include <vtestbed/core/types.hh>
#include <vtestbed/geometry/basis.hh>
#include <vtestbed/geometry/inertia_tensor.hh>
#include <vtestbed/geometry/quaternion.hh>

template <class T>
void
vtb::core::Ship<T>::hull(hull_type&& rhs) {
    this->_hull = std::move(rhs);
    this->_inertia_matrix = this->_hull.mass_moments().inertia.matrix();
    this->_mass = this->_hull.mass();
}

template <class T>
void
vtb::core::Ship<T>::hull(const hull_type& rhs) {
    this->_hull = rhs;
    this->_inertia_matrix = this->_hull.mass_moments().inertia.matrix();
    this->_mass = this->_hull.mass();
}

template <class T>
T
vtb::core::Ship<T>::flooded_mass() const {
    T sum{};
    for (const auto& room : compartments()) { sum += room.flooded_mass(); }
    return sum;
}

template <class T>
void
vtb::core::Ship<T>::angular_velocity(const vec3& angvelocity) {
    const auto& R = rotation_matrix();
    const auto& R_inv = inverse(R);
    const auto& I_body = inertia_matrix();
    angular_displacement(1, angvelocity);
    angular_momentum(R*(I_body*(R_inv*angvelocity)));
}

template <class T>
void
vtb::core::Ship<T>::euler_angles(const vec3& angles) {
    quaternion(vtb::geometry::from_euler_angles(angles));
}

template <class T>
void
vtb::core::Ship<T>::quaternion(const quaternion_type& q) {
    using vtb::geometry::rotation_matrix;
    this->_quaternion = q;
    this->_rotation_matrix = rotation_matrix(q);
}

template <class T>
auto
vtb::core::Ship<T>::state_vector() const -> state_vector_type {
    state_vector_type v;
    v.position = position(0);
    v.velocity = position(1);
    v.quaternion = quaternion();
    v.angmomentum = angular_momentum();
    return v;
};

template <class T>
void
vtb::core::Ship<T>::state_vector(const state_vector_type& v, const state_vector_type& dv, T dt) {
    auto old_position_1 = position(1);
    auto old_angdisplacement_1 = angular_displacement(1);
    this->position(0, v.position);
    this->position(1, v.velocity);
    this->position(2, dv.velocity);
    this->quaternion(unit(v.quaternion));
    this->angular_momentum(v.angmomentum);
    const auto& R = rotation_matrix();
    const auto& R_inv = inverse(R);
    const auto& I_body_inv = inverse(inertia_matrix());
//  auto I_inv = R*I_body_inv*R_inv;
//  vec3 angvelocity = I_inv*v.angmomentum;
    auto angvelocity = R*(I_body_inv*(R_inv*v.angmomentum));
    auto angacceleration = R*(I_body_inv*(R_inv*dv.angmomentum));
    this->angular_displacement(0, angular_displacement() + angvelocity*dt);
    this->angular_displacement(1, angvelocity);
    this->angular_displacement(2, angacceleration);
}

template <class T>
void
vtb::core::Ship<T>::reset() {
    this->_quaternion.clear();
    this->_position[0] = 0;
    this->_position[1] = 0;
    this->_position[2] = 0;
    this->_angdisplacement[0] = 0;
    this->_angdisplacement[1] = 0;
    this->_angdisplacement[2] = 0;
    this->_angmomentum = 0;
}

template <class T>
void
vtb::core::Ship<T>::dump(std::ostream& out) const {
    out << std::setw(20) << "position"
        << std::setw(12) << "[m]"
        << "    " << this->position(0)
        << '\n';
    out << std::setw(20) << "velocity"
        << std::setw(12) << "[m/s]"
        << "    " << this->position(1)
        << '\n';
    out << std::setw(20) << "acceleration"
        << std::setw(12) << "[m/s^2]"
        << "    " << this->position(2)
        << '\n';
    out << std::setw(20) << "Euler angles"
        << std::setw(12) << "[rad]"
        << "    " << this->euler_angles()
        << '\n';
    out << std::setw(20) << "angular velocity"
        << std::setw(12) << "[rad/s]"
        << "    " << this->angular_displacement(1)
        << '\n';
    out << std::setw(20) << "angular acceleration"
        << std::setw(12) << "[rad/s^2]"
        << "    " << this->angular_displacement(2)
        << '\n';
    out << std::setw(20) << "mass"
        << std::setw(12) << "[t]"
        << "    " << this->mass()/T{1000}
        << '\n';
    out << std::setw(20) << "dimensions"
        << std::setw(12) << "[m]"
        << "    " << this->hull().bounding_box().extent()
        << '\n';
    out << std::setw(20) << "no. of vertices"
        << std::setw(12) << ""
        << "    " << this->hull().vertices().size()
        << '\n';
    out << std::setw(20) << "no. of faces"
        << std::setw(12) << ""
        << "    " << this->hull().faces().size()
        << '\n';
    out << std::setw(20) << "average face area"
        << std::setw(12) << "[m^2]"
        << "    " << this->hull().average_face_area()
        << '\n';
    out << std::setw(20) << "volume"
        << std::setw(12) << "[m^3]"
        << "    " << volume(this->hull())
        << '\n';
    out << std::setw(20) << "centroid"
        << std::setw(12) << "[m]"
        << "    " << centroid(this->hull())
        << '\n';
}

template <class T>
std::ostream&
vtb::core::operator<<(std::ostream& out, const Ship<T>& rhs) {
    rhs.dump(out); return out;
}

template <class T>
vtb::core::bstream&
vtb::core::operator<<(bstream& out, const Ship<T>& rhs) {
    out << rhs._quaternion;
    for (const auto& x : rhs._position) { out << x; }
    for (auto& x : rhs._angdisplacement) { out << x; }
    out << rhs._hull;
    out << rhs._mass;
    return out;
}

template <class T>
vtb::core::bstream&
vtb::core::operator>>(bstream& in, Ship<T>& rhs) {
    in >> rhs._quaternion;
    for (auto& x : rhs._position) { in >> x; }
    for (auto& x : rhs._angdisplacement) { in >> x; }
    in >> rhs._hull;
    in >> rhs._mass;
    return in;
}

template class vtb::core::Ship<VTB_REAL_TYPE>;

template std::ostream&
vtb::core::operator<<(std::ostream& out, const Ship<VTB_REAL_TYPE>& rhs);

template vtb::core::bstream&
vtb::core::operator<<(bstream& out, const Ship<VTB_REAL_TYPE>& rhs);

template vtb::core::bstream&
vtb::core::operator>>(bstream& in, Ship<VTB_REAL_TYPE>& rhs);