X-Git-Url: http://git.localhorst.tv/?p=space.git;a=blobdiff_plain;f=src%2Fai%2FAutopilot.cpp;fp=src%2Fai%2FAutopilot.cpp;h=0cc281bc674c2d9fbc9734c61c84b9091290df82;hp=d23d319f085f2504788383ed210721a144d8faa9;hb=501ffe20da16eaab69e668871001f735697c4a42;hpb=1cb5ed22d7772abe6f9893be90f26f46dbde39f7 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); } }