TurnByTurn.cpp

Go to the documentation of this file.

#include "TurnByTurn.h"
#define _USE_MATH_DEFINES
#include <math.h>
#include "DXUT.h"
#include "globals.h"

TurnByTurn::TurnByTurn(Bus* pBus, Logger* pLogger, Networking* pNetworking) {
        bus = pBus;
        logger = pLogger;
        logger->AddToCallStack("TurnByTurn::ctor");
        networking = pNetworking;
}

TurnByTurn::~TurnByTurn() {
        SAFE_RELEASE(m_avRouteMap);
}

void TurnByTurn::Update(bool pIsGrndVehicle, float pElapsedTime) {
        logger->AddToCallStack("TurnByTurn::Update");
        elapsedCounter += pElapsedTime;

        // formatting the display text and Bill's guide arrow idea
        // requesting vocal guidance approach the next node
        // advancing to the next node
        // requesting vocal guidance for new node

        if (pIsGrndVehicle)
        {
                // If we recieved new nodes, we reset the indices
                if (bus->turnByTurnDirty) {
                        closestWaypointIndex = 0;
                        activeWaypointIndex = 0;
                }
                // so your "x" value in the same scale is "(float)tcp.u + assayProgressU"
                // and "y" should be "(float)tcp.v + assayProgressV"
                vehiclePos.x = static_cast<float>(bus->tcp.u) + bus->assayProgressU;
                vehiclePos.y = static_cast<float>(bus->tcp.v) + bus->assayProgressV;
                mapMagnifier = 10.0f / bus->MapScaleInnerRingKm;

                if (!bus->nodes.empty()) {
                        // First we get the distance between us and the current waypoint
                        float closestWaypointDist{ 0.0f };
                        float activeWaypointDist{ 0.0f };
                        try {
                                closestWaypointDist = distanceBetweenPositions(vehiclePos, bus->nodes.at(closestWaypointIndex).position);
                                activeWaypointDist = distanceBetweenPositions(vehiclePos, bus->nodes.at(activeWaypointIndex).position);
                        }
                        catch (std::exception error) {
                                closestWaypointDist = 99.0f;
                                activeWaypointDist = 99.0f;
                        }
                        // Then we check if any waypoint is closer
                        float nodeDist = 9999.0f;
                        Snode node;
                        bool clearNodes = false;
                        for (auto iter = bus->nodes.begin(); iter != bus->nodes.end(); ++iter) {
                                node = *iter;
                                nodeDist = distanceBetweenPositions(vehiclePos, node.position);
                                // If this waypoint is closer, this becomes the active waypoint
                                if (nodeDist < closestWaypointDist) {
                                        closestWaypointIndex = iter - bus->nodes.begin();
                                }
                        }
                        // If our closest waypoint isn't the currently active one or the one we just came from
                        if (closestWaypointIndex != activeWaypointIndex && closestWaypointIndex != activeWaypointIndex - 1) {
                                // if the closestWaypointIndex is smaller then the previous, we reset our navigation to that index
                                if (closestWaypointIndex < activeWaypointIndex - 1) {
                                        //activeWaypointIndex = closestWaypointIndex;
                                }
                                else if (closestWaypointIndex > activeWaypointIndex) {
                                        // We somehow skipped a part, but we forward our active waypoint to the closest one
                                        // activeWaypointIndex = closestWaypointIndex;
                                        //this->sendUpdateToServer(bus->nodes.at(activeWaypointIndex).edgeId, 0.0f, 0, UpdateType::TYPE_NEXT_NODE);
                                        //
                                }
                        }
                        // If we are closer to the waypoint then the threshhold, we reached this waypoint and the next becomes active
                        for (auto iter = bus->nodes.begin() + activeWaypointIndex; iter != bus->nodes.end(); ++iter) {
                                node = *iter;
                                const float dist = distanceBetweenPositions(vehiclePos, node.position);
                                // If we reached our next waypoint, we set the next one as active
                                if (dist < cReachedWaypointThreshold) {
                                        activeWaypointIndex = (iter - bus->nodes.begin());
                                        nextWaypointIndex = (iter - bus->nodes.begin()) + 1;
                                        // Check if this was the last waypoint
                                        if (nextWaypointIndex >= bus->nodes.size() - 1) {
                                                // TODO: Last waypoint reached, finished navigation
                                                activeWaypointIndex = 0;
                                                nextWaypointIndex = 0;
                                                closestWaypointIndex = 0;
                                                clearNodes = true;
                                                this->sendUpdateToServer(0, 0.0f, 0, UpdateType::TYPE_NAVIGATION_COMPLETED);
                                                headingChangeStartWaypointIndex = -1;
                                                headingChangeEndWaypointIndex = -1;
                                        }
                                }
                        }
                        float distanceTraveled = 0.0f;
                        if (lastVehiclePos.x != 0.0f && lastVehiclePos.y != 0.0f) {
                                distanceTraveled = distanceBetweenPositions(vehiclePos, lastVehiclePos);
                        }
                        this->updateHeadingState();
                        if (clearNodes) {
                                bus->nodes.clear();
                        }
                }
                if (bus->turnByTurnDirty && !bus->nodes.empty()) {
                        this->CreateVB(DXUTGetD3D9Device());
                        headingChangeStartWaypointIndex = -1;
                        headingChangeEndWaypointIndex = -1;
                        bus->turnByTurnDirty = false;
                }
                if (elapsedCounter > 0.5f) {
                        this->UpdateVB(DXUTGetD3D9Device());
                        elapsedCounter = 0.0f;
                }
                lastVehiclePos = vehiclePos;
        }
}

