Dmatrix.cpp

Go to the documentation of this file.

/*PGR-GNU*****************************************************************
 
FILE: Dmatrix.cpp
 
Copyright (c) 2015 pgRouting developers
Mail: [email protected]
 
------
 
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
 
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
 
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 
 ********************************************************************PGR-GNU*/
 
#include "cpp_common/Dmatrix.h"
 
#include <string.h>
#include <sstream>
#include <algorithm>
#include <limits>
#include <vector>
#include <map>
#include <cmath>
 
#include "tsp/tour.h"
#include "cpp_common/pgr_assert.h"
 
 
namespace pgrouting {
namespace tsp {
 
double
Dmatrix::tourCost(const Tour &tour) const {
    double total_cost(0);
    if (tour.cities.empty()) return total_cost;
 
    auto prev_id = tour.cities.front();
    for (const auto &id : tour.cities) {
        if (id == tour.cities.front()) continue;
 
        pgassert(distance(prev_id, id) != (std::numeric_limits<double>::max)());
 
        total_cost += costs[prev_id][id];
        prev_id = id;
    }
    total_cost += costs[prev_id][tour.cities.front()];
    return total_cost;
}
 
 
 
void
Dmatrix::set_ids(const std::vector < Matrix_cell_t > &data_costs) {
    ids.reserve(data_costs.size() * 2);
    for (const auto &cost : data_costs) {
        ids.push_back(cost.from_vid);
        ids.push_back(cost.to_vid);
    }
    std::sort(ids.begin(), ids.end());
    ids.erase(std::unique(ids.begin(), ids.end()), ids.end());
    /*
     * freeing up unused space
     */
    ids.shrink_to_fit();
}
 
bool
Dmatrix::has_id(int64_t id) const {
    for (const auto &i : ids) {
        if (i == id) return true;
    }
    return false;
}
 
 
size_t
Dmatrix::get_index(int64_t id) const {
    for (size_t pos = 0; pos < ids.size(); ++pos) {
        if (ids[pos] == id) return pos;
    }
    throw std::make_pair(std::string("(INTERNAL) Dmatrix: Unable to find node on matrix"), id);
}
 
int64_t
Dmatrix::get_id(size_t id) const {
    return ids[id];
}
 
/*
 * Transforms the input data to a matrix
 */
Dmatrix::Dmatrix(const std::vector < Matrix_cell_t > &data_costs) {
    set_ids(data_costs);
    costs.resize(
            ids.size(),
            std::vector<double>(
                ids.size(),
                (std::numeric_limits<double>::max)()));
 
    for (const auto &data : data_costs) {
        costs[get_index(data.from_vid)][get_index(data.to_vid)] = data.cost;
    }
 
    for (size_t i = 0; i < costs.size(); ++i) {
        costs[i][i] = 0;
    }
}
 
 
double
get_distance(std::pair<double, double> p1 , std::pair<double, double> p2) {
    auto dx = p1.first - p2.first;
    auto dy = p1.second - p2.second;
    return std::sqrt(dx * dx + dy * dy);
}
 
/*
 * constructor for euclidean
 */
Dmatrix::Dmatrix(const std::map<std::pair<double, double>, int64_t> &euclidean_data) {
    ids.reserve(euclidean_data.size());
    for (const auto &e: euclidean_data) {
        ids.push_back(e.second);
    }
    costs.resize(
            ids.size(),
            std::vector<double>(
                ids.size(),
                (std::numeric_limits<double>::max)()));
 
    for (const auto &from : euclidean_data) {
        for (const auto &to : euclidean_data) {
            auto from_id = get_index(from.second);
            auto to_id = get_index(to.second);
            costs[from_id][to_id] = get_distance(from.first, to.first);
            costs[to_id][from_id] = costs[from_id][to_id];
        }
    }
 
    for (size_t i = 0; i < costs.size(); ++i) {
        costs[i][i] = 0;
    }
}
 
bool
Dmatrix::has_no_infinity() const {
    for (const auto &row : costs) {
        for (const auto &val : row) {
            if (val == (std::numeric_limits<double>::infinity)()) return false;
            if (val == (std::numeric_limits<double>::max)()) return false;
        }
    }
    return true;
}
 
 
bool
Dmatrix::obeys_triangle_inequality() const {
    for (size_t i = 0; i < costs.size(); ++i) {
        for (size_t j = 0; j < costs.size(); ++j) {
            for (size_t k = 0; k < costs.size(); ++k) {
                if (!(costs[i][k] <= (costs[i][j] + costs[j][k]))) return false;
            }
        }
    }
    return true;
}
 
bool
Dmatrix::is_symmetric() const {
    for (size_t i = 0; i < costs.size(); ++i) {
        for (size_t j = 0; j < costs.size(); ++j) {
            if (0.000001 < std::fabs(costs[i][j] - costs[j][i])) {
                std::ostringstream log;
                log << "i \t" << i
                    << "j \t" << j
                    << "costs[i][j] \t" << costs[i][j]
                    << "costs[j][i] \t" << costs[j][i]
                    << "\n";
                log << (*this);
                pgassertwm(false, log.str());
                return false;
            }
        }
    }
    return true;
}
 
 
std::ostream& operator<<(std::ostream &log, const Dmatrix &matrix) {
    for (const auto id : matrix.ids) {
        log << "\t" << id;
    }
    log << "\n";
    size_t i = 0;
    for (const auto &row : matrix.costs) {
        size_t j = 0;
        for (const auto cost : row) {
            log << "Internal(" << i << "," << j << ")"
                << "\tUsers(" << matrix.ids[i] << "," << matrix.ids[j] << ")"
                << "\t = " << cost
#if 0
                << "\t(" << matrix.get_index(matrix.ids[i])
                << "," << matrix.get_index(matrix.ids[j]) << ")"
                << "\t = " << matrix.costs[i][j]
                << "\t = " << matrix.costs[j][i]
                << "=inf:"
                <<  (matrix.costs[i][j] ==
                        (std::numeric_limits<double>::infinity)())
                << "=inf:"
                <<  (matrix.costs[j][i] ==
                        (std::numeric_limits<double>::infinity)())
#endif
                << "\n";
            ++j;
        }
        ++i;
    }
#if 0
    for (size_t i = 0; i < matrix.costs.size(); ++i) {
        for (size_t j = 0; j < matrix.costs.size(); ++j) {
            for (size_t k = 0; k < matrix.costs.size(); ++k) {
                log << matrix.costs[i][k] << " <= ("
                    << matrix.costs[i][j] << " + "  << matrix.costs[j][k] << ")"
                    << (matrix.costs[i][k]
                            <= (matrix.costs[i][j] + matrix.costs[j][k]))
                    << "\n";
            }
        }
    }
#endif
    return log;
}
 
 
}  // namespace tsp
}  // namespace pgrouting