Cosy/Teleport to waypoint

Blip wpblip;
Vector3 wpcoords;
float groundz;

void PrintText(char *text)
{
    PRINT_STRING_WITH_LITERAL_STRING_NOW("string", text, 5000, true);
}

void TeleportToWaypoint(void)
{
    wpblip = GET_FIRST_BLIP_INFO_ID(BLIP_WAYPOINT);
    if( DOES_BLIP_EXIST(wpblip) )
    {
        GET_BLIP_COORDS(wpblip, &wpcoords);
        SET_CHAR_COORDINATES(GetPlayerPed(), wpcoords.x, wpcoords.y, 0.0);
        SET_GAME_CAM_HEADING(0.0);
        REQUEST_COLLISION_AT_POSN(wpcoords.x, wpcoords.y, 0.0);
        LOAD_ALL_OBJECTS_NOW();

        GET_GROUND_Z_FOR_3D_COORD(wpcoords.x, wpcoords.y, 1000, &groundz);
        SET_CHAR_COORDINATES(GetPlayerPed(), wpcoords.x, wpcoords.y, groundz);
        SET_GAME_CAM_HEADING(0.0);
        PrintText("Successfully teleported to waypoint");
    }
    else PrintText("No waypoint set");
}