#include "../graphics/Canvas.h"
#include "../graphics/Color.h"
-#include <iostream>
-using namespace std;
+#include <limits>
namespace space {
void Autopilot::Update(float deltaT) {
- const Vector<float> deltaP(*target - ctrl->pos);
- const float distance = Length(deltaP);
- const Vector<float> deltaPnorm(deltaP / distance);
+ // reset
+ ctrl->linThrottle = 0;
+ ctrl->rotThrottle = 0;
- const Vector<float> 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<float> projDiff = *target - proj;
+ const float projDist = Length(projDiff);
+ const Vector<float> 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<float> velNorm = ctrl->vel / speed;
- const float onTarget = Dot(deltaPnorm, velNorm);
+ const Vector<float> correction =
+ *target - ctrl->pos - (normVel * Distance());
- const float linTTH = speed / maxAcc;
- const float linDTH = (maxAcc * linTTH * linTTH) / 2;
+ const Vector<float> 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<float>::epsilon()
+ && ctrl->vel.y * dt < std::numeric_limits<float>::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<float> v) const {
+ return std::abs(Dot(ctrl->Dir(), v) - 1)
+ < std::numeric_limits<float>::epsilon();
+}
+
+bool Autopilot::OnTarget() const {
+ return std::abs(Dot(Norm(ctrl->vel), Norm(*target - ctrl->pos)) - 1)
+ < std::numeric_limits<float>::epsilon();
+}
+
+
+float Autopilot::Speed() const {
+ return speed;
+}
+float Autopilot::Distance() const {
+ return dist;
+}
- planFrom = Vector<float>(ctrl->pos);
- planTo = Vector<float>(*target);
- if (speed > 0 && onTarget < 1) {
- planFrom += velNorm * linDTH;
- planTo -= velNorm * linDTH;
+float Autopilot::DistanceToHalt() const {
+ if (StandingStill()) {
+ return 0;
}
- const Vector<float> plan(planTo - planFrom);
- const float planLen = Length(plan);
- const Vector<float> 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<float> tgt) {
+ const Vector<float> 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);
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<float> 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<float> v) {
+ const float pointing = Dot(v, ctrl->Dir());
+ if (std::abs(pointing) > 0.7071) {
+ ctrl->linThrottle = pointing * pointing * sigma(pointing);
+ } else {
+ ctrl->linThrottle = 0;
}
}
Vector<float> screenPos(cam.ToScreen(ctrl->pos));
Vector<float> screenTgt(cam.ToScreen(*target));
- Vector<float> screenPlanFrom(cam.ToScreen(planFrom));
- Vector<float> screenPlanTo(cam.ToScreen(planTo));
+ Vector<float> 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);
}
}
projects / space.git / blobdiff