diff --git a/config-example.toml b/config-example.toml index 961febf..9018b40 100644 --- a/config-example.toml +++ b/config-example.toml @@ -1,11 +1,11 @@ [simulation] dt = 0.01 -output.dt = 0.1 +output.dt = 0.01 [launcher] length = 5 -angle = 86 -# angle.start = 86 +angle = 85 +# angle.start = 85 # angle.end = 89 [rocket] @@ -13,9 +13,12 @@ name = "example rocket" type = "hybrid" [[rocket.stage]] -engine = "larkspur_xp_300.resm" +engine = "K240.eng" length = 1.1540 diameter= 0.112 mass = 5.679 -Cd = 0.450 +Cd = 0.440 +Cna = 7.840 +lcg0 = 0.913 +lcgf = 0.905 parachute= {condition = "top", delay = 2.5} diff --git a/src/coordinate/earth.hpp b/src/coordinate/earth.hpp index 6df1a63..e1324ba 100644 --- a/src/coordinate/earth.hpp +++ b/src/coordinate/earth.hpp @@ -22,7 +22,7 @@ #ifndef COORDINATE_EARTH_HPP_ #define COORDINATE_EARTH_HPP_ -#include "../environment/earth.hpp" +#include "../environment/earth/ellipsoid.hpp" namespace trochia::coordinate::earth { using namespace environment::earth; diff --git a/src/coordinate/local.hpp b/src/coordinate/local.hpp index c711a07..66473a5 100644 --- a/src/coordinate/local.hpp +++ b/src/coordinate/local.hpp @@ -29,42 +29,42 @@ namespace trochia::coordinate::local { Vector3 vec; - inline auto operator+(const Base &b) const -> const Base { + inline auto operator+(const Base &b) const -> Base { Base ret; ret.vec = this->vec + b.vec; return ret; } - inline auto operator-(const Base &b) const -> const Base { + inline auto operator-(const Base &b) const -> Base { Base ret; ret.vec = this->vec - b.vec; return ret; } - inline auto operator*(const math::Float &b) const -> const Base { + inline auto operator*(const math::Float &b) const -> Base { Base ret; ret.vec = this->vec * b; return ret; } - inline auto operator/(const math::Float &b) const -> const Base { + inline auto operator/(const math::Float &b) const -> Base { Base ret; ret.vec = this->vec / b; return ret; } - inline auto operator+=(const Base &b) -> const Base& { + inline auto operator+=(const Base &b) -> Base& { this->vec += b.vec; return *this; } - inline auto operator-=(const Base &b) -> const Base& { + inline auto operator-=(const Base &b) -> Base& { this->vec -= b.vec; return *this; } - inline auto operator*=(const math::Float &b) -> const Base& { + inline auto operator*=(const math::Float &b) -> Base& { this->vec *= b; return *this; } - inline auto operator/=(const math::Float &b) -> const Base& { + inline auto operator/=(const math::Float &b) -> Base& { this->vec /= b; return *this; } @@ -74,23 +74,23 @@ namespace trochia::coordinate::local { public: NED() : Base() {} - auto north() const -> const Float { return vec.x(); } - auto east() const -> const Float { return vec.y(); } - auto down() const -> const Float { return vec.z(); } + auto north() const -> Float { return vec.x(); } + auto east() const -> Float { return vec.y(); } + auto down() const -> Float { return vec.z(); } - auto south() const -> const Float { return -1.0*north(); } - auto west() const -> const Float { return -1.0*east(); } - auto up() const -> const Float { return -1.0*down(); } + auto south() const -> Float { return -1.0*north(); } + auto west() const -> Float { return -1.0*east(); } + auto up() const -> Float { return -1.0*down(); } - auto altitude() const -> const Float { return up(); } + auto altitude() const -> Float { return up(); } - auto north(const Float &n) -> void{ vec.x() = n; } - auto east(const Float &e) -> void{ vec.y() = e; } - auto down(const Float &d) -> void{ vec.z() = d; } + auto north(const Float &n) -> void { vec.x() = n; } + auto east(const Float &e) -> void { vec.y() = e; } + auto down(const Float &d) -> void { vec.z() = d; } - auto south(const Float &s) -> void{ north(-1.0*s); } - auto west(const Float &w) -> void{ east(-1.0*w); } - auto up(const Float &u) -> void{ down(-1.0*u); } + auto south(const Float &s) -> void { north(-1.0*s); } + auto west(const Float &w) -> void { east(-1.0*w); } + auto up(const Float &u) -> void { down(-1.0*u); } template operator Frame() { @@ -106,19 +106,19 @@ namespace trochia::coordinate::local { public: ENU() : Base() {} - auto east() const -> const Float { return vec.x(); } - auto north() const -> const Float { return vec.y(); } - auto up() const -> const Float { return vec.z(); } + auto east() const -> Float { return vec.x(); } + auto north() const -> Float { return vec.y(); } + auto up() const -> Float { return vec.z(); } - auto west() const -> const Float { return -1.0*east(); } - auto south() const -> const Float { return -1.0*north(); } - auto down() const -> const Float { return -1.0*up(); } + auto west() const -> Float { return -1.0*east(); } + auto south() const -> Float { return -1.0*north(); } + auto down() const -> Float { return -1.0*up(); } - auto altitude() const -> const Float { return up(); } + auto altitude() const -> Float { return up(); } - auto east(const Float &e) -> void{ vec.x() = e; } - auto north(const Float &n) -> void{ vec.y() = n; } - auto up(const Float &u) -> void{ vec.z() = u; } + auto east(const Float &e) -> void { vec.x() = e; } + auto north(const Float &n) -> void { vec.y() = n; } + auto up(const Float &u) -> void { vec.z() = u; } template operator Frame() { diff --git a/src/engine.hpp b/src/engine.hpp index e6c3c1b..885f24c 100644 --- a/src/engine.hpp +++ b/src/engine.hpp @@ -82,16 +82,16 @@ namespace trochia { itr = data.cbegin(); } - inline auto progress(const math::Float &time) const -> const math::Float { + inline auto progress(const math::Float &time) const -> math::Float { return time / time_max; } - inline auto weight(const math::Float &time) const -> const math::Float { + inline auto weight(const math::Float &time) const -> math::Float { const auto prop = math::lerp(weight_prop, 0, this->progress(time)); return weight_total - weight_prop + prop; } - auto thrust(const math::Float &time) -> const math::Float { + auto thrust(const math::Float &time) -> math::Float { const auto &next = itr+1; const auto &time_now = itr->first; const auto &thrust_now = itr->second; diff --git a/src/environment.hpp b/src/environment.hpp new file mode 100644 index 0000000..82fc587 --- /dev/null +++ b/src/environment.hpp @@ -0,0 +1,32 @@ +/* ----------------------------------------------------------------------- * + * + * Copyright (C) 2019 sksat + * + * This file is part of Trochia. + * + * Trochia 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. + * + * Trochia 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 Trochia. If not, see . + * + * ----------------------------------------------------------------------- */ + +#ifndef ENVIRONMENT_HPP_ +#define ENVIRONMENT_HPP_ + +#include "environment/physics.hpp" // fundamental physical constants +#include "environment/temperature.hpp" +#include "environment/earth.hpp" +#include "environment/gravity.hpp" +#include "environment/air.hpp" +#include "environment/launcher.hpp" + +#endif diff --git a/src/environment/air.hpp b/src/environment/air.hpp index 55718f9..21f1753 100644 --- a/src/environment/air.hpp +++ b/src/environment/air.hpp @@ -23,8 +23,26 @@ #define ENVIRONMENT_AIR_HPP_ #include "../math.hpp" +#include "temperature.hpp" -namespace environment::air { +namespace trochia::environment::air { + // from FROGS (return kelvin) + auto temperature(const math::Float &height) -> temperature::kelvin { + const auto height_km = height * 0.001; + return temperature::celsius(15.0 - 6.5*height_km); + } + + // from FROGS (return Pa) + auto pressure(const temperature::kelvin &t) -> math::Float { + const math::Float t_ = t; + return 101325.0 * std::pow((288.15 / t_), -5.256); + } + + // from FROGS (Don't use this equation over 11km) + auto density(const temperature::kelvin &t) -> math::Float { + const math::Float t_ = t; + return (0.0034837 * pressure(t)) / t_; + } } #endif diff --git a/src/environment/earth.hpp b/src/environment/earth.hpp index 0ba84ff..e14dae3 100644 --- a/src/environment/earth.hpp +++ b/src/environment/earth.hpp @@ -25,61 +25,11 @@ #include "../math.hpp" namespace trochia::environment::earth { - namespace ellipsoid { - using math::Float; - - class Ellipsoid { - public: - constexpr Ellipsoid(Float a, Float f_inv) : a(a), f_inv(f_inv), - f(0.0), b(0.0), e(0.0), e2(0.0) - { - calc_parameters(); - } - - constexpr auto calc_parameters() -> void { - // https://psgsv2.gsi.go.jp/koukyou/jyunsoku/pdf/h28/h28_junsoku_furoku6.pdf - f = 1.0 / f_inv; - b = a * (f_inv - 1.0) / f_inv; - - const auto f_tmp = 2.0 * f_inv - 1.0; - e = math::sqrt(f_tmp) / f_inv; - e2= f_tmp / (f_inv * f_inv); - } - - constexpr auto W(const Float &lat) const -> const Float { - const auto sin = math::sin(lat); - const auto sin2 = sin*sin; - return math::sqrt(1.0 - e2*sin2); - } - - constexpr auto N(const Float &lat) const -> const Float { - return a / W(lat); - } - - constexpr auto M(const Float &lat) const -> const Float { - const auto w = W(lat); - const auto W2= w*w; - return N(lat) * (1.0 - e2) / W2; - } - - Float a, f_inv; - Float f, b, e, e2; - }; - - // https://www.jmu.edu/cisr/research/sic/standards/ellipsoid.htm - constexpr auto Airy_1830 = Ellipsoid(6'377'563, 299.33); - constexpr auto Everest_1830 = Ellipsoid(6'377'276.3, 300.80); - constexpr auto Bessel_1841 = Ellipsoid(6'377'397.2, 299.15); - constexpr auto Clarke_1866 = Ellipsoid(6'378'206.4, 294.98); - constexpr auto Clarke_1880 = Ellipsoid(6'378'249.2, 293.47); - constexpr auto International_1924 = Ellipsoid(6'378'388, 297); - constexpr auto Krasovsky_1924 = Ellipsoid(6'378'245, 298.3); - constexpr auto International_Astronomical_Union_1968 - = Ellipsoid(6'378'160, 298.25); - constexpr auto WGS72 = Ellipsoid(6'378'135, 298.26); - constexpr auto GRS80 = Ellipsoid(6'378'137, 298.26); - constexpr auto WGS84 = Ellipsoid(GRS80.a, 298.25722); - } + // Chronological Scientific Tables 2014 + constexpr math::Float re = 6.3781366e6; } +#include "earth/ellipsoid.hpp" +#include "earth/geodesy.hpp" + #endif diff --git a/src/environment/earth/ellipsoid.hpp b/src/environment/earth/ellipsoid.hpp new file mode 100644 index 0000000..2e80077 --- /dev/null +++ b/src/environment/earth/ellipsoid.hpp @@ -0,0 +1,85 @@ +/* ----------------------------------------------------------------------- * + * + * Copyright (C) 2019 sksat + * + * This file is part of Trochia. + * + * Trochia 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. + * + * Trochia 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 Trochia. If not, see . + * + * ----------------------------------------------------------------------- */ + +#ifndef ENVIRONMENT_EARTH_ELLIPSOID_HPP_ +#define ENVIRONMENT_EARTH_ELLIPSOID_HPP_ + +#include "../../math.hpp" + +namespace trochia::environment::earth { + namespace ellipsoid { + using math::Float; + + class Ellipsoid { + public: + constexpr Ellipsoid(Float a, Float f_inv) : a(a), f_inv(f_inv), + f(0.0), b(0.0), e(0.0), e2(0.0) + { + calc_parameters(); + } + + constexpr auto calc_parameters() -> void { + // https://psgsv2.gsi.go.jp/koukyou/jyunsoku/pdf/h28/h28_junsoku_furoku6.pdf + f = 1.0 / f_inv; + b = a * (f_inv - 1.0) / f_inv; + + const auto f_tmp = 2.0 * f_inv - 1.0; + e = math::sqrt(f_tmp) / f_inv; + e2= f_tmp / (f_inv * f_inv); + } + + constexpr auto W(const Float &lat) const -> Float { + const auto sin = math::sin(lat); + const auto sin2 = sin*sin; + return math::sqrt(1.0 - e2*sin2); + } + + constexpr auto N(const Float &lat) const -> Float { + return a / W(lat); + } + + constexpr auto M(const Float &lat) const -> Float { + const auto w = W(lat); + const auto W2= w*w; + return N(lat) * (1.0 - e2) / W2; + } + + Float a, f_inv; + Float f, b, e, e2; + }; + + // https://www.jmu.edu/cisr/research/sic/standards/ellipsoid.htm + constexpr auto Airy_1830 = Ellipsoid(6'377'563, 299.33); + constexpr auto Everest_1830 = Ellipsoid(6'377'276.3, 300.80); + constexpr auto Bessel_1841 = Ellipsoid(6'377'397.2, 299.15); + constexpr auto Clarke_1866 = Ellipsoid(6'378'206.4, 294.98); + constexpr auto Clarke_1880 = Ellipsoid(6'378'249.2, 293.47); + constexpr auto International_1924 = Ellipsoid(6'378'388, 297); + constexpr auto Krasovsky_1924 = Ellipsoid(6'378'245, 298.3); + constexpr auto International_Astronomical_Union_1968 + = Ellipsoid(6'378'160, 298.25); + constexpr auto WGS72 = Ellipsoid(6'378'135, 298.26); + constexpr auto GRS80 = Ellipsoid(6'378'137, 298.26); + constexpr auto WGS84 = Ellipsoid(GRS80.a, 298.25722); + } +} + +#endif diff --git a/src/environment/earth/geodesy.hpp b/src/environment/earth/geodesy.hpp new file mode 100644 index 0000000..33538d4 --- /dev/null +++ b/src/environment/earth/geodesy.hpp @@ -0,0 +1,41 @@ +/* ----------------------------------------------------------------------- * + * + * Copyright (C) 2019 sksat + * + * This file is part of Trochia. + * + * Trochia 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. + * + * Trochia 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 Trochia. If not, see . + * + * ----------------------------------------------------------------------- */ + +#ifndef ENVIRONMENT_EARTH_GEODESY_HPP_ +#define ENVIRONMENT_EARTH_GEODESY_HPP_ + +#include "../earth.hpp" + +namespace trochia::environment::earth::geodesy { + // geopotential height + // U.S. Standard Atmosphere, 1976: https://ntrs.nasa.gov/search.jsp?R=19770009539 + auto potential_height(const math::Float &height) -> math::Float { + // the adopted, effective earth's radius, 6356.766km + // used for computing g(Z) for 45-degree north latitude, + // and used for relating H and Z at that latitude + constexpr math::Float r0 = 6356766; + constexpr math::Float gamma = 1.0; + + return (r0 * height) / (gamma * r0 - height); + } +} + +#endif diff --git a/src/environment/gravity.hpp b/src/environment/gravity.hpp index bf9a17d..b76c4a2 100644 --- a/src/environment/gravity.hpp +++ b/src/environment/gravity.hpp @@ -23,27 +23,18 @@ #define ENVIRONMENT_GRAVITY_HPP_ #include "../math.hpp" +#include "physics.hpp" +#include "earth.hpp" namespace trochia::environment { - namespace constant { - // 2018 CODATA recommended values - constexpr math::Float G = 6.6743015e-11; - - // standard acceleration of gravity(definition) - constexpr math::Float gn = 9.80665; - - // Chronological Scientific Tables 2014 - constexpr math::Float re = 6.3781366e6; - } - // TODO // latitude model(Helmert equation, WGS-84) // gravitation model // other planet - inline auto gravity(const math::Float &altitude) -> const math::Float { - const auto tmp = constant::re / (constant::re + altitude); - return constant::gn * tmp * tmp; + inline auto gravity(const math::Float &altitude) -> math::Float { + const auto tmp = earth::re / (earth::re + altitude); + return physics::g0 * tmp * tmp; } } diff --git a/src/environment/launcher.hpp b/src/environment/launcher.hpp index fcde7ca..5186eb9 100644 --- a/src/environment/launcher.hpp +++ b/src/environment/launcher.hpp @@ -36,7 +36,7 @@ namespace trochia::environment { math::Quaternion angle; public: - auto get_angle() const -> const math::Quaternion { return this->angle; } + auto get_angle() const -> math::Quaternion { return this->angle; } auto set_angle(const math::Vector3 &euler) -> void { this->set_angle(euler.x(), euler.y(), euler.z()); diff --git a/src/environment/physics.hpp b/src/environment/physics.hpp new file mode 100644 index 0000000..3f0ceb2 --- /dev/null +++ b/src/environment/physics.hpp @@ -0,0 +1,67 @@ +/* ----------------------------------------------------------------------- * + * + * Copyright (C) 2019 sksat + * + * This file is part of Trochia. + * + * Trochia 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. + * + * Trochia 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 Trochia. If not, see . + * + * ----------------------------------------------------------------------- */ + +#ifndef ENVIRONMENT_PHYSICS_HPP_ +#define ENVIRONMENT_PHYSICS_HPP_ + +#include "../math.hpp" + +namespace trochia::environment::physics { + using math::Float; + + // standard acceleration of gravity(definition value) + // The third General Conference on Weights and Mesures(1901) + constexpr Float g0 = 9.80665; + + inline namespace CODATA2018 { + // 2018 CODATA recommended values + // https://physics.nist.gov/cuu/pdf/wall_2018.pdf + + // speed of light in vacuum + constexpr Float c = 299792458; + + // Newtonian constant of gravitation + constexpr Float G = 6.6743015e-11; + + // molar gas constant + constexpr Float R = 8.314462618; + } + + namespace CODATA2014 { + // 2014 CODATA recommended values + // https://physics.nist.gov/cuu/pdf/wallet_2014.pdf + + constexpr Float c = 299792458; + constexpr Float G = 6.6740831e-11; + constexpr Float R = 8.314459848; + } + + namespace CODATA2010 { + // 2010 CODATA recommended values + // https://physics.nist.gov/cuu/pdf/all_2010.pdf + + constexpr Float c = 299792458; + constexpr Float G = 6.6738480e-11; + constexpr Float R = 8.314462175; + } +} + +#endif diff --git a/src/environment/temperature.hpp b/src/environment/temperature.hpp new file mode 100644 index 0000000..11b93b3 --- /dev/null +++ b/src/environment/temperature.hpp @@ -0,0 +1,54 @@ +/* ----------------------------------------------------------------------- * + * + * Copyright (C) 2019 sksat + * + * This file is part of Trochia. + * + * Trochia 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. + * + * Trochia 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 Trochia. If not, see . + * + * ----------------------------------------------------------------------- */ + +#ifndef ENVIRONMENT_TEMPERATURE_HPP_ +#define ENVIRONMENT_TEMPERATURE_HPP_ + +#include "../math.hpp" + +namespace trochia::environment::temperature { + class thermodynamic { + public: + constexpr thermodynamic() : t(0.0) {} + constexpr thermodynamic(const math::Float &t) : t(t) {} + + virtual operator math::Float() const { + return t; + } + protected: + math::Float t; + }; + + using kelvin = thermodynamic; + + class celsius : public thermodynamic { + public: + constexpr celsius() : thermodynamic(273.15) {} + constexpr celsius(const thermodynamic &t) : thermodynamic(t) {} + constexpr celsius(const math::Float &t) : thermodynamic(t + 273.15) {} + + operator math::Float() const { + return this->t - 273.15; + } + }; +} + +#endif diff --git a/src/io/config.cpp b/src/io/config.cpp index ad5b466..076fcd6 100644 --- a/src/io/config.cpp +++ b/src/io/config.cpp @@ -56,8 +56,13 @@ auto load(const std::string &fname, std::vector &sims) -> void { const auto engine = stage[0].at("engine"); sim.rocket.engine.load_eng(engine.as_string()); + sim.rocket.diameter = stage[0].at("diameter").as_floating(); + sim.rocket.lcg0 = stage[0].at("lcg0").as_floating(); sim.rocket.lcgf = stage[0].at("lcgf").as_floating(); + + sim.rocket.Cd = stage[0].at("Cd").as_floating(); + sim.rocket.Cna= stage[0].at("Cna").as_floating(); } sims.push_back(sim); diff --git a/src/math.hpp b/src/math.hpp index 7e42aeb..78550a1 100644 --- a/src/math.hpp +++ b/src/math.hpp @@ -77,15 +77,15 @@ namespace trochia::math { return series; } - constexpr auto deg2rad(const Float °) -> const Float { + constexpr auto deg2rad(const Float °) -> Float { return deg * (pi / 180.0); } - constexpr auto rad2deg(const Float &rad) -> const Float { + constexpr auto rad2deg(const Float &rad) -> Float { return rad * (180.0 / pi); } [[deprecated("please use AngleAxis")]] - inline auto euler2quat(const Float &roll, const Float &pitch, const Float &yaw) -> const Quaternion { + inline auto euler2quat(const Float &roll, const Float &pitch, const Float &yaw) -> Quaternion { using Eigen::AngleAxisd; Quaternion q; @@ -95,11 +95,11 @@ namespace trochia::math { return q; } - inline auto quat2euler(const Quaternion &q) -> const Vector3 { + inline auto quat2euler(const Quaternion &q) -> Vector3 { return q.toRotationMatrix().eulerAngles(0, 1, 2); } - inline auto lerp(const Float &a, const Float &b, const Float &t) -> const Float { + inline auto lerp(const Float &a, const Float &b, const Float &t) -> Float { return a + t*(b - a); } } diff --git a/src/object.hpp b/src/object.hpp index af7a949..ebbadf6 100644 --- a/src/object.hpp +++ b/src/object.hpp @@ -42,7 +42,7 @@ namespace trochia::object { return ret; } - virtual auto weight() const -> const math::Float { + virtual auto weight() const -> math::Float { return 0.0; } @@ -52,45 +52,45 @@ namespace trochia::object { } // operator - auto operator+=(const Object &o) -> const Object { + auto operator+=(const Object &o) -> Object { this->pos += o.pos; this->vel += o.vel; return *this; } - auto operator-=(const Object &o) -> const Object { + auto operator-=(const Object &o) -> Object { this->pos -= o.pos; this->vel -= o.vel; return *this; } - auto operator*=(const math::Float &a) -> const Object { + auto operator*=(const math::Float &a) -> Object { this->pos *= a; this->vel *= a; return *this; } - auto operator/=(const math::Float &a) -> const Object { + auto operator/=(const math::Float &a) -> Object { this->pos /= a; this->vel /= a; return *this; } - auto operator+(const Object &o) const -> const Object { + auto operator+(const Object &o) const -> Object { Object ret = *this; ret += o; return ret; } - auto operator-(const Object &o) const -> const Object { + auto operator-(const Object &o) const -> Object { Object ret = *this; ret -= o; return ret; } - auto operator*(const math::Float &a) const -> const Object { + auto operator*(const math::Float &a) const -> Object { Object ret = *this; ret *= a; return ret; } - auto operator/(const math::Float &a) const -> const Object { + auto operator/(const math::Float &a) const -> Object { Object ret = *this; ret /= a; return ret; diff --git a/src/rocket.hpp b/src/rocket.hpp index d804c7d..4badab6 100644 --- a/src/rocket.hpp +++ b/src/rocket.hpp @@ -41,17 +41,20 @@ namespace trochia::rocket { std::string name; Engine engine; + math::Float diameter; // 直径(m) math::Float lcg0, lcgf; // 重心位置 + math::Float Cd; // 抗力係数 + math::Float Cna; static auto dx(const math::Float &t, const Rocket &r) -> const Rocket { return object::Object::dx(t, r); } - auto lcg() const -> const math::Float { + auto lcg() const -> math::Float { return math::lerp(lcg0, lcgf, engine.progress(this->time)); } - auto weight() const -> const math::Float { + auto weight() const -> math::Float { return 10.0 + engine.weight(this->time); } }; diff --git a/src/simulation.cpp b/src/simulation.cpp index 0e38782..cf85ff0 100644 --- a/src/simulation.cpp +++ b/src/simulation.cpp @@ -23,8 +23,7 @@ #include "math.hpp" #include "simulation.hpp" #include "solver.hpp" -#include "environment/launcher.hpp" -#include "environment/gravity.hpp" +#include "environment.hpp" auto trochia::do_simulation(Simulation &sim) -> void { const auto &timeout = sim.timeout; @@ -62,13 +61,39 @@ auto trochia::do_simulation(Simulation &sim) -> void { const auto &time = s.t; rocket.time = time; + const coordinate::local::NED vel_ned = rocket.vel; + const auto vab_ned = vel_ned.vec; + + // Body座標系での対気速度ベクトル + const auto vab = coordinate::dcm::ned2body(rocket.quat) * vab_ned; + const auto va = vab.norm(); + + // tan(attack) = z/x + // arctan(z/x) = attack + const auto angle_attack = (vab.x()!=0.0 ? std::atan(vab.z() / vab.x()) : 0.5*math::pi); + const auto angle_side_slip = (va!=0.0 ? std::atan(vab.y() / va) : 0.5*math::pi); + + // 代表面積 + const auto S = rocket.diameter * rocket.diameter * math::pi / 4; + + const auto altitude = rocket.pos.altitude(); + const auto geo_height = environment::earth::geodesy::potential_height(altitude); + const auto temperature = environment::air::temperature(geo_height); + const auto rho = environment::air::density(temperature); + + // 空気抵抗 + const auto q = 0.5 * rho * va * va; // 動圧 + const auto D = rocket.Cd * q * S; + const auto Y = rocket.Cna * q * S * std::sin(angle_side_slip); + const auto N = rocket.Cna * q * S * std::sin(angle_attack); + // thrust const auto thrust = rocket.engine.thrust(time); // first stage only const auto force = coordinate::body::Body( - thrust, - 0.0, - 0.0 + thrust - D, + -1.0 * Y, + -1.0 * N ); rocket.force(force.to_local(rocket.quat)); @@ -87,9 +112,19 @@ auto trochia::do_simulation(Simulation &sim) -> void { // log if(step % output_rate == 0){ + const auto altitude = rocket.pos.altitude(); + const auto geo_height = environment::earth::geodesy::potential_height(altitude); + const auto temperature = environment::air::temperature(geo_height); + std::cout << time << " " - << rocket.pos.altitude() << " " - << environment::gravity(rocket.pos.altitude()) << std::endl; + << altitude << " " + << geo_height << " " + << (math::Float)environment::temperature::celsius(temperature) << " " + << environment::air::pressure(temperature) / 100.0 << " " + << environment::air::density(temperature) << " " + << angle_attack << " " + << Y + << std::endl; } if(step > 100 && rocket.pos.altitude() <= 0.0)