1 files changed, 73 insertions, 55 deletions

diff --git a/src/crepe/system/AISystem.cpp b/src/crepe/system/AISystem.cpp
index 64e93fc..1d8ffb9 100644
--- a/src/crepe/system/AISystem.cpp
+++ b/src/crepe/system/AISystem.cpp
@@ -1,61 +1,86 @@
 #include <algorithm>
 #include <cmath>
 
-#include "api/LoopTimer.h"
-#include "api/Rigidbody.h"
-#include "api/Transform.h"
 #include "manager/ComponentManager.h"
+#include "manager/LoopTimerManager.h"
 #include "manager/Mediator.h"
 
 #include "AISystem.h"
-#include "types.h"
 
 using namespace crepe;
+using namespace std::chrono;
 
 void AISystem::update() {
 	const Mediator & mediator = this->mediator;
 	ComponentManager & mgr = mediator.component_manager;
+	LoopTimerManager & timer = mediator.loop_timer;
 	RefVector<AI> ai_components = mgr.get_components_by_type<AI>();
+	LoopTimerManager & loop_timer = mediator.loop_timer;
 
-	double dt = LoopTimer::get_instance().get_fixed_delta_time();
+	//TODO: Use fixed loop dt (this is not available at master at the moment)
+	duration_t dt = loop_timer.get_delta_time();
 
+	// Loop through all AI components
 	for (AI & ai : ai_components) {
+		if (!ai.active) {
+			continue;
+		}
+
 		RefVector<Rigidbody> rigidbodies
 			= mgr.get_components_by_id<Rigidbody>(ai.game_object_id);
+		if (rigidbodies.empty()) {
+			throw std::runtime_error(
+				"AI component must be attached to a GameObject with a Rigidbody component");
+		}
 		Rigidbody & rigidbody = rigidbodies.front().get();
+		if (!rigidbody.active) {
+			continue;
+		}
+		if (rigidbody.data.mass <= 0) {
+			throw std::runtime_error("Mass must be greater than 0");
+		}
 
-		vec2 force = this->calculate(ai);
+		// Calculate the force to apply to the entity
+		vec2 force = this->calculate(ai, rigidbody);
+		// Calculate the acceleration (using the above calculated force)
 		vec2 acceleration = force / rigidbody.data.mass;
-		rigidbody.data.linear_velocity += acceleration * dt;
+		// Finally, update Rigidbody's velocity
+		rigidbody.data.linear_velocity += acceleration * duration_cast<seconds>(dt).count();
 	}
 }
 
