From 501ffe20da16eaab69e668871001f735697c4a42 Mon Sep 17 00:00:00 2001 From: Daniel Karbach Date: Mon, 30 Dec 2013 14:48:11 +0100 Subject: [PATCH] less sucky autopilot --- src/ai/Autopilot.cpp | 250 ++++++++++++++++++++++++++++++++-------- src/ai/Autopilot.h | 47 +++++++- src/app/Application.cpp | 22 ++++ src/app/Application.h | 4 + src/graphics/Vector.h | 4 + 5 files changed, 278 insertions(+), 49 deletions(-) diff --git a/src/ai/Autopilot.cpp b/src/ai/Autopilot.cpp index d23d319..0cc281b 100644 --- a/src/ai/Autopilot.cpp +++ b/src/ai/Autopilot.cpp @@ -6,8 +6,7 @@ #include "../graphics/Canvas.h" #include "../graphics/Color.h" -#include -using namespace std; +#include namespace space { @@ -20,36 +19,165 @@ Autopilot::Autopilot(Ship &ctrl, const Vector &target) void Autopilot::Update(float deltaT) { - const Vector deltaP(*target - ctrl->pos); - const float distance = Length(deltaP); - const Vector deltaPnorm(deltaP / distance); + // reset + ctrl->linThrottle = 0; + ctrl->rotThrottle = 0; - const Vector dir = ctrl->Dir(); + // cache + dt = deltaT; + dp = *target - ctrl->pos; + dist = Length(dp); + normDP = dp / dist; + speed = Length(ctrl->vel); + normVel = ctrl->vel / speed; + proj = ctrl->pos + (normVel * (DistanceToHalt() * 2)); + + if (Distance() < 0.75) { + if (DistanceToHalt() < Distance() + (speed * dt)) { + if (StandingStill()) { + Face(-normDP); + } else { + Face(-normVel); + } + } else { + Halt(); + } + return; + } + + if (StandingStill()) { + FaceTarget(); + AccelerateAlong(normDP); + return; + } + + const Vector projDiff = *target - proj; + const float projDist = Length(projDiff); + const Vector normProj = projDiff / projDist; + const float onProj = Dot(normProj, normVel); - const float faceTarget = Dot(deltaPnorm, dir); - const float maxAcc = faceTarget < 0 - ? ctrl->MaxFwdAcc() - : ctrl->MaxRevAcc(); + if (projDist <= 0.75 || (onProj < -0.9 && !StandingStill())) { + Halt(); + return; + } + + Face(normProj); + AccelerateAlong(normProj); + return; + + if (OnTarget()) { + if (FacingTarget()) { + if (FarAway()) { + ctrl->linThrottle = 1; + } else if (ReallyClose() && Speed() <= ctrl->MaxFwdAcc()) { + ctrl->linThrottle = Speed() / ctrl->MaxRevAcc() * -1; + } else if (InBrakingDistance()) { + ctrl->linThrottle = -1; + } else { + ctrl->linThrottle = 1; + } + } else if (FacingOpposite()) { + if (FarAway()) { + FaceTarget(); + } else if (ReallyClose() && Speed() <= ctrl->MaxFwdAcc()) { + ctrl->linThrottle = Speed() / ctrl->MaxFwdAcc(); + } else if (InBrakingDistance()) { + ctrl->linThrottle = 1; + } else { + ctrl->linThrottle = -1; + } + } else { + FaceTarget(); + } + return; + } - const float speed = Length(ctrl->vel); - const Vector velNorm = ctrl->vel / speed; - const float onTarget = Dot(deltaPnorm, velNorm); + const Vector correction = + *target - ctrl->pos - (normVel * Distance()); - const float linTTH = speed / maxAcc; - const float linDTH = (maxAcc * linTTH * linTTH) / 2; + const Vector normCorrection = Norm(correction); + float faceCorrection = Dot(normCorrection, ctrl->Dir()); + + Face(normCorrection); + ctrl->linThrottle = faceCorrection * faceCorrection; +} + +bool Autopilot::StandingStill() const { + return ctrl->vel.x * dt < std::numeric_limits::epsilon() + && ctrl->vel.y * dt < std::numeric_limits::epsilon(); +} + +bool Autopilot::ReallySlow() const { + return Speed() < ctrl->MaxFwdAcc() * dt; +} + +bool Autopilot::FacingTarget() const { + return Facing(normDP); +} + +bool Autopilot::FacingOpposite() const { + return Facing(-normDP); +} + +bool Autopilot::Facing(Vector v) const { + return std::abs(Dot(ctrl->Dir(), v) - 1) + < std::numeric_limits::epsilon(); +} + +bool Autopilot::OnTarget() const { + return std::abs(Dot(Norm(ctrl->vel), Norm(*target - ctrl->pos)) - 1) + < std::numeric_limits::epsilon(); +} + + +float Autopilot::Speed() const { + return speed; +} +float Autopilot::Distance() const { + return dist; +} - planFrom = Vector(ctrl->pos); - planTo = Vector(*target); - if (speed > 0 && onTarget < 1) { - planFrom += velNorm * linDTH; - planTo -= velNorm * linDTH; +float Autopilot::DistanceToHalt() const { + if (StandingStill()) { + return 0; } - const Vector plan(planTo - planFrom); - const float planLen = Length(plan); - const Vector planNorm(plan / planLen); + const float timeToHalt = Speed() / ctrl->MaxFwdAcc(); + const float angDiff = std::acos(Dot(normVel, ctrl->Dir())); + const float timeToTurn = std::sqrt(angDiff / ctrl->MaxRotAcc()); + return (ctrl->MaxFwdAcc() * timeToHalt * timeToHalt) / 2 + + timeToTurn * Speed(); +} + +bool Autopilot::ReallyClose() const { + return Distance() <= ctrl->MaxFwdAcc(); +} + +bool Autopilot::InBrakingDistance() const { + return Distance() <= DistanceToHalt(); +} + +bool Autopilot::FarAway() const { + return Distance() > 1.5 * DistanceToHalt(); +} + + +float Autopilot::TargetVelAngle() const { + return std::atan2(normDP.y, normDP.x) - std::atan2(normVel.y, normVel.x); +} + + +void Autopilot::FaceTarget() { + Face(Norm(*target - ctrl->pos)); +} + +void Autopilot::Face(Vector tgt) { + const Vector dir = ctrl->Dir(); - float facePlan = Dot(planNorm, dir); - ctrl->linThrottle = facePlan * facePlan; + float angle = std::atan2(tgt.y, tgt.x) - std::atan2(dir.y, dir.x); + if (angle > PI) angle -= PI2; + if (angle < -PI) angle += PI2; + const float absAngle = std::abs(angle); + const int sigAngle = sigma(angle); const float absRotVel = std::abs(ctrl->rotVel); const int sigRotVel = sigma(ctrl->rotVel); @@ -57,29 +185,63 @@ void Autopilot::Update(float deltaT) { const float rotTTH = absRotVel / rotMaxAcc; const float rotDTH = (rotMaxAcc * rotTTH * rotTTH) / 2; - float planAngle = - std::atan2(planNorm.y, planNorm.x) - std::atan2(dir.y, dir.x); - if (planAngle > PI) planAngle -= PI2; - if (planAngle < -PI) planAngle += PI2; - const float absPlanAngle = std::acos(facePlan); - const int sigPlanAngle = sigma(planAngle); - // pos rot = clockwise // neg rot = counter clockwise - if (sigPlanAngle == 0) { - // pointing straight at the plan - } else if (sigPlanAngle == sigRotVel) { + if (sigAngle == 0) { + if (absRotVel > 0) { + if (absRotVel <= rotMaxAcc) { + ctrl->rotThrottle = -sigRotVel * absRotVel / rotMaxAcc; + } else { + ctrl->rotThrottle = -sigRotVel; + } + } else { + // done + ctrl->rotThrottle = 0; + } + } else if (sigAngle == sigRotVel) { // turning in the right direction - if (rotDTH >= absPlanAngle) { + if (rotDTH >= absAngle) { // overshooting - ctrl->rotThrottle = -sigPlanAngle; + if (absRotVel <= rotMaxAcc) { + ctrl->rotThrottle = -sigAngle * absRotVel / rotMaxAcc; + } else { + ctrl->rotThrottle = -sigAngle; + } } else { - ctrl->rotThrottle = sigPlanAngle; + ctrl->rotThrottle = sigAngle; } } else { // turning in the wrong direction - ctrl->rotThrottle = sigPlanAngle; + ctrl->rotThrottle = sigAngle; + } +} + +void Autopilot::Halt() { + if (StandingStill()) { + Face(normDP); + return; + } + + Vector nnVel = Norm(-normVel); + Face(nnVel); + if (ReallySlow()) { + if (Facing(nnVel)) { + ctrl->linThrottle = Speed() / ctrl->MaxFwdAcc(); + } else if (Facing(-nnVel)) { + ctrl->linThrottle = -Speed() / ctrl->MaxFwdAcc(); + } + } else { + AccelerateAlong(nnVel); + } +} + +void Autopilot::AccelerateAlong(Vector v) { + const float pointing = Dot(v, ctrl->Dir()); + if (std::abs(pointing) > 0.7071) { + ctrl->linThrottle = pointing * pointing * sigma(pointing); + } else { + ctrl->linThrottle = 0; } } @@ -91,20 +253,16 @@ void Autopilot::Render(Canvas &canv, const Camera &cam) const { Vector screenPos(cam.ToScreen(ctrl->pos)); Vector screenTgt(cam.ToScreen(*target)); - Vector screenPlanFrom(cam.ToScreen(planFrom)); - Vector screenPlanTo(cam.ToScreen(planTo)); + Vector screenProj(cam.ToScreen(proj)); canv.SetColor(tgtColor); canv.Arrow(screenPos, screenTgt); canv.SetColor(velColor); - canv.Arrow(screenPos, screenPlanFrom); - canv.Arrow(screenTgt, screenPlanTo); + canv.Arrow(screenPos, screenProj); canv.SetColor(planColor); - canv.Arrow(screenPlanFrom, screenPlanTo); - - cout << "max acc: " << ctrl->MaxFwdAcc() << ", distance: " << Length(*target - ctrl->pos) << endl; + canv.Arrow(screenProj, screenTgt); } } diff --git a/src/ai/Autopilot.h b/src/ai/Autopilot.h index b15448e..1a814e6 100644 --- a/src/ai/Autopilot.h +++ b/src/ai/Autopilot.h @@ -20,13 +20,54 @@ public: void Render(Canvas &, const Camera &) const; +private: + // velocity is so small that the next integration makes no difference + bool StandingStill() const; + // can halt within the current frame + bool ReallySlow() const; + // can halt within one second + bool Slow() const; + + // pointing exactly at target + bool FacingTarget() const; + // pointing exactly away from target + bool FacingOpposite() const; + // ship pointing in direction of given normalized vector + bool Facing(Vector) const; + bool OnTarget() const; + + float Speed() const; + float Distance() const; + float DistanceToHalt() const; + bool ReallyClose() const; + bool InBrakingDistance() const; + bool FarAway() const; + + float TargetVelAngle() const; + + // point the ship towards the target + void FaceTarget(); + // point ship into direction of given normalized vector + void Face(Vector); + // stop the ship asap + void Halt(); + // accelerate if it improves moving in given normalized direction + void AccelerateAlong(Vector); + private: Ship *ctrl; const Vector *target; - // cache members for debug drawing - Vector planFrom; - Vector planTo; + float dt; + + Vector dp; + float dist; + Vector normDP; + + float speed; + Vector normVel; + + Vector proj; }; diff --git a/src/app/Application.cpp b/src/app/Application.cpp index e3c6a2b..b69cc85 100644 --- a/src/app/Application.cpp +++ b/src/app/Application.cpp @@ -12,6 +12,20 @@ Application::Application(Canvas &c) , focus(Vector(500, 500), 500) , cam(c.Size(), focus.Pos()) , controlled(univ.AddShip(Ship())) +, linGauge( + Vector(15, 100), + Vector(10, 10), + Color(0xFF, 0xFF, 0xFF), + Color(0x00, 0x00, 0x00), + Color(0x00, 0xFF, 0x00), + Color(0xFF, 0x00, 0x00)) +, rotGauge( + Vector(15, 100), + Vector(27, 10), + Color(0xFF, 0xFF, 0xFF), + Color(0x00, 0x00, 0x00), + Color(0x33, 0x33, 0xFF), + Color(0x00, 0x00, 0xFF)) , autopilot(*controlled, focus.Pos()) , apEnabled(false) , last(SDL_GetTicks()) @@ -34,6 +48,11 @@ void Application::Run() { void Application::Loop(int delta) { HandleEvents(); + if (delta == 0) { + SDL_Delay(1); + return; + } + if (!paused) { Update(delta); } @@ -200,6 +219,9 @@ void Application::Render() { } autopilot.Render(canvas, cam); + + linGauge.Render(canvas, controlled->linThrottle); + rotGauge.Render(canvas, controlled->rotThrottle); } } diff --git a/src/app/Application.h b/src/app/Application.h index 4d2dd2d..17fdce8 100644 --- a/src/app/Application.h +++ b/src/app/Application.h @@ -5,6 +5,7 @@ #include "../graphics/Camera.h" #include "../graphics/Moveable.h" #include "../graphics/Vector.h" +#include "../ui/DualGauge.h" #include "../world/Universe.h" #include @@ -45,6 +46,9 @@ private: Ship *controlled; Vector control; + DualGauge linGauge; + DualGauge rotGauge; + Autopilot autopilot; bool apEnabled;; diff --git a/src/graphics/Vector.h b/src/graphics/Vector.h index 4cf920b..97dfd4a 100644 --- a/src/graphics/Vector.h +++ b/src/graphics/Vector.h @@ -154,6 +154,10 @@ template constexpr Scalar Length(Vector v) { return std::sqrt(Dot(v, v)); } +template +constexpr Vector Norm(Vector v) { + return v / Length(v); +} template constexpr Vector Rotate90(Vector v) { -- 2.39.2