+double Body::Inertia() const noexcept {
+ // assume solid sphere for now
+ return (2.0/5.0) * Mass() * pow(Radius(), 2);
+}
+
+double Body::GravitationalParameter() const noexcept {
+ return G * Mass();
+}
+
+double Body::OrbitalPeriod() const noexcept {
+ if (parent) {
+ return PI_2p0 * sqrt(pow(orbit.SemiMajorAxis(), 3) / (G * (parent->Mass() + Mass())));
+ } else {
+ return 0.0;
+ }
+}
+
+double Body::RotationalPeriod() const noexcept {
+ if (std::abs(angular) < std::numeric_limits<double>::epsilon()) {
+ return std::numeric_limits<double>::infinity();
+ } else {
+ return PI_2p0 * Inertia() / angular;
+ }
+}
+
+glm::dmat4 Body::ToUniverse() const noexcept {
+ glm::dmat4 m(1.0);
+ const Body *b = this;
+ while (b->HasParent()) {
+ m = b->ToParent() * m;
+ b = &b->Parent();
+ }
+ return m;
+}
+
+glm::dmat4 Body::FromUniverse() const noexcept {
+ glm::dmat4 m(1.0);
+ const Body *b = this;
+ while (b->HasParent()) {
+ m *= b->FromParent();
+ b = &b->Parent();
+ }
+ return m;
+}
+
+namespace {
+std::vector<creature::Creature *> ccache;
+std::vector<CreatureCreatureCollision> collisions;
+}
+
+void Body::Tick(double dt) {
+ rotation += dt * AngularMomentum() / Inertia();
+ Cache();
+ ccache = Creatures();
+ for (creature::Creature *c : ccache) {
+ c->Tick(dt);
+ }
+ // first remove creatures so they don't collide
+ for (auto c = Creatures().begin(); c != Creatures().end();) {
+ if ((*c)->Removable()) {
+ delete *c;
+ c = Creatures().erase(c);
+ } else {
+ ++c;
+ }
+ }
+ CheckCollision();
+}
+
+void Body::Cache() noexcept {
+ if (parent) {
+ orbital =
+ orbit.Matrix(PI_2p0 * (GetSimulation().Time() / OrbitalPeriod()))
+ * glm::eulerAngleXY(axis_tilt.x, axis_tilt.y);
+ inverse_orbital =
+ glm::eulerAngleYX(-axis_tilt.y, -axis_tilt.x)
+ * orbit.InverseMatrix(PI_2p0 * (GetSimulation().Time() / OrbitalPeriod()));
+ } else {
+ orbital = glm::eulerAngleXY(axis_tilt.x, axis_tilt.y);
+ inverse_orbital = glm::eulerAngleYX(-axis_tilt.y, -axis_tilt.x);
+ }
+ local =
+ glm::eulerAngleY(rotation)
+ * glm::eulerAngleXY(surface_tilt.x, surface_tilt.y);
+ inverse_local =
+ glm::eulerAngleYX(-surface_tilt.y, -surface_tilt.x)
+ * glm::eulerAngleY(-rotation);
+}
+
+void Body::CheckCollision() noexcept {
+ if (Creatures().size() < 2) return;
+ collisions.clear();
+ auto end = Creatures().end();
+ for (auto i = Creatures().begin(); i != end; ++i) {
+ math::AABB i_box((*i)->CollisionBox());
+ glm::dmat4 i_mat((*i)->CollisionTransform());
+ for (auto j = (i + 1); j != end; ++j) {
+ glm::dvec3 diff((*i)->GetSituation().Position() - (*j)->GetSituation().Position());
+ double max_dist = ((*i)->Size() + (*j)->Size()) * 1.74;
+ if (length2(diff) > max_dist * max_dist) continue;
+ math::AABB j_box((*j)->CollisionBox());
+ glm::dmat4 j_mat((*j)->CollisionTransform());
+ glm::dvec3 normal;
+ double depth;
+ if (Intersect(i_box, i_mat, j_box, j_mat, normal, depth)) {
+ collisions.push_back({ **i, **j, normal, depth });
+ }
+ }
+ }
+ for (auto &c : collisions) {
+ c.A().GetSituation().Move(c.Normal() * (c.Depth() * -0.5));
+ c.B().GetSituation().Move(c.Normal() * (c.Depth() * 0.5));
+ c.A().GetSituation().Accelerate(c.Normal() * -dot(c.Normal(), c.AVel()));
+ c.B().GetSituation().Accelerate(c.Normal() * -dot(c.Normal(), c.BVel()));
+ // TODO: notify participants so they can be annoyed
+ }
+}
+
+void Body::AddCreature(creature::Creature *c) {
+ creatures.push_back(c);
+}
+
+void Body::RemoveCreature(creature::Creature *c) {
+ auto entry = std::find(creatures.begin(), creatures.end(), c);
+ if (entry != creatures.end()) {
+ creatures.erase(entry);
+ }
+}
+
+
+CreatureCreatureCollision::~CreatureCreatureCollision() {
+}
+
+const glm::dvec3 &CreatureCreatureCollision::APos() const noexcept {
+ return a->GetSituation().Position();
+}
+
+const glm::dvec3 &CreatureCreatureCollision::AVel() const noexcept {
+ return a->GetSituation().Velocity();
+}
+
+const glm::dvec3 &CreatureCreatureCollision::BPos() const noexcept {
+ return b->GetSituation().Position();
+}
+
+const glm::dvec3 &CreatureCreatureCollision::BVel() const noexcept {
+ return b->GetSituation().Velocity();
+}
+
+
+Orbit::Orbit()
+: sma(1.0)
+, ecc(0.0)
+, inc(0.0)
+, asc(0.0)
+, arg(0.0)
+, mna(0.0) {
+}
+
+Orbit::~Orbit() {
+}
+
+double Orbit::SemiMajorAxis() const noexcept {
+ return sma;
+}
+
+Orbit &Orbit::SemiMajorAxis(double s) noexcept {
+ sma = s;
+ return *this;
+}
+
+double Orbit::Eccentricity() const noexcept {
+ return ecc;
+}
+
+Orbit &Orbit::Eccentricity(double e) noexcept {
+ ecc = e;
+ return *this;
+}
+
+double Orbit::Inclination() const noexcept {
+ return inc;
+}
+
+Orbit &Orbit::Inclination(double i) noexcept {
+ inc = i;
+ return *this;
+}
+
+double Orbit::LongitudeAscending() const noexcept {
+ return asc;
+}
+
+Orbit &Orbit::LongitudeAscending(double l) noexcept {
+ asc = l;
+ return *this;
+}
+
+double Orbit::ArgumentPeriapsis() const noexcept {
+ return arg;
+}
+
+Orbit &Orbit::ArgumentPeriapsis(double a) noexcept {
+ arg = a;
+ return *this;
+}
+
+double Orbit::MeanAnomaly() const noexcept {
+ return mna;
+}
+
+Orbit &Orbit::MeanAnomaly(double m) noexcept {
+ mna = m;
+ return *this;
+}
+
+namespace {
+
+double mean2eccentric(double M, double e) {
+ double E = M; // eccentric anomaly, solve M = E - e sin E
+ // limit to 100 steps to prevent deadlocks in impossible situations
+ for (int i = 0; i < 100; ++i) {
+ double dE = (E - e * sin(E) - M) / (1 - e * cos(E));
+ E -= dE;
+ if (abs(dE) < 1.0e-6) break;
+ }
+ return E;
+}
+
+}
+
+glm::dmat4 Orbit::Matrix(double t) const noexcept {
+ double M = mna + t;
+ double E = mean2eccentric(M, ecc);
+
+ // coordinates in orbital plane, P=x, Q=-z
+ double P = sma * (cos(E) - ecc);
+ double Q = sma * sin(E) * sqrt(1 - (ecc * ecc));
+
+ return glm::yawPitchRoll(asc, inc, arg) * glm::translate(glm::dvec3(P, 0.0, -Q));
+}
+
+glm::dmat4 Orbit::InverseMatrix(double t) const noexcept {
+ double M = mna + t;
+ double E = mean2eccentric(M, ecc);
+ double P = sma * (cos(E) - ecc);
+ double Q = sma * sin(E) * sqrt(1 - (ecc * ecc));
+ return glm::translate(glm::dvec3(-P, 0.0, Q)) * glm::transpose(glm::yawPitchRoll(asc, inc, arg));
+}
+