More following shit

5 files changed, 92 insertions, 15 deletions

diff --git a/src/entities/soldato.c b/src/entities/soldato.c
index f19faa7..30dfa6b 100644
--- a/src/entities/soldato.c
+++ b/src/entities/soldato.c
@@ -3,27 +3,52 @@
 
 void initSoldato(Entity * entity, Game * game) {
 	entity->model = &game->assets.models[SOLDATO_ASSET];
+
+	// Acceleration.
+	entity->useAcceleration = true;
+	entity->acceleration = (EntityAcceleration){
+		.speedUp = 5.0,
+		.speedDown = 0.00001
+	};
+
+	// PID configs.
+	PIDConfig speedPIDConfig = {
+		.kP = -0.5,
+		.kI = 0.0,
+		.kD = 0.0,
+		.angleMode = false,
+		.doClamp = true,
+		.min = 0.0,
+		.max = 30.0
+	};
+
+	// Allocate data.
+	entity->data = KF_MALLOC(sizeof(Soldato));
+
+	if (entity->data == NULL) {
+		ALLOCATION_ERROR;
+		return;
+	}
+
+	Soldato * data = (Soldato*)entity->data;
+	data->speedPID = createPID(speedPIDConfig);
 }
 
 void closeSoldato(Entity * entity) {
+	if (entity->data != NULL)
+		KF_FREE(entity->data);
 }
 
 void updateSoldato(Game * game, Entity * entity) {
 	Entity * player = getEntityFromWorld(game->world, 0);
-
-	// Get direction.
-	Vector3 direction = Vector3Subtract(entity->position, player->position);
-	direction = Vector3Normalize(direction);
-
-	// Get look at and rotation.
-	Matrix matrix = MatrixLookAt(Vector3Zero(), direction, (Vector3){0, 1, 0});
-	Quaternion rotation = QuaternionFromMatrix(matrix);
-	rotation = QuaternionInvert(rotation);
-
-	entity->rotation = rotation;
-
-	entity->velocity.velocity = Vector3Scale(direction, -10.0);
-	entityUpdatePosition(entity);
+	Soldato * data = (Soldato*)entity->data;
+
+	entityFlightToPoint(
+		entity,
+		player->position,
+		&data->speedPID,
+		5.0
+	);
 }
 
 void drawSoldato(Game * game, Entity * entity) {
diff --git a/src/entities/soldato.h b/src/entities/soldato.h
index 6992e00..5123aee 100644
--- a/src/entities/soldato.h
+++ b/src/entities/soldato.h
@@ -1,9 +1,14 @@
 #include "gameCommon.h"
 #include "entity.h"
+#include "PID.h"
 
 #ifndef SOLDATO_H
 #define SOLDATO_H
 
+typedef struct Soldato {
+	PID speedPID;
+} Soldato;
+
 void initSoldato(Entity * entity, Game * game);
 void closeSoldato(Entity * entity);
 void updateSoldato(Game * game, Entity * entity);
diff --git a/src/entity.c b/src/entity.c
index ee15d18..e918617 100644
--- a/src/entity.c
+++ b/src/entity.c
@@ -151,3 +151,46 @@ void entityJoystickControl(Entity * entity, Vector3 stick, float speed) {
 
 	entityUpdatePosition(entity);
 }
+
+void entityFlightToPoint(Entity * entity, Vector3 point, PID * speedPID, float rotationSpeed) {
+	float t = GetFrameTime();
+
+	// Get distance and direction.
+	Vector3 dis = Vector3Subtract(entity->position, point);
+	Vector3 direction = Vector3Normalize(dis);
+
+	// Get look at and rotation.
+	Matrix matrix = MatrixLookAt(Vector3Zero(), direction, (Vector3){0.0, 1.0, 0.0});
+	Quaternion rotation = QuaternionInvert(QuaternionFromMatrix(matrix));
+
+	// Rotate this fucker.
+	if (rotationSpeed == 0.0)
+		entity->rotation = rotation;
+	else
+		entity->rotation = QuaternionSlerp(entity->rotation, rotation, t * rotationSpeed);
+
+	// Velocity control.
+	float s = runPID(0.0, Vector3Length(dis), speedPID);
+	Matrix m = QuaternionToMatrix(QuaternionInvert(entity->rotation));
+
+	// Accelerate.
+	if (entity->useAcceleration)
+		s = accelerateValue(
+			s,
+			entity->lastVelocity.speed,
+			entity->acceleration.speedUp * t,
+			entity->acceleration.speedDown * t
+		);
+
+	// Velocity.
+	entity->velocity.velocity = (Vector3){
+		m.m2 * s,
+		m.m6 * s,
+		m.m10 * s
+	};
+
+	entityUpdatePosition(entity);
+
+	entity->velocity.speed = s;
+	entity->lastVelocity = entity->velocity;
+}
diff --git a/src/entity.h b/src/entity.h
index 98db0f5..c6fa6c6 100644
--- a/src/entity.h
+++ b/src/entity.h
@@ -1,5 +1,6 @@
 #include "gameCommon.h"
 #include "util.h"
+#include "PID.h"
 
 #ifndef	ENTITY_H
 #define ENTITY_H
@@ -91,4 +92,7 @@ void entityUpdateRotation(Entity * entity);
 
 void entityJoystickControl(Entity * entity, Vector3 stick, float speed);
 
+// 0.0 rotationSpeed for directly setting the rotation.
+void entityFlightToPoint(Entity * entity, Vector3 point, PID * speedPID, float rotationSpeed);
+
 #endif
diff --git a/src/playerCamera.c b/src/playerCamera.c
index 2bbbdc0..ded59b7 100644
--- a/src/playerCamera.c
+++ b/src/playerCamera.c
@@ -26,11 +26,11 @@ void updatePlayerCamera(Camera3D * camera, Game * game) {
 	camera->position = Vector3Add(camera->position, player->position);
 
 	camera->up = (Vector3){
-
 		m.m1 + m.m2,
 		m.m5 + m.m6,
 		m.m9 + m.m10
 	};
+
 	camera->position = (Vector3){20.0, 20.0, 20.0};
 	camera->up = (Vector3){0.0, 1.0, 0.0};
 }