Better soldato following

6 files changed, 83 insertions, 51 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);
diff --git a/src/entity.c b/src/entity.c
index 0757109..f4c3747 100644
--- a/src/entity.c
+++ b/src/entity.c
@@ -59,6 +59,10 @@ Entity createEntity(EntityType type, Game * game) {
 		.updateCb = info.updateCb,
 		.drawCb = info.drawCb,
 		.health = ENTITY_MAX_HEALTH,
+		.follow = (EntityFollow){
+			.leaderId = ENTITY_NONE,
+			.followerId = ENTITY_NONE
+		},
 		.collision.hit = false,
 		.data = NULL
 	};
diff --git a/src/entity.h b/src/entity.h
index 6246fdd..5faa413 100644
--- a/src/entity.h
+++ b/src/entity.h
@@ -72,6 +72,15 @@ typedef struct EntityCollision {
 	EntityFingerprint fromFingerprint;
 } EntityCollision;
 
+// Used for hanlding the soldato follower and leader thing.
+typedef struct EntityFollow {
+	EntityId leaderId;
+	EntityFingerprint leaderFingerprint;
+
+	EntityId followerId;
+	EntityFingerprint followerFingerprint;
+} EntityFollow;
+
 // Health stuff.
 #define ENTITY_MIN_HEALTH 0.0
 #define ENTITY_MAX_HEALTH 1.0
@@ -105,6 +114,8 @@ typedef struct Entity {
 	EntityUpdateCb updateCb;
 	EntityDrawCb drawCb;
 
+	EntityFollow follow;
+
 	// Health is a percent from 1.0 to 0.0.
 	float health;
 
diff --git a/src/playerCamera.c b/src/playerCamera.c
index f759166..6b165f3 100644
--- a/src/playerCamera.c
+++ b/src/playerCamera.c
@@ -23,7 +23,7 @@ void updatePlayerCamera(Camera3D * camera, Game * game) {
 	// Up.
 	camera->up = Vector3RotateByQuaternion((Vector3){0.0, 1.0, 0.0}, player->rotation);
 
-	camera->target = player->position;
-	camera->position = (Vector3){10.0, 10.0, 10.0};
-	camera->up = (Vector3){0.0, 1.0, 0.0};
+	//camera->target = player->position;
+	//camera->position = (Vector3){10.0, 10.0, 10.0};
+	//camera->up = (Vector3){0.0, 1.0, 0.0};
 }
diff --git a/src/world.c b/src/world.c
index 946a283..6d51a09 100644
--- a/src/world.c
+++ b/src/world.c
@@ -233,11 +233,14 @@ void handleCollisionInWorld(Entity * entity1, Entity * entity2) {
 	//if (entity2->id != 0)
 	//	entity2->health = 0.0;
 	
+
+	if (entity1->type != ENTITY_ANTIFA && entity2->type != ENTITY_ANTIFA)
+		setSoldatoLeader(entity1, entity2);
+	
 	switch (entity1->type) {
 		case ENTITY_ANTIFA:
 			break;
 		case ENTITY_SOLDATO:
-			setSoldatoLeader(entity1, entity2);
 			break;
 		case ENTITY_CAPORALE:
 			break;