I've got a unit and a series of points in 3D space which approximate the path it's been given. However, I'm not sure where to go from there. The unit has certain physical constraints, like, it can only turn so fast, it can only accelerate so fast/decelerate so quickly, etc. I thought that this would be the simple part and finding the path would be the hard part, but apparently it's not. Here's the code I've written:

```
void Sim::Context::tick() {
    auto t = 1.0f / Sim::ticks_per_sec;
    std::for_each(players.begin(), players.end(), [&](std::unique_ptr<Sim::Player>& p) {
        auto units = p->GetOwnedUnits();
        std::for_each(units.begin(), units.end(), [&](Sim::Unit* unit) {
            auto t = 1.0f / Sim::ticks_per_sec;
            auto u = unit->GetVelocity();
            auto a = unit->GetAcceleration();
            if (u == 0 && a == 0)
                return;
            auto rotated_heading = Math::Vector(0, 0, 1) * unit->GetRotation();
            auto bp = unit->GetBlueprint();
            auto v = t * a + u;
            if (v > bp->MaxVelocity) { // Do a more accurate lerp since we went over max velocity
                // leerrrppppp
                auto frac = (bp->MaxVelocity - u) * (t / (v - u));
                auto total_increase = (u * t) + (0.5f * a * frac * frac) + (bp->MaxVelocity * (t - frac));
                unit->SetPosition((rotated_heading * total_increase) + unit->GetPosition());
                unit->GetOwningPlayer()->OnUnitPositionChange(unit, unit->GetPosition());
                unit->SetVelocity(bp->MaxVelocity);
                return;
            }
            auto increase_due_to_velocity = u * t;
            auto increase_due_to_acceleration = 0.5f * a * t * t;
            auto rotated_increase = rotated_heading * increase_due_to_acceleration;
            unit->SetPosition(rotated_increase + unit->GetPosition());
            unit->SetVelocity(v);
            unit->GetOwningPlayer()->OnUnitPositionChange(unit, unit->GetPosition());
        });
    });
    for(auto orderit = UnitOrders.begin();orderit != UnitOrders.end();) {
        if (auto move = dynamic_cast<Move*>(orderit->second.get())) {
            auto unit = orderit->first;            
            if (Math::Length(unit->GetPosition() - move->target) < 2) { // Close enough to target
                orderit = UnitOrders.erase(orderit);
                unit->SetAcceleration((-4 * unit->GetVelocity()) / (t * t)); // Let the setter implement the cap
                continue;
            }
            auto path = GetPath(unit->GetPosition(), move->target, unit);
            if (path.size() == 1) {
                orderit++;
                continue;
            }
            auto target_heading = Math::Normalize(path[1] - unit->GetPosition());
            auto current_heading = Math::Normalize(Math::Vector(0, 0, 1) * unit->GetRotation());
            auto axis = Math::Normalize(Math::Cross(current_heading, target_heading)); // Every path has at least two positions- a beginning and an end.
            auto dot_value = Math::Dot(target_heading, current_heading);
            if (dot_value > 1)
                dot_value = 1;
            if (dot_value < -1)
                dot_value = -1;
            auto degrees_angle = std::acos(dot_value);
            auto angle = glm::degrees(degrees_angle);
            if (target_heading == current_heading) {
                unit->SetAcceleration(unit->GetBlueprint()->MaxAcceleration);
                orderit++;
                continue;
            }
            if (std::abs(angle) > (unit->GetBlueprint()->MaxTurnRate * t)) {
                angle = unit->GetBlueprint()->MaxTurnRate * t;
                // Not done turning, slow down as much as possible.
                unit->SetAcceleration((-4 * unit->GetVelocity()) / (t * t)); // Let the setter implement the cap
            } else {
                // Done turning- let's rock on.
                unit->SetAcceleration(unit->GetBlueprint()->MaxAcceleration);
            }
            auto added_rotation = Math::RotateAxis(angle, axis);
            auto total_rotation = unit->GetRotation() * added_rotation;
            if (total_rotation.x != total_rotation.x)
                __debugbreak();
            unit->SetRotation(total_rotation);
            orderit++;
        }
    }
}

```

Sometimes the units rotate to move backwards towards the target, instead of forwards.

Sometimes the units rotate to face a few degrees away from the target, backwards or forwards. The exact value of a "few" is quite variant.

The units turn to face the target, and then move towards it. This is by design, but what's not is the several-second gap in the middle- but only the first time the unit is ordered to move. After that it's perfectly responsive.

I don't really understand how this can go wrong- the logic is simple. As you can see, I split most of my processes up into many intermediate variables so that I can debug their values, but the truth is that when I look, they seem fine. For example, the `target_heading` and `current_heading` variables are equal (or close to) when the unit is facing the wrong direction, but moving in the right direction. This definitely puzzles me.

Any suggestions as to where I can look for the source of the errors?