Chatgpt saved my ass again

11 files changed, 85 insertions, 38 deletions

diff --git a/src/PID.c b/src/PID.c
index edecd5e..5349358 100644
--- a/src/PID.c
+++ b/src/PID.c
@@ -1,4 +1,5 @@
 #include "PID.h"
+#include "util.h"
 
 PID createPID(PIDConfig config) {
 	PID pid = (PID){
@@ -8,6 +9,7 @@ PID createPID(PIDConfig config) {
 		.kP = config.kP,
 		.kI = config.kI,
 		.kD = config.kD,
+		.angleMode = config.angleMode,
 		.doClamp = config.doClamp,
 		.min = config.min,
 		.max = config.max,
@@ -21,7 +23,10 @@ PID createPID(PIDConfig config) {
 
 float runPID(float setpoint, float processValue, PID * pid) {
 	// Get error.
-	pid->error = setpoint - processValue;
+	if (pid->angleMode)
+		pid->error = angleDis(setpoint, processValue);
+	else
+		pid->error = setpoint - processValue;
 
 	// Set p, i and d.
 	pid->p = pid->error * pid->kP;
@@ -49,3 +54,12 @@ void resetPID(PID * pid) {
 	pid->pastError = 0.0;
 	pid->output = 0.0;
 }
+
+float angleDis(float a, float b) {
+	float dir = b - a;
+
+	if (fabsf(dir) > (PI/2))
+		dir = -(signum(dir) * PI) + dir;
+
+	return dir;
+}
diff --git a/src/PID.h b/src/PID.h
index 649a113..03eadf3 100644
--- a/src/PID.h
+++ b/src/PID.h
@@ -10,6 +10,8 @@ typedef struct PIDConfig {
 	float kI;
 	float kD;
 
+	// Angles are fucking weird.
+	bool angleMode;
 	bool doClamp;
 	float min;
 	float max;
@@ -24,6 +26,7 @@ typedef struct PID {
 	float kI;
 	float kD;
 
+	bool angleMode;
 	bool doClamp;
 	float min;
 	float max;
@@ -38,4 +41,7 @@ PID createPID(PIDConfig config);
 float runPID(float setpoint, float processValue, PID * pid);
 void resetPID(PID * pid);
 
+// Angle shit.
+float angleDis(float a, float b);
+
 #endif
diff --git a/src/entities/mussolini.c b/src/entities/mussolini.c
index 2770d2f..9ea54c7 100644
--- a/src/entities/mussolini.c
+++ b/src/entities/mussolini.c
@@ -1,32 +1,44 @@
 #include "mussolini.h"
 #include "assets.h"
 #include "game.h"
+#include "util.h"
 
 void initMussolini(Entity * entity, Game * game) {
 	entity->model = &game->assets.models[MUSSOLINI_ASSET];
+
+	// PID configs.
+	PIDConfig stickPIDConfig = {
+		.kP = 0.5,
+		.kI = 0.0,
+		.kD = 0.0,
+		.angleMode = false,
+		.doClamp = false,
+		.min = 0.0,
+		.max = 0.0
+	};
+
+	// Allocate data.
+	entity->data = KF_MALLOC(sizeof(Mussolini));
+
+	if (entity->data == NULL) {
+		ALLOCATION_ERROR;
+		return;
+	}
+
+	Mussolini * data = (Mussolini*)entity->data;
+	data->xStickPID = createPID(stickPIDConfig);
+	data->yStickPID = createPID(stickPIDConfig);
+	data->zStickPID = createPID(stickPIDConfig);
 }
 
 void closeMussolini(Entity * entity) {
+	if (entity->data != NULL)
+		KF_FREE(entity->data);
 }
 
 void updateMussolini(Game * game, Entity * entity) {
-	Entity * player = getEntityFromWorld(game->world, 0);
-
-	float pitch = Vector2Angle(
-		(Vector2){entity->position.y, entity->position.x},
-		(Vector2){player->position.y, player->position.x}
-	);
-
-	float yaw = Vector2Angle(
-		(Vector2){entity->position.x, entity->position.z},
-		(Vector2){player->position.x, player->position.z}
-	);
-
-	entity->rotation = QuaternionFromEuler(
-		pitch - (PI/2),
-		-(yaw - (PI/2)),
-		0.0
-	);
+	Entity * ship = getEntityFromWorld(game->world, 0);
+	Mussolini * data = (Mussolini*)entity->data;
 }
 
 void drawMussolini(Game * game, Entity * entity) {
diff --git a/src/entities/mussolini.h b/src/entities/mussolini.h
index ee595f5..46520a7 100644
--- a/src/entities/mussolini.h
+++ b/src/entities/mussolini.h
@@ -1,9 +1,16 @@
 #include "gameCommon.h"
 #include "entity.h"
+#include "PID.h"
 
 #ifndef MUSSOLINI_H
 #define MUSSOLINI_H
 
+typedef struct Mussolini {
+	PID xStickPID;
+	PID yStickPID;
+	PID zStickPID;
+} Mussolini;
+
 void initMussolini(Entity * entity, Game * game);
 void closeMussolini(Entity * entity);
 void updateMussolini(Game * game, Entity * entity);
diff --git a/src/entities/soldato.c b/src/entities/soldato.c
index da8df84..f19faa7 100644
--- a/src/entities/soldato.c
+++ b/src/entities/soldato.c
@@ -3,14 +3,27 @@
 
 void initSoldato(Entity * entity, Game * game) {
 	entity->model = &game->assets.models[SOLDATO_ASSET];
-	entity->velocity.angularVelocity = (AxisAngle){(Vector3){1.0, 1.0, 1.0}, 1.0};
 }
 
 void closeSoldato(Entity * entity) {
 }
 
 void updateSoldato(Game * game, Entity * entity) {
-	entityUpdateRotation(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);
 }
 
 void drawSoldato(Game * game, Entity * entity) {
diff --git a/src/entity.c b/src/entity.c
index 0d91ac8..ee15d18 100644
--- a/src/entity.c
+++ b/src/entity.c
@@ -146,7 +146,7 @@ void entityJoystickControl(Entity * entity, Vector3 stick, float speed) {
 	entity->velocity.velocity = (Vector3){
 		m.m2 * s,
 		m.m6 * s,
-		m.m10 * s,
+		m.m10 * s
 	};
 
 	entityUpdatePosition(entity);
diff --git a/src/game.c b/src/game.c
index f916584..8096779 100644
--- a/src/game.c
+++ b/src/game.c
@@ -26,12 +26,7 @@ void initGame(Game * game) {
 	// Debug.
 	WorldEntry entries[] = {
 		(WorldEntry){ENTITY_ANTIFA, Vector3Zero(), QuaternionIdentity()},
-		(WorldEntry){ENTITY_SOLDATO, (Vector3){20.0, 20.0, 20.0}, QuaternionIdentity()},
-		(WorldEntry){ENTITY_CAPORALE, (Vector3){30.0, 30.0, 30.0}, QuaternionIdentity()},
-		(WorldEntry){ENTITY_SERGENTE, (Vector3){40.0, 40.0, 40.0}, QuaternionIdentity()},
-		(WorldEntry){ENTITY_MARESCIALLO, (Vector3){50.0, 50.0, 50.0}, QuaternionIdentity()},
-		(WorldEntry){ENTITY_GENERALE, (Vector3){60.0, 60.0, 60.0}, QuaternionIdentity()},
-		(WorldEntry){ENTITY_MUSSOLINI, (Vector3){0.0, 0.0, 150.0}, QuaternionIdentity()}
+		(WorldEntry){ENTITY_SOLDATO, (Vector3){0.0, 0.0, 10.0}, QuaternionIdentity()}
 	};
 
 	addEntriesToWorld(
diff --git a/src/playerCamera.c b/src/playerCamera.c
index fd7ffac..2bbbdc0 100644
--- a/src/playerCamera.c
+++ b/src/playerCamera.c
@@ -24,9 +24,13 @@ 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};
 }
diff --git a/src/settings.c b/src/settings.c
index d58b222..831ccc3 100644
--- a/src/settings.c
+++ b/src/settings.c
@@ -2,7 +2,7 @@
 
 void initSettings(Settings * settings) {
 	*settings = (Settings){
-		.controlMode = KEYBOARD_AND_MOUSE_CONTROL,
+		.controlMode = JOYSTICK_CONTROL,
 		.mouseSensitivity = 0.05,
 		.scrollBarSpeed = 10.0,
 		.gamePadNum = 0,
diff --git a/src/util.c b/src/util.c
index 10d4c09..e39478c 100644
--- a/src/util.c
+++ b/src/util.c
@@ -13,13 +13,6 @@ float signum(float n) {
 		return 0.0;
 }
 
-float closestAngle(float a1, float a2) {
-	float a = fmodf(a1, PI);
-	float b = fmodf(a2, PI);
-	float dir = b - a;
-
-	if (fabsf(dir) > (PI/2))
-		dir = -(signum(dir) * PI) + dir;
-
-	return dir;
+void printVector3(Vector3 v) {
+	printf("%f %f %f\n", v.x, v.y, v.z);
 }
diff --git a/src/util.h b/src/util.h
index 1d013f7..817d472 100644
--- a/src/util.h
+++ b/src/util.h
@@ -17,7 +17,10 @@ typedef struct AxisAngle {
 
 AxisAngle AxisAngleIdentity();
 
+// boring math shit.
 float signum(float n);
-float closestAngle(float a1, float a2);
+
+// Debugging stuff.
+void printVector3(Vector3 v);
 
 #endif