void TurnByTurn::Draw(IDirect3DDevice9* pd3dDevice)
{
        logger->AddToCallStack("TurnByTurn::Draw");
        // problem is this line data is not conducive to sprite drawing!
        // problem is this would be perfect in a module but modules don't have vertexbuffers!
        // and you could not simply translate and scale the static buffer since the line should only go from our current location to the next node!
        if (!m_avRouteMap)
                return;

        HRESULT hr;
        D3DVIEWPORT9 mvp, oldmvp;
        // 49,271 and 128x128
        // all of these use displayscaley because the aspect is locked
        mvp.X = 0;
        mvp.Y = 213;
        mvp.Width = 137;
        mvp.Height = 64;
        mvp.MinZ = 0.0f; mvp.MaxZ = 1.0f;
        V(pd3dDevice->GetViewport(&oldmvp));
        V(pd3dDevice->SetViewport(&mvp));

        V(pd3dDevice->SetRenderState(D3DRS_ALPHABLENDENABLE, false));
        V(pd3dDevice->SetTextureStageState(0, D3DTSS_COLOROP, D3DTOP_SELECTARG2));
        V(pd3dDevice->SetStreamSource(0, m_avRouteMap, 0, sizeof(D3DSCREEN)));
        V(pd3dDevice->SetFVF(D3DFVF_SCREEN));
        V(pd3dDevice->DrawPrimitive(D3DPT_LINESTRIP, 0, lineCount));

        V(pd3dDevice->SetViewport(&oldmvp));
}

void TurnByTurn::CreateVB(IDirect3DDevice9* pd3dDevice)
{
        logger->AddToCallStack("TurnByTurn::CreateVB");
        
        if (!bus)
                return;
        
        HRESULT hr;
        SAFE_RELEASE(m_avRouteMap);
        /*V(pd3dDevice->CreateVertexBuffer((bus->nodes.size() + 1) * sizeof(D3DSCREEN),
                D3DUSAGE_WRITEONLY | D3DUSAGE_DYNAMIC, D3DFVF_SCREEN,
                D3DPOOL_DEFAULT, &m_avRouteMap, nullptr));*/
}

void TurnByTurn::UpdateVB(IDirect3DDevice9* pd3dDevice)
{
        // As long as the bus isn't initialized yet, we can't really do anything
        logger->AddToCallStack("TurnByTurn::UpdateVB");

        if (!bus)
                return;

        if (bus->nodes.empty())
                return;

        if (!m_avRouteMap)
                return;

        HRESULT hr;
        D3DSCREEN* vtxs;
        V(m_avRouteMap->Lock(0, 0, reinterpret_cast<void**>(&vtxs), D3DLOCK_DISCARD));

        // Begin node is always the center of the screen
        (*vtxs).x = 68.0f; (*vtxs).y = 248.0f; (*vtxs).z = 0.0f; (*vtxs).w = 1.0f;
        (*vtxs).diffuse = D3DCOLOR_XRGB(255, 255, 255);
        vtxs++;

        Snode node;
        lineCount = 0;
        // 68,236 is the center of the texture which is 907,352 ... which means 
        for (auto iter = bus->nodes.begin() + nextWaypointIndex; iter != bus->nodes.end(); ++iter) 
        {
                node = *iter;
                const D3DXVECTOR2 mapPosition = convertWorldPositionToMapPosition(node.position);
                (*vtxs).x = 68.0f + mapPosition.x; (*vtxs).y = 248.0f + mapPosition.y; (*vtxs).z = 0.0f; (*vtxs).w = 1.0f;
                (*vtxs).diffuse = D3DCOLOR_XRGB(255, 255, 0);
                vtxs++;
                lineCount++;
        }

        V(m_avRouteMap->Unlock());
}

