Better soldato following

2 files changed, 61 insertions, 47 deletions

diff --git a/src/entities/soldato.c b/src/entities/soldato.c
index ad454d8..69dd99a 100644
--- a/src/entities/soldato.c
+++ b/src/entities/soldato.c
@@ -20,26 +20,29 @@ void initSoldato(Entity * entity, Game * game) {
 
 	Soldato * data = (Soldato*)entity->data;
 
-	PIDConfig speedPID = (PIDConfig){
-		.kP = 0.5,
+	PIDConfig followingPID = (PIDConfig){
+		.kP = 1.0,
 		.kI = 0.0,
 		.kD = 0.0,
 		.angleMode = false,
 		.doClamp = true,
 		.min = 0.0,
-		.max = 20.0
+		.max = 50.0
 	};
 
 	// Create fly to point.
-	data->flyToPoint = (EntityFlyToPointInfo){
-		.controller.bangbang.speed = 50.0,
-		.controller.bangbang.stopAt = entity->radius * 2,
+	data->flyToPointLeading = (EntityFlyToPointInfo){
+		.controller.bangbang.speed = 50,
+		.controller.bangbang.stopAt = 0.0,
 		.controlType = ENTITY_FLY_TO_POINT_BANG_BANG,
-		.rotationSpeed = 0.0
+		.rotationSpeed = 50.0
 	};
 
-	data->leaderId = ENTITY_NONE;
-	data->followerId = ENTITY_NONE;
+	data->flyToPointFollowing = (EntityFlyToPointInfo){
+		.controller.speedPID = createPID(followingPID),
+		.controlType = ENTITY_FLY_TO_POINT_PID,
+		.rotationSpeed = 50.0
+	};
 }
 
 void closeSoldato(Entity * entity) {
@@ -55,16 +58,16 @@ void soldatoFollowLeader(Game * game, Entity * entity) {
 	Entity * leader;
 
 	// Get leader.
-	leader = getEntityFromWorld(game->world, data->leaderId);
+	leader = getEntityFromWorld(game->world, entity->follow.leaderId);
 
 	// Bye bye leader.
 	if (leader == NULL) {
-		data->leaderId = ENTITY_NONE;
+		entity->follow.leaderId = ENTITY_NONE;
 		return;
 	}
 
-	if (leader->fingerprint != data->leaderFingerprint) {
-		data->leaderId = ENTITY_NONE;
+	if (leader->fingerprint != entity->follow.leaderFingerprint) {
+		entity->follow.leaderId = ENTITY_NONE;
 		return;
 	}
 
@@ -72,28 +75,27 @@ void soldatoFollowLeader(Game * game, Entity * entity) {
 	entityFlyToPoint(
 		entity,
 		leader->position,
-		&data->flyToPoint
+		&data->flyToPointFollowing
 	);
 }
 
 void handleFollower(Game * game, Entity * entity) {
-	Soldato * data = (Soldato*)entity->data;
 	Entity * follower;
 
-	if (data->followerId == ENTITY_NONE)
+	if (entity->follow.followerId == ENTITY_NONE)
 		return;
 
 	// Get follower.
-	follower = getEntityFromWorld(game->world, data->followerId);
+	follower = getEntityFromWorld(game->world, entity->follow.followerId);
 
 	// Bye bye follower.
 	if (follower == NULL) {
-		data->followerId = ENTITY_NONE;
+		entity->follow.followerId = ENTITY_NONE;
 		return;
 	}
 
-	if (follower->fingerprint != data->followerFingerprint) {
-		data->followerId = ENTITY_NONE;
+	if (follower->fingerprint != entity->follow.followerFingerprint) {
+		entity->follow.followerId = ENTITY_NONE;
 		return;
 	}
 }
@@ -105,11 +107,11 @@ void updateSoldato(Game * game, Entity * entity) {
 	Soldato * data = (Soldato*)entity->data;
 
 	// Fly to player if no leader.
-	if (data->leaderId == ENTITY_NONE) {
+	if (entity->follow.leaderId == ENTITY_NONE) {
 		entityFlyToPoint(
 			entity,
 			player->position,
-			&data->flyToPoint
+			&data->flyToPointLeading
 		);
 	} else {
 		soldatoFollowLeader(game, entity);
@@ -122,35 +124,52 @@ void updateSoldato(Game * game, Entity * 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);
-	//}
+	if (entity->follow.leaderId != ENTITY_NONE) {
+		leader = getEntityFromWorld(game->world, entity->follow.leaderId);
+
+		if (leader == NULL)
+			return;
+
+		DrawLine3D(entity->position, leader->position, BLUE);
+	}
 }
 
 void setSoldatoLeader(Entity * entity1, Entity * entity2) {
-	Soldato * data1;
-	Soldato * data2;
+	Entity * follower;
+	Entity * leader;
 
-	if (entity1->type != ENTITY_SOLDATO || entity2->type != ENTITY_SOLDATO)
+	// Who is follower and leader.
+	if (entity1->type == ENTITY_SOLDATO && entity2->type == ENTITY_SOLDATO) {
+		if (entity1->follow.leaderId == ENTITY_NONE) {
+			follower = entity1;
+			leader = entity2;
+		} else {
+			follower = entity2;
+			leader = entity1;
+		}
+	} else if (entity1->type == ENTITY_SOLDATO) {
+		follower = entity1;
+		leader = entity2;
+	} else if (entity2->type == ENTITY_SOLDATO) {
+		follower = entity2;
+		leader = entity1;
+	} else {
 		return;
-
-	data1 = (Soldato*)entity1->data;
-	data2 = (Soldato*)entity2->data;
+	}
 
 	// Already has follower.
-	if (data2->followerId != ENTITY_NONE)
+	if (leader->follow.followerId != ENTITY_NONE)
+		return;
+	// Already has leader.
+	if (follower->follow.leaderId != ENTITY_NONE)
 		return;
 
-	if (data1->leaderId == ENTITY_NONE) {
-		data1->leaderId = entity2->id;
-		data1->leaderFingerprint = entity2->fingerprint;
+	follower->follow.leaderId = leader->id;
+	follower->follow.leaderFingerprint = leader->fingerprint;
 
-		data2->followerId = entity1->id;
-		data2->followerFingerprint = entity1->fingerprint;
-	}
+	leader->follow.followerId = follower->id;
+	leader->follow.followerFingerprint = follower->fingerprint;
 }
diff --git a/src/entities/soldato.h b/src/entities/soldato.h
index 460c799..eb336db 100644
--- a/src/entities/soldato.h
+++ b/src/entities/soldato.h
@@ -6,13 +6,8 @@
 #define SOLDATO_H
 
 typedef struct Soldato {
-	EntityFlyToPointInfo flyToPoint;
-
-	EntityId leaderId;
-	EntityFingerprint leaderFingerprint;
-
-	EntityId followerId;
-	EntityFingerprint followerFingerprint;
+	EntityFlyToPointInfo flyToPointLeading;
+	EntityFlyToPointInfo flyToPointFollowing;
 } Soldato;
 
 void initSoldato(Entity * entity, Game * game);