From 77a06748f9f394486cad833e2ca351e8dbcc7361 Mon Sep 17 00:00:00 2001 From: nathansmithsmith Date: Sun, 16 Jul 2023 21:55:38 -0600 Subject: More following shit --- src/entity.c | 43 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) (limited to 'src/entity.c') 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; +} -- cgit v1.2.3