D3DXVECTOR2 TurnByTurn::convertWorldPositionToMapPosition(const D3DXVECTOR2& pPos) {

        // Citymap.dds is 2048,2048 and has 3 x 3 sectors
        // Map starts at tcp.u 519 tcp.v = 1101
        // this means each sector is 682,666667 pixels at 100%
        // One grid tile in CityMap.dds = 170,6666666666667 at 100% 
        // position in a sector is on 0.0 to 1.0f 
        // so we multiply position in sector (0.0 - 1.0) with the pixelsize of a sector to get the pixel position on the CityMap.dds
        // then we have to take zoom into account       
        // center for x = 68.0f;
        // center for y = 248.0f;       
        // so mag is 1 at 10km scale ... but that is actually 138/2048 or 0.673828125km across, 0.3369140625 from the center to edge    

        D3DXVECTOR2 retPos;
        retPos.x = (pPos.x - mapStartTcpU) - (vehiclePos.x - mapStartTcpU);
        retPos.y = (pPos.y - mapStartTcpV) - (vehiclePos.y - mapStartTcpV);
        retPos.x *= (sectorSize * mapMagnifier);
        retPos.y *= (sectorSize * mapMagnifier);
        // somehow we need to offset Y a little bit to make it look good on the map
        retPos.y -= 3.0f;
        return retPos;
}

void TurnByTurn::ReleaseResources()
{
        SAFE_RELEASE(m_avRouteMap);
}

void TurnByTurn::sendUpdateToServer(int pEdgeId, float pDistance, size_t pDirection, size_t pType) {
        // Communication to server
        // So in the packets you send me I need the edgeId of course (unless you don't want it to say the street name because it isn't changing then sent -1)
        // distance ... 
        // direction enumeration (like 0 is straight, 1 is bear left, 2 is turn left, 3 is bear right, 4 is turn right, 5 is u-turn) ...
        // and also what type it is (0=early notice of change, 1=affect change, 2=after change was successful, 3=node closest 5=navigation finished)
        SClientPacket outpacket;
        outpacket.type = 25; // TurnByTurn
        outpacket.f_x = static_cast<float>(pEdgeId); // edgeId
        outpacket.f_y = pDistance; // distance in meters
        outpacket.f_z = static_cast<float>(pDirection); // direction enum 
        outpacket.f_w = static_cast<float>(pType); // type 
        networking->SendToServer(&outpacket, sizeof(SClientPacket), true);
}

