4 files changed, 120 insertions, 5 deletions

diff --git a/src/entities/caporale.c b/src/entities/caporale.c
index c8b4345..bfdc610 100644
--- a/src/entities/caporale.c
+++ b/src/entities/caporale.c
@@ -22,7 +22,8 @@ void initCaporale(Entity * entity, Game * game) {
 		.controller.bangbang.speed = 80,
 		.controller.bangbang.stopAt = 0.0,
 		.controlType = ENTITY_FLY_TO_POINT_BANG_BANG,
-		.rotationSpeed = 0.5
+		.rotationSpeed = 0.5,
+		.applyRotation = false
 	};
 }
 
diff --git a/src/entities/sergente.c b/src/entities/sergente.c
index 2614ec8..e9b0b71 100644
--- a/src/entities/sergente.c
+++ b/src/entities/sergente.c
@@ -4,17 +4,114 @@
 
 void initSergente(Entity * entity, Game * game) {
 	entity->model = &game->assets.models[SERGENTE_ASSET];
+	entity->collisionModel = entityCreateCollisionModel(*entity->model);
+	entity->transformedCollisionModel = entityCreateCollisionModel(*entity->model);
 	setEntityRadius(entity);
-	entity->velocity.angularVelocity = (AxisAngle){(Vector3){1.0, 1.0, 1.0}, 1.0};
+
+	// Allocate data.
+	entity->data = KF_MALLOC(sizeof(Sergente));
+
+	if (entity->data == NULL) {
+		ALLOCATION_ERROR;
+		return;
+	}
+
+	Sergente * data = (Sergente*)entity->data;
+
+	data->flyToPoint = (EntityFlyToPointInfo){
+		.controller.bangbang.speed = 85.0,
+		.controller.bangbang.stopAt = 0.0,
+		.controlType = ENTITY_FLY_TO_POINT_BANG_BANG,
+		.rotationSpeed = 0.0
+	};
+
+	createSergenteTarget(game, entity);
 }
 
 void closeSergente(Entity * entity) {
+	if (entity->data != NULL)
+		KF_FREE(entity->data);
+
+	entityFreeCollisionModel(entity->collisionModel);
+	entityFreeCollisionModel(entity->transformedCollisionModel);
+}
+
+void updateRotationSergente(Game * game, Entity * entity) {
+	float t = GetFrameTime();
+	Entity * player = getEntityFromWorld(game->world, 0);
+
+	// Get rotation.
+	Matrix matrix = MatrixLookAt(player->position, entity->position, (Vector3){0.0, 1.0, 0.0});
+	Quaternion rotation = QuaternionInvert(QuaternionFromMatrix(matrix));
+
+	// Update current rotation.
+	entity->rotation = QuaternionSlerp(entity->rotation, rotation, t * SERGENTE_ROTATION_SPEED);
 }
 
 void updateSergente(Game * game, Entity * entity) {
-	entityUpdateRotation(entity);
+	entityUpdateLastValues(entity);
+
+	Sergente * data = (Sergente*)entity->data;
+
+	entityFlyToPoint(entity, data->target, &data->flyToPoint);
+
+	// Create next point.
+	if (Vector3Distance(entity->position, data->target) <= SERGENTE_NEXT_POINT_THRESHOLD)
+		createSergenteTarget(game, entity);
+
+	updateRotationSergente(game, entity);
+	entityCheckTransformedCollisionModel(entity);
 }
 
 void drawSergente(Game * game, Entity * entity) {
 	entityDraw(entity);
+
+	// Test if facing player always.
+	DrawLine3D(
+		entity->position,
+		Vector3Add(
+			entity->position,
+			Vector3Scale(Vector3RotateByQuaternion((Vector3){0.0, 0.0, 1.0}, entity->rotation), 500.0)
+		),
+		BLUE
+	);
+
+	// The fucking debug line.
+	//DrawLine3D(entity->position, ((Sergente*)entity->data)->target, BLUE);
+}
+
+void comeBackToPlayerSergente(Game * game, Entity * entity) {
+	Entity * player = getEntityFromWorld(game->world, 0);
+	Sergente * data = (Sergente*)entity->data;
+
+	data->target = Vector3Subtract(player->position, entity->position);
+	data->target = Vector3Scale(data->target, SERGENTE_COME_BACK_PERCENT);
+	data->target = Vector3Add(entity->position, data->target);
+}
+
+void createSergenteTarget(Game * game, Entity * entity) {
+	Sergente * data = (Sergente*)entity->data;
+	Entity * player = getEntityFromWorld(game->world, 0);
+
+	// To far away.
+	if (Vector3Distance(entity->position, player->position) >= SERGENTE_COME_BACK_DIS) {
+		comeBackToPlayerSergente(game, entity);
+		return;
+	}
+
+	SetRandomSeed(time(NULL));
+
+	// Set target direction.
+	data->target = (Vector3){
+		GetRandomValue(-UCHAR_MAX, UCHAR_MAX),
+		GetRandomValue(-UCHAR_MAX, UCHAR_MAX),
+		GetRandomValue(-UCHAR_MAX, UCHAR_MAX)
+	};
+
+	data->target = Vector3Normalize(data->target);
+
+	// Scale target and transform.
+	float dis = GetRandomValue(SERGENTE_TARGET_DIS_MIN, SERGENTE_TARGET_DIS_MAX);
+	data->target = Vector3Scale(data->target, dis);
+	data->target = Vector3Add(entity->position, data->target);
 }
diff --git a/src/entities/sergente.h b/src/entities/sergente.h
index 15d4c0d..9ef4e0f 100644
--- a/src/entities/sergente.h
+++ b/src/entities/sergente.h
@@ -4,9 +4,24 @@
 #ifndef SERGENTE_H
 #define SERGENTE_H
 
+#define SERGENTE_TARGET_DIS_MIN 1
+#define SERGENTE_TARGET_DIS_MAX 100
+#define SERGENTE_NEXT_POINT_THRESHOLD 0.1
+#define SERGENTE_COME_BACK_DIS 200.0
+#define SERGENTE_COME_BACK_PERCENT 0.5
+#define SERGENTE_ROTATION_SPEED 40.0
+
+typedef struct Sergente {
+	EntityFlyToPointInfo flyToPoint;
+	Vector3 target;
+} Sergente;
+
 void initSergente(Entity * entity, Game * game);
 void closeSergente(Entity * entity);
 void updateSergente(Game * game, Entity * entity);
 void drawSergente(Game * game, Entity * entity);
 
+// Sets target to random.
+void createSergenteTarget(Game * game, Entity * entity);
+
 #endif
diff --git a/src/entities/soldato.c b/src/entities/soldato.c
index f4ef912..46a0e23 100644
--- a/src/entities/soldato.c
+++ b/src/entities/soldato.c
@@ -37,13 +37,15 @@ void initSoldato(Entity * entity, Game * game) {
 		.controller.bangbang.speed = 50,
 		.controller.bangbang.stopAt = 0.0,
 		.controlType = ENTITY_FLY_TO_POINT_BANG_BANG,
-		.rotationSpeed = 50.0
+		.rotationSpeed = 50.0,
+		.applyRotation = true
 	};
 
 	data->flyToPointFollowing = (EntityFlyToPointInfo){
 		.controller.speedPID = createPID(followingPID),
 		.controlType = ENTITY_FLY_TO_POINT_PID,
-		.rotationSpeed = 50.0
+		.rotationSpeed = 50.0,
+		.applyRotation = true
 	};
 }