-vec2 AISystem::calculate(AI & ai) {
+vec2 AISystem::calculate(AI & ai, const Rigidbody & rigidbody) {
+	const Mediator & mediator = this->mediator;
+	ComponentManager & mgr = mediator.component_manager;
+	RefVector<Transform> transforms = mgr.get_components_by_id<Transform>(ai.game_object_id);
+	Transform & transform = transforms.front().get();
+
 	vec2 force;
 
-	if (ai.on(AI::BehaviorType::FLEE)) {
-		vec2 force_to_add = this->flee(ai);
+	// Run all the behaviors that are on, and stop if the force gets too high
+	if (ai.on(AI::BehaviorTypeMask::FLEE)) {
+		vec2 force_to_add = this->flee(ai, rigidbody, transform);
 
 		if (!this->accumulate_force(ai, force, force_to_add)) {
 			return force;
 		}
 	}
-	if (ai.on(AI::BehaviorType::ARRIVE)) {
-		vec2 force_to_add = this->arrive(ai);
+	if (ai.on(AI::BehaviorTypeMask::ARRIVE)) {
+		vec2 force_to_add = this->arrive(ai, rigidbody, transform);
 
 		if (!this->accumulate_force(ai, force, force_to_add)) {
 			return force;
 		}
 	}
-	if (ai.on(AI::BehaviorType::SEEK)) {
-		vec2 force_to_add = this->seek(ai);
+	if (ai.on(AI::BehaviorTypeMask::SEEK)) {
+		vec2 force_to_add = this->seek(ai, rigidbody, transform);
 
 		if (!this->accumulate_force(ai, force, force_to_add)) {
 			return force;
 		}
 	}
-	if (ai.on(AI::BehaviorType::PATH_FOLLOW)) {
-		vec2 force_to_add = this->path_follow(ai);
+	if (ai.on(AI::BehaviorTypeMask::PATH_FOLLOW)) {
+		vec2 force_to_add = this->path_follow(ai, rigidbody, transform);
 
 		if (!this->accumulate_force(ai, force, force_to_add)) {
 			return force;
@@ -65,18 +90,21 @@ vec2 AISystem::calculate(AI & ai) {
 	return force;
 }
 
-bool AISystem::accumulate_force(AI & ai, vec2 & running_total, vec2 force_to_add) {
+bool AISystem::accumulate_force(const AI & ai, vec2 & running_total, vec2 & force_to_add) {
 	float magnitude = running_total.length();
 	float magnitude_remaining = ai.max_force - magnitude;
 
 	if (magnitude_remaining <= 0.0f) {
+		// If the force is already at/above the max force, return false
 		return false;
 	}
 
 	float magnitude_to_add = force_to_add.length();
 	if (magnitude_to_add < magnitude_remaining) {
+		// If the force to add is less than the remaining force, add it
 		running_total += force_to_add;
 	} else {
+		// If the force to add is greater than the remaining force, add a fraction of it
 		force_to_add.normalize();
 		running_total += force_to_add * magnitude_remaining;
 	}
@@ -84,14 +112,9 @@ bool AISystem::accumulate_force(AI & ai, vec2 & running_total, vec2 force_to_add
 	return true;
 }
 
-vec2 AISystem::seek(const AI & ai) {
-	const Mediator & mediator = this->mediator;
-	ComponentManager & mgr = mediator.component_manager;
-	RefVector<Transform> transforms = mgr.get_components_by_id<Transform>(ai.game_object_id);
-	Transform & transform = transforms.front().get();
-	RefVector<Rigidbody> rigidbodies = mgr.get_components_by_id<Rigidbody>(ai.game_object_id);
-	Rigidbody & rigidbody = rigidbodies.front().get();
-
+vec2 AISystem::seek(const AI & ai, const Rigidbody & rigidbody,
+					const Transform & transform) const {
+	// Calculate the desired velocity
 	vec2 desired_velocity = ai.seek_target - transform.position;
 	desired_velocity.normalize();
 	desired_velocity *= rigidbody.data.max_linear_velocity;
@@ -99,36 +122,29 @@ vec2 AISystem::seek(const AI & ai) {
 	return desired_velocity - rigidbody.data.linear_velocity;
 }
 
-vec2 AISystem::flee(const AI & ai) {
-	const Mediator & mediator = this->mediator;
-	ComponentManager & mgr = mediator.component_manager;
-	RefVector<Transform> transforms = mgr.get_components_by_id<Transform>(ai.game_object_id);
-	Transform & transform = transforms.front().get();
-	RefVector<Rigidbody> rigidbodies = mgr.get_components_by_id<Rigidbody>(ai.game_object_id);
-	Rigidbody & rigidbody = rigidbodies.front().get();
-
+vec2 AISystem::flee(const AI & ai, const Rigidbody & rigidbody,
+					const Transform & transform) const {
+	// Calculate the desired velocity if the entity is within the panic distance
 	vec2 desired_velocity = transform.position - ai.flee_target;
 	if (desired_velocity.length_squared() > ai.square_flee_panic_distance) {
 		return vec2{0, 0};
 	}
-
 	desired_velocity.normalize();
 	desired_velocity *= rigidbody.data.max_linear_velocity;
 
 	return desired_velocity - rigidbody.data.linear_velocity;
 }
 
-vec2 AISystem::arrive(const AI & ai) {
-	const Mediator & mediator = this->mediator;
-	ComponentManager & mgr = mediator.component_manager;
-	RefVector<Transform> transforms = mgr.get_components_by_id<Transform>(ai.game_object_id);
-	Transform & transform = transforms.front().get();
-	RefVector<Rigidbody> rigidbodies = mgr.get_components_by_id<Rigidbody>(ai.game_object_id);
-	Rigidbody & rigidbody = rigidbodies.front().get();
-
-	vec2 to_target = ai.seek_target - transform.position;
+vec2 AISystem::arrive(const AI & ai, const Rigidbody & rigidbody,
+					  const Transform & transform) const {
+	// Calculate the desired velocity (taking into account the deceleration rate)
+	vec2 to_target = ai.arrive_target - transform.position;
 	float distance = to_target.length();
 	if (distance > 0.0f) {
+		if (ai.arrive_deceleration <= 0.0f) {
+			throw std::runtime_error("Deceleration rate must be greater than 0");
+		}
+
 		float speed = distance / ai.arrive_deceleration;
 		speed = std::min(speed, rigidbody.data.max_linear_velocity);
 		vec2 desired_velocity = to_target * (speed / distance);
@@ -139,32 +155,34 @@ vec2 AISystem::arrive(const AI & ai) {
 	return vec2{0, 0};
 }
 
-vec2 AISystem::path_follow(AI & ai) {
-	const Mediator & mediator = this->mediator;
-	ComponentManager & mgr = mediator.component_manager;
-	RefVector<Transform> transforms = mgr.get_components_by_id<Transform>(ai.game_object_id);
-	Transform & transform = transforms.front().get();
-	RefVector<Rigidbody> rigidbodies = mgr.get_components_by_id<Rigidbody>(ai.game_object_id);
-	Rigidbody & rigidbody = rigidbodies.front().get();
-
+vec2 AISystem::path_follow(AI & ai, const Rigidbody & rigidbody, const Transform & transform) {
 	if (ai.path.empty()) {
 		return vec2{0, 0};
 	}
 
-	vec2 to_target = ai.path.at(ai.path_index) - transform.position;
+	// Get the target node
+	vec2 target = ai.path.at(ai.path_index);
+	// Calculate the force to apply to the entity
+	vec2 to_target = target - transform.position;
 	if (to_target.length_squared() > ai.path_node_distance * ai.path_node_distance) {
-		ai.seek_target = ai.path.at(ai.path_index);
+		// If the entity is not close enough to the target node, seek it
+		ai.seek_target = target;
+		ai.arrive_target = target;
 	} else {
+		// If the entity is close enough to the target node, move to the next node
 		ai.path_index++;
 		if (ai.path_index >= ai.path.size()) {
 			if (ai.path_loop) {
+				// If the path is looping, reset the path index
 				ai.path_index = 0;
 			} else {
+				// If the path is not looping, arrive at the last node
 				ai.path_index = ai.path.size() - 1;
-				return this->arrive(ai);
+				return this->arrive(ai, rigidbody, transform);
 			}
 		}
 	}
 
-	return this->seek(ai);
+	// Seek the target node
+	return this->seek(ai, rigidbody, transform);
 }