void TurnByTurn::LookAhead()
{
        if (bus->nodes.empty()) 
                return;
        
        char msg[255];
        float distanceChecked = 0.0f;
        const float drivingDirection = angleBetweenPositionsDeg(lastVehiclePos, vehiclePos);
        D3DXVECTOR2 lastPosition;
        lastPosition = bus->nodes.at(headingChangeEndWaypointIndex).position;
        distanceUntilFirstHeadingChange = distanceBetweenPositions(vehiclePos, lastPosition);
        size_t iterCounter = 0;
        for (size_t index = headingChangeEndWaypointIndex; index < bus->nodes.size() - 1; ++index) {
                // Connections between most nodes look like this
                // ___ <- A
                //        \ <- B
                //    | <- C
                // If we only A -> B, we will only get a direction change of 45 degrees
                // So by default, you want to check A -> C nodes ahead, and see what direction change that gives you (in this case, closer to 90 degrees)
                // If that is not possible (becase we are at the end of the list) then we take only 1 node ahead
                bool gotChange = false;
                float heading = 0.0;
                float headingDiff = 0.0;
                float distanceIncludingThisLoop = 0.0;
                heading = angleBetweenPositionsDeg(lastPosition, bus->nodes.at(index + 1).position);
                headingDiff = this->getHeadingChange(heading, bus->HeadingTrueDegrees);
                distanceIncludingThisLoop = distanceBetweenPositions(lastPosition, bus->nodes.at(index + 1).position);
                if (index + 2 < bus->nodes.size() && abs(headingDiff) > cDirectionChangeThreshold) {
                        heading = angleBetweenPositionsDeg(bus->nodes.at(index + 1).position, bus->nodes.at(index + 2).position);
                        headingDiff = this->getHeadingChange(heading, bus->HeadingTrueDegrees);
                        distanceIncludingThisLoop += distanceBetweenPositions(bus->nodes.at(index + 1).position, bus->nodes.at(index + 2).position);
                }
                lastPosition = bus->nodes.at(index + 1).position;

                if (abs(headingDiff) > cDirectionChangeThreshold) {
                        gotChange = true;
                        headingChangeStartWaypointIndex = index;
                        headingChangeEndWaypointIndex = index + 1;
                        edgeIdAfterFirstHeadingChange = bus->nodes.at(index + 1).edgeId;
                        firstHeadingChangeDegrees = headingDiff;
                        nextNodeUpdateSent = false;
                        sprintf_s(msg, 255, "Heading diff exceeded threshold! TargetHeading: %f, CurrentHeading: %f, diff: %f, distance: %f, iter: %u, startIndex: %u, endIndex: %u", heading, bus->HeadingTrueDegrees, headingDiff, distanceChecked, iterCounter, headingChangeStartWaypointIndex, headingChangeEndWaypointIndex);
                        this->logger->Log(msg, Logger::Debug);
                        break;
                }

                distanceChecked += distanceIncludingThisLoop;

                if (distanceChecked >= cDistanceToCheckAhead) {
                        break;
                }
                iterCounter++;
        }

        size_t direction = Direction::STRAIGHT;
        if (firstHeadingChangeDegrees < -30.0f && firstHeadingChangeDegrees >= -60.0f) {
                direction = Direction::BEAR_LEFT;
        }
        else if (firstHeadingChangeDegrees < -60.0f && firstHeadingChangeDegrees > -130.0f) {
                direction = Direction::TURN_LEFT;
        }
        else if (firstHeadingChangeDegrees > 30.0f && firstHeadingChangeDegrees <= 60.0f) {
                direction = Direction::BEAR_RIGHT;
        }
        else if (firstHeadingChangeDegrees > 60.0f && firstHeadingChangeDegrees < 130.0f) {
                direction = Direction::TURN_RIGHT;
        }
        else if (firstHeadingChangeDegrees > 130.0f || firstHeadingChangeDegrees < -130.0f) {
                direction = Direction::U_TURN;
        }
        firstHeadingChangeDirection = direction;
}

float TurnByTurn::angleBetweenPositionsDeg(const D3DXVECTOR2& startingPos, const D3DXVECTOR2& targetPos) {
        const D3DXVECTOR2 directionVector = targetPos - startingPos;
        D3DXVECTOR2 normalizedDirVector;
        D3DXVec2Normalize(&normalizedDirVector, &directionVector);
        return vectorToDegrees(normalizedDirVector);
}

float TurnByTurn::vectorToDegrees(const D3DXVECTOR2& vec) {
        const float twoPi = 2.0f * static_cast<float>(M_PI);
        float radians = atan2f(vec.y, vec.x);
        if (radians < 0.0f) {
                return fmodf(twoPi + radians + static_cast<float>(M_PI_2), twoPi) * (180.0f / static_cast<float>(M_PI));
        }
        else
        {
                return fmodf(radians + static_cast<float>(M_PI_2), twoPi) * (180.0f / static_cast<float>(M_PI));
        }
}

float TurnByTurn::distanceBetweenPositions(const D3DXVECTOR2& positionA, const D3DXVECTOR2& positionB) {
        const D3DXVECTOR2 distanceVec = positionB - positionA;
        return D3DXVec2Length(&distanceVec);
}

float TurnByTurn::convertDistanceToMeters(const float dist) {
        // Based on that coordinates 0.0 to 1.0 = 2000meter
        return dist * 2000.0f;
}


float TurnByTurn::getHeadingChange(float pTargetHeading, float pCurrentHeading) {
        float headingChange = pTargetHeading - pCurrentHeading;
        return headingChange = customModulo(headingChange + 180.0f, 360.0f) - 180.0f;
}

/*
* Implemented this function since the standard cmath modulo keeps the - sign which is not what we want
*/
float TurnByTurn::customModulo(float a, float n) {
        return a - floor(a / n) * n;
}

