Add wander, seek, and flee steering behaviors, improve conversions between quaternions and matrices

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)

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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<float> 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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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<float> 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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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

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
-

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<component::transform, component::steering>().each(
-		[&](entity::id entity_id, auto& transform, auto& boid)
+	registry.view<component::steering, component::transform>().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<float>(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<float>(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

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

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;

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);

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<double>(2000, 1, 1, 12, 0, 0.0, utc);
-		const double spring_2000 = physics::time::gregorian::to_ut1<double>(2000, 6, 19, 12, 0, 0.0, utc);
-		game::world::set_time(ctx, 14622.5);
+		const double summer_2022 = physics::time::gregorian::to_ut1<double>(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<float>::identity;
+			transform.world = math::transform<float>::identity;
+			transform.warp = true;
+			ctx.entity_registry->assign<entity::component::transform>(boid_eid, transform);
+			
+			// Create model component
+			entity::component::model model;
+			model.render_model = ctx.resource_manager->load<render::model>("boid.mdl");
+			model.instance_count = 0;
+			model.layers = 1;
+			ctx.entity_registry->assign<entity::component::model>(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<float>::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<entity::component::steering>(boid_eid, steering);
+		}
 	}
 	
 	// Load biome

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])
 	};
 }
 

src/math/matrix-functions.hpp

@@ -241,15 +241,14 @@ template
 matrix<T, N1, M1> resize(const matrix<T, N0, M0>& 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 <class T>
-matrix<T, 4, 4> rotate(const matrix<T, 4, 4>& m, T angle, const vector<T, 3>& axis);
+matrix<T, 3, 3> rotate(T angle, const vector<T, 3>& 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 <class T>
-matrix<T, 4, 4> rotate(const matrix<T, 4, 4>& m, T angle, const vector<T, 3>& axis)
+matrix<T, 3, 3> rotate(T angle, const vector<T, 3>& axis)
 {
 	const T c = std::cos(angle);
 	const T s = std::sin(angle);
 	const vector<T, 3> temp = mul(axis, T(1) - c);
 
-	matrix<T, 4, 4> rotation;
-	rotation[0][0] = axis[0] * temp[0] + c
+	matrix<T, 3, 3> 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 <class T>

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 <typename T, std::size_t I, std::size_t ... Is>
-constexpr vector<T, sizeof...(Is)> identity_matrix_column(const std::index_sequence<Is...>&)
+template <typename T, std::size_t I, std::size_t... Is>
+constexpr vector<T, sizeof...(Is)> identity_matrix_row(const std::index_sequence<Is...>&)
 {
 	return {(Is == I ? T{1} : T{0})...};
 }
 
-template <typename T, std::size_t ... Is>
-constexpr matrix<T, sizeof...(Is), sizeof...(Is)> identity_matrix_rows(const std::index_sequence<Is...>& is)
+template <typename T, std::size_t... Is>
+constexpr matrix<T, sizeof...(Is), sizeof...(Is)> identity_matrix(const std::index_sequence<Is...>& is)
 {
-	return {{identity_matrix_column<T, Is>(is)...}};
-}
-
-template <typename T, std::size_t N>
-constexpr matrix<T, N, N> identity_matrix()
-{
-	return identity_matrix_rows<T>(std::make_index_sequence<N>{});
+	return {{identity_matrix_row<T, Is>(is)...}};
 }
 
 template <typename T, std::size_t N, std::size_t M>
-constexpr matrix<T, N, M> matrix<T, N, M>::identity = identity_matrix<T, N>();
+constexpr matrix<T, N, M> matrix<T, N, M>::identity = identity_matrix<T>(std::make_index_sequence<N>{});
 
 /// 2x2 matrix.
 template <typename T>

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 <class T>
 quaternion<T> sub(const quaternion<T>& x, const quaternion<T>& 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 <class T>
+void swing_twist(const quaternion<T>& q, const vector<T, 3>& a, quaternion<T>& qs, quaternion<T>& 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<T, 3, 3> 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 <class T>
 matrix<T, 3, 3> matrix_cast(const quaternion<T>& 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 <class T>
@@ -349,14 +364,7 @@ inline quaternion negate(const quaternion& x)
 template <class T>
 quaternion<T> nlerp(const quaternion<T>& x, const quaternion<T>& 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 <class T>
@@ -409,61 +417,80 @@ inline quaternion sub(const quaternion& x, const quaternion& y)
 }
 
 template <class T>
-quaternion<T> quaternion_cast(const matrix<T, 3, 3>& m)
+void swing_twist(const quaternion<T>& q, const vector<T, 3>& a, quaternion<T>& qs, quaternion<T>& qt, T epsilon)
 {
-	T r;
-	vector<T, 3> i;
+	if (q.x * q.x + q.y * q.y + q.z * q.z > epsilon)
+	{
+		const vector<T, 3> pa = mul(a, (a.x * q.x + a.y * q.y + a.z * q.z));
+		qt = normalize(quaternion<T>{q.w, pa.x, pa.y, pa.z});
+		qs = mul(q, conjugate(qt));
+	}
+	else
+	{
+		qt = angle_axis(pi<T>, a);
+		
+		const vector<T, 3> qa = mul(q, a);
+		const vector<T, 3> sa = cross(a, qa);
+		if (length_squared(sa) > epsilon)
+			qs = angle_axis(std::acos(dot(a, qa)), sa);
+		else
+			qs = quaternion<T>::identity;
+	}
+}
 
+template <class T>
+quaternion<T> quaternion_cast(const matrix<T, 3, 3>& 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 <class T2, class T1>

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 <typename T, std::size_t N>

src/physics/time/gregorian.hpp

@@ -25,7 +25,7 @@
 namespace physics {
 namespace time {
 
-// Gregorian calender time.
+/// Gregorian calender time.
 namespace gregorian {
 
 /**

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.

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.