Soldato following

2 files changed, 115 insertions, 24 deletions

diff --git a/src/entities/soldato.c b/src/entities/soldato.c
index 2cc0e07..ad454d8 100644
--- a/src/entities/soldato.c
+++ b/src/entities/soldato.c
@@ -8,22 +8,7 @@ void initSoldato(Entity * entity, Game * game) {
 	setEntityRadius(entity);
 
 	// Acceleration.
-	entity->useAcceleration = true;
-	entity->acceleration = (EntityAcceleration){
-		.speedUp = 20.0,
-		.speedDown = 15.0
-	};
-
-	// PID configs.
-	PIDConfig speedPIDConfig = {
-		.kP = 1,
-		.kI = 0.0,
-		.kD = 0.0,
-		.angleMode = false,
-		.doClamp = true,
-		.min = 0.0,
-		.max = 100.0
-	};
+	entity->useAcceleration = false;
 
 	// Allocate data.
 	entity->data = KF_MALLOC(sizeof(Soldato));
@@ -35,14 +20,26 @@ void initSoldato(Entity * entity, Game * game) {
 
 	Soldato * data = (Soldato*)entity->data;
 
+	PIDConfig speedPID = (PIDConfig){
+		.kP = 0.5,
+		.kI = 0.0,
+		.kD = 0.0,
+		.angleMode = false,
+		.doClamp = true,
+		.min = 0.0,
+		.max = 20.0
+	};
+
 	// Create fly to point.
 	data->flyToPoint = (EntityFlyToPointInfo){
-		//.controller.speedPID = createPID(speedPIDConfig),
-		.controller.bangbang.speed = 20.0,
-		.controller.bangbang.stopAt = 0.0,
+		.controller.bangbang.speed = 50.0,
+		.controller.bangbang.stopAt = entity->radius * 2,
 		.controlType = ENTITY_FLY_TO_POINT_BANG_BANG,
 		.rotationSpeed = 0.0
 	};
+
+	data->leaderId = ENTITY_NONE;
+	data->followerId = ENTITY_NONE;
 }
 
 void closeSoldato(Entity * entity) {
@@ -53,21 +50,107 @@ void closeSoldato(Entity * entity) {
 	entityFreeCollisionModel(entity->transformedCollisionModel);
 }
 
-void updateSoldato(Game * game, Entity * entity) {
-	entityUpdateLastValues(entity);
-
-	Entity * player = getEntityFromWorld(game->world, 0);
+void soldatoFollowLeader(Game * game, Entity * entity) {
 	Soldato * data = (Soldato*)entity->data;
+	Entity * leader;
+
+	// Get leader.
+	leader = getEntityFromWorld(game->world, data->leaderId);
+
+	// Bye bye leader.
+	if (leader == NULL) {
+		data->leaderId = ENTITY_NONE;
+		return;
+	}
 
+	if (leader->fingerprint != data->leaderFingerprint) {
+		data->leaderId = ENTITY_NONE;
+		return;
+	}
+
+	// Fly to leader.
 	entityFlyToPoint(
 		entity,
-		player->position,
+		leader->position,
 		&data->flyToPoint
 	);
+}
+
+void handleFollower(Game * game, Entity * entity) {
+	Soldato * data = (Soldato*)entity->data;
+	Entity * follower;
+
+	if (data->followerId == ENTITY_NONE)
+		return;
+
+	// Get follower.
+	follower = getEntityFromWorld(game->world, data->followerId);
+
+	// Bye bye follower.
+	if (follower == NULL) {
+		data->followerId = ENTITY_NONE;
+		return;
+	}
+
+	if (follower->fingerprint != data->followerFingerprint) {
+		data->followerId = ENTITY_NONE;
+		return;
+	}
+}
+
+void updateSoldato(Game * game, Entity * entity) {
+	entityUpdateLastValues(entity);
+
+	Entity * player = getEntityFromWorld(game->world, 0);
+	Soldato * data = (Soldato*)entity->data;
+
+	// Fly to player if no leader.
+	if (data->leaderId == ENTITY_NONE) {
+		entityFlyToPoint(
+			entity,
+			player->position,
+			&data->flyToPoint
+		);
+	} else {
+		soldatoFollowLeader(game, entity);
+		handleFollower(game, entity);
+	}
 
 	entityCheckTransformedCollisionModel(entity);
 }
 
 void drawSoldato(Game * game, Entity * entity) {
 	entityDraw(entity);
+
+	Soldato * data = (Soldato*)entity->data;
+	Entity * leader;
+
+	// Debug line.
+	//if (data->leaderId != ENTITY_NONE) {
+	//	leader = getEntityFromWorld(game->world, data->leaderId);
+	//	DrawLine3D(entity->position, leader->position, BLUE);
+	//}
+}
+
+void setSoldatoLeader(Entity * entity1, Entity * entity2) {
+	Soldato * data1;
+	Soldato * data2;
+
+	if (entity1->type != ENTITY_SOLDATO || entity2->type != ENTITY_SOLDATO)
+		return;
+
+	data1 = (Soldato*)entity1->data;
+	data2 = (Soldato*)entity2->data;
+
+	// Already has follower.
+	if (data2->followerId != ENTITY_NONE)
+		return;
+
+	if (data1->leaderId == ENTITY_NONE) {
+		data1->leaderId = entity2->id;
+		data1->leaderFingerprint = entity2->fingerprint;
+
+		data2->followerId = entity1->id;
+		data2->followerFingerprint = entity1->fingerprint;
+	}
 }
diff --git a/src/entities/soldato.h b/src/entities/soldato.h
index f93206d..460c799 100644
--- a/src/entities/soldato.h
+++ b/src/entities/soldato.h
@@ -7,6 +7,12 @@
 
 typedef struct Soldato {
 	EntityFlyToPointInfo flyToPoint;
+
+	EntityId leaderId;
+	EntityFingerprint leaderFingerprint;
+
+	EntityId followerId;
+	EntityFingerprint followerFingerprint;
 } Soldato;
 
 void initSoldato(Entity * entity, Game * game);
@@ -14,4 +20,6 @@ void closeSoldato(Entity * entity);
 void updateSoldato(Game * game, Entity * entity);
 void drawSoldato(Game * game, Entity * entity);
 
+void setSoldatoLeader(Entity * entity1, Entity * entity2);
+
 #endif