void TurnByTurn::checkForCue() {
        // We don't check for cues if we are at, or past the heading change
        const float maxWarnDist = 0.070f;
        const float minWarnDist = 0.020f;
        const float maxSpeed = 0.040f;
        // linear mapping from 0.0 -> 0.040ms speed to 0.020 -> 0.070 distance
        const float warnDist = ((bus->GroundSpeedKms / maxSpeed) * (maxWarnDist - minWarnDist)) + minWarnDist;
        if (distanceUntilFirstHeadingChange <= 0.0f) {
                return;
        }
        else if (distanceUntilFirstHeadingChange <= warnDist && !nextNodeUpdateSent) {
                // We send a cue when we are at less then 40 meters away from heading change and have not sent the next node update yet
                char msg[255];
                sprintf_s(msg, 255, "Sending next node update to server Direction: %d,  distanceMeters: %f, speed: %f", firstHeadingChangeDirection, distanceUntilFirstHeadingChange, bus->GroundSpeedKms);
                this->logger->Log(msg, Logger::Debug);
                sendUpdateToServer(edgeIdAfterFirstHeadingChange, 30.0f, firstHeadingChangeDirection, UpdateType::TYPE_NEXT_NODE);
                nextNodeUpdateSent = true;
        }
        else if (distanceUntilFirstHeadingChange <= 0.05f && approachingNodeLastDistanceSent > 0.06f) {
                // We send a cue when we are at less then 100 meters away from heading change
                char msg[255];
                sprintf_s(msg, 255, "Sending approaching next node update to server Direction: %d,  distance: %f, edgeId: %d", firstHeadingChangeDirection, distanceUntilFirstHeadingChange, edgeIdAfterFirstHeadingChange);
                this->logger->Log(msg, Logger::Debug);
                sendUpdateToServer(edgeIdAfterFirstHeadingChange, 100.0f, firstHeadingChangeDirection, UpdateType::TYPE_APPROACHING_NEXT_NODE);
                approachingNodeLastDistanceSent = 0.03f;
        }
}

void TurnByTurn::updateHeadingState() {
        char msg[255];
        if (this->headingChangeStartWaypointIndex != -1 && this->headingChangeStartWaypointIndex < bus->nodes.size()) {
                const float angleDegreesHeadingChangeStartWaypoint = this->angleBetweenPositionsDeg(this->vehiclePos, bus->nodes.at(headingChangeStartWaypointIndex).position);
                const float headingChange = this->getHeadingChange(angleDegreesHeadingChangeStartWaypoint, bus->HeadingTrueDegrees);
                if (headingChange > -90.0f && headingChange < 90.0f) {
                        // Heading change is still ahead of us
                        if (this->headingState != State::APPROACHING_HEADING_CHANGE) {
                                this->headingState = State::APPROACHING_HEADING_CHANGE;
                                sprintf_s(msg, 255, "Changing heading state to approach");
                                this->logger->Log(msg, Logger::Debug);
                        }

                        distanceUntilFirstHeadingChange = this->distanceBetweenPositions(this->vehiclePos, bus->nodes.at(headingChangeStartWaypointIndex).position);
                        this->checkForCue();
                }
                else {
                        if (this->headingChangeEndWaypointIndex != -1 && this->headingChangeEndWaypointIndex < bus->nodes.size()) {
                                const float angleDegreesHeadingChangeEndWaypoint = this->angleBetweenPositionsDeg(this->vehiclePos, bus->nodes.at(headingChangeEndWaypointIndex).position);
                                const float headingChange = this->getHeadingChange(angleDegreesHeadingChangeEndWaypoint, bus->HeadingTrueDegrees);
                                if (headingChange < -90.0f || headingChange > 90.0) {
                                        // If the angle is smaller then -100 or bigger then 100, it's behind us, then we look ahead to next 
                                        if (this->headingState != State::COMPLETED_HEADING_CHANGE) {
                                                sprintf_s(msg, 255, "Changing heading state to completed");
                                                this->logger->Log(msg, Logger::Debug);
                                                this->headingState = State::COMPLETED_HEADING_CHANGE;
                                                this->LookAhead();
                                        }
                                }
                                else {
                                        if (this->headingState != State::IN_HEADING_CHANGE) {
                                                sprintf_s(msg, 255, "Changing heading state to in heading change");
                                                this->logger->Log(msg, Logger::Debug);
                                                this->headingState = State::IN_HEADING_CHANGE;
                                        }

                                }
                        }

                }
        }
        else if (this->headingChangeEndWaypointIndex == -1) {
                headingChangeEndWaypointIndex = closestWaypointIndex;
                this->LookAhead();
        }
}