More following shit

1 files changed, 43 insertions, 0 deletions

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;
+}