diff --git a/CMakeLists.txt b/CMakeLists.txt index 8b94016..7faddc1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,6 @@ cmake_minimum_required(VERSION 3.7) + option(VERSION_STRING "Project version string" "0.0.0") project(antkeeper VERSION ${VERSION_STRING} LANGUAGES CXX) diff --git a/src/ai/steering/agent.hpp b/src/ai/steering/agent.hpp new file mode 100644 index 0000000..9abe7f5 --- /dev/null +++ b/src/ai/steering/agent.hpp @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2021 Christopher J. Howard + * + * This file is part of Antkeeper source code. + * + * Antkeeper source code 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. + * + * Antkeeper source code 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 Antkeeper source code. If not, see . + */ + +#ifndef ANTKEEPER_AI_STEERING_AGENT_HPP +#define ANTKEEPER_AI_STEERING_AGENT_HPP + +#include "utility/fundamental-types.hpp" +#include "math/quaternion-type.hpp" + +namespace ai { +namespace steering { + +/** + * Autonomous agent governed by steering behaviors. + */ +struct agent +{ + /// Mass of the agent. + float mass; + + /// Cartesian position vector. + float3 position; + + /// Velocity vector. + float3 velocity; + + /// Acceleration vector. + float3 acceleration; + + /// Maximum force. + float max_force; + + /// Maximum speed. + float max_speed; + + /// Maximum speed squared. + float max_speed_squared; + + /// Orientation quaternion. + math::quaternion orientation; + + /// Orthonormal basis forward direction vector. + float3 forward; + + /// Orthonormal basis up direction vector. + float3 up; +}; + +} // namespace steering +} // namespace ai + +#endif // ANTKEEPER_AI_STEERING_AGENT_HPP diff --git a/src/ai/steering/behavior/flee.cpp b/src/ai/steering/behavior/flee.cpp new file mode 100644 index 0000000..f5db702 --- /dev/null +++ b/src/ai/steering/behavior/flee.cpp @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2021 Christopher J. Howard + * + * This file is part of Antkeeper source code. + * + * Antkeeper source code 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. + * + * Antkeeper source code 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 Antkeeper source code. If not, see . + */ + +#include "ai/steering/behavior/flee.hpp" + +namespace ai { +namespace steering { +namespace behavior { + +float3 flee(const agent& agent, const float3& target) +{ + float3 force = {0, 0, 0}; + const float3 difference = target - agent.position; + const float distance_squared = math::dot(difference, difference); + + if (distance_squared) + { + const float inverse_distance = 1.0f / std::sqrt(distance_squared); + force = difference * inverse_distance * agent.max_force; + force = agent.velocity - force; + } + + return force; +} + +} // namespace behavior +} // namespace steering +} // namespace ai diff --git a/src/ai/steering/behavior/flee.hpp b/src/ai/steering/behavior/flee.hpp new file mode 100644 index 0000000..a4c0343 --- /dev/null +++ b/src/ai/steering/behavior/flee.hpp @@ -0,0 +1,43 @@ +/* + * Copyright (C) 2021 Christopher J. Howard + * + * This file is part of Antkeeper source code. + * + * Antkeeper source code 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. + * + * Antkeeper source code 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 Antkeeper source code. If not, see . + */ + +#ifndef ANTKEEPER_AI_STEERING_BEHAVIOR_FLEE_HPP +#define ANTKEEPER_AI_STEERING_BEHAVIOR_FLEE_HPP + +#include "ai/steering/agent.hpp" +#include "utility/fundamental-types.hpp" + +namespace ai { +namespace steering { +namespace behavior { + +/** + * Attempts to steer an agent so that it moves away from a target. + * + * @param agent Autonomous agent to steer. + * @param target Target position. + * @return Flee force. + */ +float3 flee(const agent& agent, const float3& target); + +} // namespace behavior +} // namespace steering +} // namespace ai + +#endif // ANTKEEPER_AI_STEERING_BEHAVIOR_FLEE_HPP diff --git a/src/ai/steering/behavior/seek.cpp b/src/ai/steering/behavior/seek.cpp new file mode 100644 index 0000000..41e8478 --- /dev/null +++ b/src/ai/steering/behavior/seek.cpp @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2021 Christopher J. Howard + * + * This file is part of Antkeeper source code. + * + * Antkeeper source code 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. + * + * Antkeeper source code 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 Antkeeper source code. If not, see . + */ + +#include "ai/steering/behavior/seek.hpp" + +namespace ai { +namespace steering { +namespace behavior { + +float3 seek(const agent& agent, const float3& target) +{ + float3 force = {0, 0, 0}; + const float3 difference = target - agent.position; + const float distance_squared = math::dot(difference, difference); + + if (distance_squared) + { + const float inverse_distance = 1.0f / std::sqrt(distance_squared); + force = difference * inverse_distance * agent.max_force; + force -= agent.velocity; + } + + return force; +} + +} // namespace behavior +} // namespace steering +} // namespace ai diff --git a/src/ai/steering/behavior/seek.hpp b/src/ai/steering/behavior/seek.hpp new file mode 100644 index 0000000..242d90e --- /dev/null +++ b/src/ai/steering/behavior/seek.hpp @@ -0,0 +1,43 @@ +/* + * Copyright (C) 2021 Christopher J. Howard + * + * This file is part of Antkeeper source code. + * + * Antkeeper source code 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. + * + * Antkeeper source code 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 Antkeeper source code. If not, see . + */ + +#ifndef ANTKEEPER_AI_STEERING_BEHAVIOR_SEEK_HPP +#define ANTKEEPER_AI_STEERING_BEHAVIOR_SEEK_HPP + +#include "ai/steering/agent.hpp" +#include "utility/fundamental-types.hpp" + +namespace ai { +namespace steering { +namespace behavior { + +/** + * Attempts to steer an agent so that it moves toward a target. + * + * @param agent Autonomous agent to steer. + * @param target Target position. + * @return Seek force. + */ +float3 seek(const agent& agent, const float3& target); + +} // namespace behavior +} // namespace steering +} // namespace ai + +#endif // ANTKEEPER_AI_STEERING_BEHAVIOR_SEEK_HPP diff --git a/src/ai/steering/behavior/wander.cpp b/src/ai/steering/behavior/wander.cpp new file mode 100644 index 0000000..f412a46 --- /dev/null +++ b/src/ai/steering/behavior/wander.cpp @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2021 Christopher J. Howard + * + * This file is part of Antkeeper source code. + * + * Antkeeper source code 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. + * + * Antkeeper source code 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 Antkeeper source code. If not, see . + */ + +#include "ai/steering/behavior/wander.hpp" +#include "ai/steering/behavior/seek.hpp" +#include "math/random.hpp" +#include "math/quaternion-functions.hpp" +#include "math/quaternion-operators.hpp" + +namespace ai { +namespace steering { +namespace behavior { + +float3 wander_2d(const agent& agent, float noise, float distance, float radius, float& angle) +{ + // Shift wander angle + angle += math::random(-noise, noise); + + // Calculate center of wander circle + const float3 center = agent.position + agent.forward * distance; + + // Decompose orientation into swing and twist rotations + math::quaternion swing, twist; + math::swing_twist(agent.orientation, agent.up, swing, twist); + + // Calculate offset to point on wander circle + const float3 offset = math::conjugate(twist) * (math::angle_axis(angle, agent.up) * agent.forward * radius); + + // Seek toward point on wander circle + return seek(agent, center + offset); +} + +/* +float3 wander_3d(const agent& agent, float distance, float radius, float delta, float& theta, float& phi) +{ + // Shift wander angles + theta += random(-delta, delta); + phi += random(-delta, delta); + + // Calculate center of wander sphere + float3 center = agent.position; + const float speed_squared = math::dot(agent.velocity, agent.velocity); + if (speed_squared) + center += (agent.velocity / std::sqrt(speed_squared)) * distance; + + // Convert spherical coordinates to Cartesian point on wander sphere + const float r_cos_theta = radius * std::cos(theta); + const float3 offset = + { + r_cos_theta * std::cos(phi), + r_cos_theta * std::sin(phi), + radius * std::sin(theta) + }; + + // Seek toward point on wander sphere + return seek(agent, center + offset); +} +*/ + +} // namespace behavior +} // namespace steering +} // namespace ai diff --git a/src/ai/steering/behavior/wander.hpp b/src/ai/steering/behavior/wander.hpp new file mode 100644 index 0000000..ea0f840 --- /dev/null +++ b/src/ai/steering/behavior/wander.hpp @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2021 Christopher J. Howard + * + * This file is part of Antkeeper source code. + * + * Antkeeper source code 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. + * + * Antkeeper source code 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 Antkeeper source code. If not, see . + */ + +#ifndef ANTKEEPER_AI_STEERING_BEHAVIOR_WANDER_HPP +#define ANTKEEPER_AI_STEERING_BEHAVIOR_WANDER_HPP + +#include "ai/steering/agent.hpp" +#include "utility/fundamental-types.hpp" + +namespace ai { +namespace steering { +namespace behavior { + +/** + * Steers an agent in a continuously shifting random direction on the yaw plane. + * + * @param agent Autonomous agent to steer. + * @param distance Distance to the center of the wander circle. + * @param noise Maximum wander angle shift, in radians. + * @param radius Radius of the wander circle. + * @param[in,out] angle Angular coordinate on the wander circle, in radians. + * + * @return Wander force. + */ +float3 wander_2d(const agent& agent, float noise, float distance, float radius, float& angle); + +/** + * Steers an agent in a continuously shifting random direction. + * + * @param agent Autonomous agent to steer. + * @param distance Distance to the wander sphere. + * @param radius Radius of the wander sphere. + * @param delta Maximum angle offset. + * @param theta Polar wander angle, in radians. + * @param phi Azimuthal wander angle, in radians. + * @param[in,out] Wander angle, in radians. + * + * @return Wander force. + */ +//float3 wander_3d(const agent& agent, float distance, float radius, float delta, float& theta, float& phi); + +} // namespace behavior +} // namespace steering +} // namespace ai + +#endif // ANTKEEPER_AI_STEERING_BEHAVIOR_WANDER_HPP diff --git a/src/ai/steering/steering.hpp b/src/ai/steering/steering.hpp new file mode 100644 index 0000000..71694a2 --- /dev/null +++ b/src/ai/steering/steering.hpp @@ -0,0 +1,34 @@ +/* + * Copyright (C) 2021 Christopher J. Howard + * + * This file is part of Antkeeper source code. + * + * Antkeeper source code 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. + * + * Antkeeper source code 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 Antkeeper source code. If not, see . + */ + +#ifndef ANTKEEPER_AI_STEERING_HPP +#define ANTKEEPER_AI_STEERING_HPP + +namespace ai { + +/** + * Autonomous agent steering. + * + * @see Reynolds, Craig. (2002). Steering Behaviors For Autonomous Characters. + */ +namespace steering {} + +} // namespace ai + +#endif // ANTKEEPER_AI_STEERING_HPP diff --git a/src/entity/components/steering.hpp b/src/entity/components/steering.hpp index 3c915aa..d8eb520 100644 --- a/src/entity/components/steering.hpp +++ b/src/entity/components/steering.hpp @@ -20,21 +20,30 @@ #ifndef ANTKEEPER_ENTITY_COMPONENT_STEERING_HPP #define ANTKEEPER_ENTITY_COMPONENT_STEERING_HPP -#include "utility/fundamental-types.hpp" +#include "ai/steering/agent.hpp" namespace entity { namespace component { struct steering { - float3 velocity; - float3 acceleration; - float max_force; - float max_speed; + /// Steering agent. + ai::steering::agent agent; + + float wander_weight; + float wander_noise; + float wander_distance; + float wander_radius; + float wander_angle; + + float seek_weight; + float3 seek_target; + + float flee_weight; + float weight_sum; }; } // namespace component } // namespace entity #endif // ANTKEEPER_ENTITY_COMPONENT_STEERING_HPP - diff --git a/src/entity/systems/steering.cpp b/src/entity/systems/steering.cpp index 73f1ac4..939d031 100644 --- a/src/entity/systems/steering.cpp +++ b/src/entity/systems/steering.cpp @@ -21,6 +21,9 @@ #include "entity/components/steering.hpp" #include "entity/components/transform.hpp" #include "entity/id.hpp" +#include "ai/steering/behavior/wander.hpp" +#include "ai/steering/behavior/seek.hpp" +#include "config.hpp" namespace entity { namespace system { @@ -31,27 +34,57 @@ steering::steering(entity::registry& registry): void steering::update(double t, double dt) { - registry.view().each( - [&](entity::id entity_id, auto& transform, auto& boid) + registry.view().each + ( + [&](entity::id entity_id, auto& steering, auto& transform) { + auto& agent = steering.agent; + + // Update agent orientation + agent.orientation = transform.local.rotation; + + // Accumulate forces + float3 force = {0, 0, 0}; + if (steering.wander_weight) + { + force += ai::steering::behavior::wander_2d(agent, steering.wander_noise * dt, steering.wander_distance, steering.wander_radius, steering.wander_angle) * steering.wander_weight; + } + if (steering.seek_weight) + { + force += ai::steering::behavior::seek(agent, steering.seek_target) * steering.seek_weight; + } + + // Normalize force + force /= steering.weight_sum; + // Accelerate - boid.velocity += boid.acceleration; - if (math::dot(boid.velocity, boid.velocity) > boid.max_speed * boid.max_speed) + agent.acceleration = force / agent.mass; + agent.velocity += agent.acceleration * static_cast(dt); + + // Limit speed + const float speed_squared = math::length_squared(agent.velocity); + if (speed_squared > agent.max_speed_squared) { - boid.velocity = math::normalize(boid.velocity) * boid.max_speed; + const float speed = std::sqrt(speed_squared); + agent.velocity = (agent.velocity / speed) * agent.max_speed; } - // Clear acceleration - boid.acceleration = {0, 0, 0}; + // Move agent + agent.position += agent.velocity * static_cast(dt); - // Move - transform.local.translation += boid.velocity; - }); -} - -void steering::wander() -{ - + // Rotate agent + if (speed_squared) + { + agent.orientation = math::look_rotation(agent.velocity / std::sqrt(speed_squared), agent.up); + agent.forward = agent.orientation * config::global_forward; + agent.up = agent.orientation * config::global_up; + } + + // Update transform + transform.local.translation = agent.position; + transform.local.rotation = agent.orientation; + } + ); } } // namespace system diff --git a/src/entity/systems/steering.hpp b/src/entity/systems/steering.hpp index 77909e9..f48dc40 100644 --- a/src/entity/systems/steering.hpp +++ b/src/entity/systems/steering.hpp @@ -31,9 +31,6 @@ class steering: public: steering(entity::registry& registry); virtual void update(double t, double dt); - -private: - void wander(); }; } // namespace system diff --git a/src/game/context.hpp b/src/game/context.hpp index bbba11f..f3db354 100644 --- a/src/game/context.hpp +++ b/src/game/context.hpp @@ -97,6 +97,7 @@ namespace entity class render; class samara; class proteome; + class steering; } } @@ -279,6 +280,7 @@ struct context entity::system::collision* collision_system; entity::system::constraint* constraint_system; entity::system::locomotion* locomotion_system; + entity::system::steering* steering_system; entity::system::snapping* snapping_system; entity::system::render* render_system; entity::system::samara* samara_system; diff --git a/src/game/state/boot.cpp b/src/game/state/boot.cpp index fdf47db..ff4d268 100644 --- a/src/game/state/boot.cpp +++ b/src/game/state/boot.cpp @@ -74,6 +74,7 @@ #include "entity/systems/atmosphere.hpp" #include "entity/systems/orbit.hpp" #include "entity/systems/proteome.hpp" +#include "entity/systems/steering.hpp" #include "entity/commands.hpp" #include "utility/paths.hpp" #include "event/event-dispatcher.hpp" @@ -872,6 +873,9 @@ void boot::setup_systems() // Setup locomotion system ctx.locomotion_system = new entity::system::locomotion(*ctx.entity_registry); + // Setup steering system + ctx.steering_system = new entity::system::steering(*ctx.entity_registry); + // Setup spatial system ctx.spatial_system = new entity::system::spatial(*ctx.entity_registry); @@ -1125,6 +1129,7 @@ void boot::setup_loop() ctx.collision_system->update(t, dt); ctx.samara_system->update(t, dt); ctx.behavior_system->update(t, dt); + ctx.steering_system->update(t, dt); ctx.locomotion_system->update(t, dt); ctx.camera_system->update(t, dt); ctx.orbit_system->update(t, dt); diff --git a/src/game/state/nuptial-flight.cpp b/src/game/state/nuptial-flight.cpp index 9cf6ce9..544b44b 100644 --- a/src/game/state/nuptial-flight.cpp +++ b/src/game/state/nuptial-flight.cpp @@ -30,6 +30,7 @@ #include "entity/components/constraints/spring-to.hpp" #include "entity/components/constraints/three-dof.hpp" #include "entity/components/constraint-stack.hpp" +#include "entity/components/steering.hpp" #include "entity/commands.hpp" #include "animation/screen-transition.hpp" #include "animation/ease.hpp" @@ -105,11 +106,54 @@ nuptial_flight::nuptial_flight(game::context& ctx): // Set world time const double utc = 0.0; const double equinox_2000 = physics::time::gregorian::to_ut1(2000, 1, 1, 12, 0, 0.0, utc); - const double spring_2000 = physics::time::gregorian::to_ut1(2000, 6, 19, 12, 0, 0.0, utc); - game::world::set_time(ctx, 14622.5); + const double summer_2022 = physics::time::gregorian::to_ut1(2022, 6, 21, 12, 0, 0.0, utc); + game::world::set_time(ctx, summer_2022); - // Freeze time + // Set time scale game::world::set_time_scale(ctx, 60.0); + + // Create boids + for (int i = 0; i < 20; ++i) + { + entity::id boid_eid = ctx.entity_registry->create(); + + // Create transform component + entity::component::transform transform; + transform.local = math::transform::identity; + transform.world = math::transform::identity; + transform.warp = true; + ctx.entity_registry->assign(boid_eid, transform); + + // Create model component + entity::component::model model; + model.render_model = ctx.resource_manager->load("boid.mdl"); + model.instance_count = 0; + model.layers = 1; + ctx.entity_registry->assign(boid_eid, model); + + // Create steering component + entity::component::steering steering; + steering.agent.mass = 1.0f; + steering.agent.position = {0, 0, 0}; + steering.agent.velocity = {0, 0, 0}; + steering.agent.acceleration = {0, 0, 0}; + steering.agent.max_force = 2.5f; + steering.agent.max_speed = 5.0f; + steering.agent.max_speed_squared = steering.agent.max_speed * steering.agent.max_speed; + steering.agent.orientation = math::quaternion::identity; + steering.agent.forward = steering.agent.orientation * config::global_forward; + steering.agent.up = steering.agent.orientation * config::global_up; + steering.wander_weight = 1.0f; + steering.wander_noise = math::radians(2000.0f); + steering.wander_distance = 10.0f; + steering.wander_radius = 5.0f; + steering.wander_angle = 0.0f; + steering.seek_weight = 0.2f; + steering.seek_target = {0, 0, 0}; + steering.flee_weight = 0.0f; + steering.weight_sum = steering.wander_weight + steering.seek_weight + steering.flee_weight; + ctx.entity_registry->assign(boid_eid, steering); + } } // Load biome diff --git a/src/geom/spherical.hpp b/src/geom/spherical.hpp index a2dda81..2d82fee 100644 --- a/src/geom/spherical.hpp +++ b/src/geom/spherical.hpp @@ -48,7 +48,7 @@ math::vector3 to_cartesian(const math::vector3& v) { x * std::cos(v[2]), x * std::sin(v[2]), - v[0] * std::sin(v[1]), + v[0] * std::sin(v[1]) }; } diff --git a/src/math/matrix-functions.hpp b/src/math/matrix-functions.hpp index acdc5f4..d4edf5d 100644 --- a/src/math/matrix-functions.hpp +++ b/src/math/matrix-functions.hpp @@ -241,15 +241,14 @@ template resize(const matrix& m); /** - * Rotates a matrix. + * Constructs a rotation matrix. * - * @param m Matrix to rotate. - * @param angle Angle of rotation (in radians). + * @param angle Angle of rotation, in radians. * @param axis Axis of rotation - * @return Rotated matrix. + * @return Rotation matrix. */ template -matrix rotate(const matrix& m, T angle, const vector& axis); +matrix rotate(T angle, const vector& axis); /** * Produces a matrix which rotates Cartesian coordinates about the x-axis by a given angle. @@ -734,31 +733,24 @@ matrix resize(const matrix& m) } template -matrix rotate(const matrix& m, T angle, const vector& axis) +matrix rotate(T angle, const vector& axis) { const T c = std::cos(angle); const T s = std::sin(angle); const vector temp = mul(axis, T(1) - c); - matrix rotation; - rotation[0][0] = axis[0] * temp[0] + c + matrix rotation; + rotation[0][0] = axis[0] * temp[0] + c; rotation[0][1] = axis[1] * temp[0] + axis[2] * s; rotation[0][2] = axis[2] * temp[0] - axis[1] * s; - rotation[0][3] = T(0); - rotation[1][0] = axis[0] * temp[1] - axis[2] * s + rotation[1][0] = axis[0] * temp[1] - axis[2] * s; rotation[1][1] = axis[1] * temp[1] + c; rotation[1][2] = axis[2] * temp[1] + axis[0] * s; - rotation[1][3] = T(0); rotation[2][0] = axis[0] * temp[2] + axis[1] * s; rotation[2][1] = axis[1] * temp[2] - axis[0] * s; - rotation[2][2] = axis[2] * temp[2] + c - rotation[2][3] = T(0); - rotation[3][0] = T(0); - rotation[3][1] = T(0); - rotation[3][2] = T(0); - rotation[3][3] = T(1); - - return mul(m, rotation); + rotation[2][2] = axis[2] * temp[2] + c; + + return rotation; } template diff --git a/src/math/matrix-type.hpp b/src/math/matrix-type.hpp index e00939d..ad2576e 100644 --- a/src/math/matrix-type.hpp +++ b/src/math/matrix-type.hpp @@ -47,26 +47,20 @@ struct matrix inline constexpr const row_type& operator[](std::size_t i) const noexcept { return columns[i]; } }; -template -constexpr vector identity_matrix_column(const std::index_sequence&) +template +constexpr vector identity_matrix_row(const std::index_sequence&) { return {(Is == I ? T{1} : T{0})...}; } -template -constexpr matrix identity_matrix_rows(const std::index_sequence& is) +template +constexpr matrix identity_matrix(const std::index_sequence& is) { - return {{identity_matrix_column(is)...}}; -} - -template -constexpr matrix identity_matrix() -{ - return identity_matrix_rows(std::make_index_sequence{}); + return {{identity_matrix_row(is)...}}; } template -constexpr matrix matrix::identity = identity_matrix(); +constexpr matrix matrix::identity = identity_matrix(std::make_index_sequence{}); /// 2x2 matrix. template diff --git a/src/math/quaternion-functions.hpp b/src/math/quaternion-functions.hpp index f64566b..514f773 100644 --- a/src/math/quaternion-functions.hpp +++ b/src/math/quaternion-functions.hpp @@ -20,6 +20,7 @@ #ifndef ANTKEEPER_MATH_QUATERNION_FUNCTIONS_HPP #define ANTKEEPER_MATH_QUATERNION_FUNCTIONS_HPP +#include "math/constants.hpp" #include "math/matrix-type.hpp" #include "math/quaternion-type.hpp" #include "math/vector-type.hpp" @@ -209,6 +210,20 @@ quaternion slerp(const quaternion& x, const quaternion& y, T a); template quaternion sub(const quaternion& x, const quaternion& y); +/** + * Decomposes a quaternion into swing and twist rotation components. + * + * @param[in] q Quaternion to decompose. + * @param[in] a Axis of twist rotation. + * @param[out] swing Swing rotation component. + * @param[out] twist Twist rotation component. + * @param[in] epsilon Threshold at which a number is considered zero. + * + * @see https://www.euclideanspace.com/maths/geometry/rotations/for/decomposition/ + */ +template +void swing_twist(const quaternion& q, const vector& a, quaternion& qs, quaternion& qt, T epsilon = T(1e-6)); + /** * Converts a 3x3 rotation matrix to a quaternion. * @@ -284,11 +299,11 @@ quaternion look_rotation(const vector& forward, vector up) up = cross(right, forward); matrix m = - {{ - {right[0], up[0], -forward[0]}, - {right[1], up[1], -forward[1]}, - {right[2], up[2], -forward[2]} - }}; + { + right, + up, + -forward + }; // Convert to quaternion return normalize(quaternion_cast(m)); @@ -297,22 +312,22 @@ quaternion look_rotation(const vector& forward, vector up) template matrix matrix_cast(const quaternion& q) { - T wx = q.w * q.x; - T wy = q.w * q.y; - T wz = q.w * q.z; - T xx = q.x * q.x; - T xy = q.x * q.y; - T xz = q.x * q.z; - T yy = q.y * q.y; - T yz = q.y * q.z; - T zz = q.z * q.z; + const T xx = q.x * q.x; + const T xy = q.x * q.y; + const T xz = q.x * q.z; + const T xw = q.x * q.w; + const T yy = q.y * q.y; + const T yz = q.y * q.z; + const T yw = q.y * q.w; + const T zz = q.z * q.z; + const T zw = q.z * q.w; return - {{ - {T(1) - (yy + zz) * T(2), (xy + wz) * T(2), (xz - wy) * T(2)}, - {(xy - wz) * T(2), T(1) - (xx + zz) * T(2), (yz + wx) * T(2)}, - {(xz + wy) * T(2), (yz - wx) * T(2), T(1) - (xx + yy) * T(2)} - }}; + { + T(1) - (yy + zz) * T(2), (xy + zw) * T(2), (xz - yw) * T(2), + (xy - zw) * T(2), T(1) - (xx + zz) * T(2), (yz + xw) * T(2), + (xz + yw) * T(2), (yz - xw) * T(2), T(1) - (xx + yy) * T(2) + }; } template @@ -349,14 +364,7 @@ inline quaternion negate(const quaternion& x) template quaternion nlerp(const quaternion& x, const quaternion& y, T a) { - if (dot(x, y) < T(0)) - { - return normalize(add(mul(x, T(1) - a), mul(y, -a))); - } - else - { - return normalize(add(mul(x, T(1) - a), mul(y, a))); - } + return normalize(add(mul(x, T(1) - a), mul(y, a * std::copysign(T(1), dot(x, y))))); } template @@ -409,61 +417,80 @@ inline quaternion sub(const quaternion& x, const quaternion& y) } template -quaternion quaternion_cast(const matrix& m) +void swing_twist(const quaternion& q, const vector& a, quaternion& qs, quaternion& qt, T epsilon) { - T r; - vector i; + if (q.x * q.x + q.y * q.y + q.z * q.z > epsilon) + { + const vector pa = mul(a, (a.x * q.x + a.y * q.y + a.z * q.z)); + qt = normalize(quaternion{q.w, pa.x, pa.y, pa.z}); + qs = mul(q, conjugate(qt)); + } + else + { + qt = angle_axis(pi, a); + + const vector qa = mul(q, a); + const vector sa = cross(a, qa); + if (length_squared(sa) > epsilon) + qs = angle_axis(std::acos(dot(a, qa)), sa); + else + qs = quaternion::identity; + } +} +template +quaternion quaternion_cast(const matrix& m) +{ T trace = m[0][0] + m[1][1] + m[2][2]; + if (trace > T(0)) { T s = T(0.5) / std::sqrt(trace + T(1)); - r = T(0.25) / s; - i = - { - (m[2][1] - m[1][2]) * s, - (m[0][2] - m[2][0]) * s, - (m[1][0] - m[0][1]) * s - }; + return + { + T(0.25) / s, + (m[1][2] - m[2][1]) * s, + (m[2][0] - m[0][2]) * s, + (m[0][1] - m[1][0]) * s + }; } else { if (m[0][0] > m[1][1] && m[0][0] > m[2][2]) { T s = T(2) * std::sqrt(T(1) + m[0][0] - m[1][1] - m[2][2]); - r = (m[2][1] - m[1][2]) / s; - i = - { - T(0.25) * s, - (m[0][1] + m[1][0]) / s, - (m[0][2] + m[2][0]) / s - }; + + return + { + (m[1][2] - m[2][1]) / s, + T(0.25) * s, + (m[1][0] + m[0][1]) / s, + (m[2][0] + m[0][2]) / s + }; } else if (m[1][1] > m[2][2]) { T s = T(2) * std::sqrt(T(1) + m[1][1] - m[0][0] - m[2][2]); - r = (m[0][2] - m[2][0]) / s; - i = - { - (m[0][1] + m[1][0]) / s, - T(0.25) * s, - (m[1][2] + m[2][1]) / s - }; + return + { + (m[2][0] - m[0][2]) / s, + (m[1][0] + m[0][1]) / s, + T(0.25) * s, + (m[2][1] + m[1][2]) / s + }; } else { T s = T(2) * std::sqrt(T(1) + m[2][2] - m[0][0] - m[1][1]); - r = (m[1][0] - m[0][1]) / s; - i = - { - (m[0][2] + m[2][0]) / s, - (m[1][2] + m[2][1]) / s, - T(0.25) * s - }; + return + { + (m[0][1] - m[1][0]) / s, + (m[2][0] + m[0][2]) / s, + (m[2][1] + m[1][2]) / s, + T(0.25) * s + }; } } - - return {r, i.x, i.y, i.z}; } template diff --git a/src/math/vector-type.hpp b/src/math/vector-type.hpp index 6ea755d..84f5a84 100644 --- a/src/math/vector-type.hpp +++ b/src/math/vector-type.hpp @@ -28,7 +28,7 @@ namespace math { /** * An `N`-dimensional Euclidean vector. * - * @tparam T Vector component type. + * @tparam T Scalar type. * @tparam N Number of dimensions. */ template diff --git a/src/physics/time/gregorian.hpp b/src/physics/time/gregorian.hpp index 01d861b..cd23c14 100644 --- a/src/physics/time/gregorian.hpp +++ b/src/physics/time/gregorian.hpp @@ -25,7 +25,7 @@ namespace physics { namespace time { -// Gregorian calender time. +/// Gregorian calender time. namespace gregorian { /** diff --git a/src/physics/time/jd.hpp b/src/physics/time/jd.hpp index 43bc8ee..3b1a3b8 100644 --- a/src/physics/time/jd.hpp +++ b/src/physics/time/jd.hpp @@ -23,11 +23,11 @@ namespace physics { namespace time { -// Julian day (JD) +/// Julian day (JD). namespace jd { /** - * Converts JD to UT1 + * Converts JD to UT1. * * @param t JD time. * @return UT1 time. diff --git a/src/physics/time/ut1.hpp b/src/physics/time/ut1.hpp index 6879aa5..8ead755 100644 --- a/src/physics/time/ut1.hpp +++ b/src/physics/time/ut1.hpp @@ -29,7 +29,7 @@ namespace time { namespace ut1 { /** - * Converts UT1 to JD + * Converts UT1 to JD. * * @param t UT1 time. * @return JD time.