base_matrix.cpp

Go to the documentation of this file.

/*PGR-GNU*****************************************************************
FILE: matrix.cpp
 
Copyright (c) 2015 pgRouting developers
Mail: [email protected]
 
Developer:
Copyright (c) 2015 Celia Virginia Vergara Castillo
 
------
 
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/base_matrix.h"
 
#include <string>
#include <sstream>
#include <algorithm>
#include <limits>
#include <vector>
#include <map>
#include <cmath>
#include <utility>
 
#include "cpp_common/identifiers.hpp"
#include "cpp_common/pgr_assert.h"
#include "c_types/matrix_cell_t.h"
#include "c_types/vroom/vroom_matrix_t.h"
 
 
namespace vrprouting {
namespace base {
 
namespace detail {
double
get_distance(std::pair<Coordinate, Coordinate> p1 , std::pair<Coordinate, Coordinate> p2) {
  auto dx = p1.first - p2.first;
  auto dy = p1.second - p2.second;
  return std::sqrt(dx * dx + dy * dy);
}
}  // namespace detail
 
void
Base_Matrix::set_ids(const std::vector<Matrix_cell_t> &data) {
  pgassert(m_ids.empty());
  Identifiers<Id> node_ids;
  /*
   * Cycle the information
   */
  for (const auto &cost : data) {
    /*
     * extract the original identifiers
     */
    node_ids += cost.from_vid;
    node_ids += cost.to_vid;
  }
  /*
   * Save the extracted information
   */
  m_ids.insert(m_ids.begin(), node_ids.begin(), node_ids.end());
}
 
 
bool
Base_Matrix::has_id(Id id) const {
  /*
   * binary search the stored identifiers
   */
  auto pos = std::lower_bound(m_ids.cbegin(), m_ids.cend(), id);
 
  /*
   * Return search results
   */
  return pos != m_ids.end() && *pos == id;
}
 
 
Idx
Base_Matrix::get_index(Id id) const {
  /*
   * binary search the stored identifiers
   */
  auto pos = std::lower_bound(m_ids.begin(), m_ids.end(), id);
  if (pos == m_ids.end()) {
    std::ostringstream msg;
    msg << *this << "\nNot found" << id;
    pgassertwm(false, msg.str());
    throw std::make_pair(std::string("(INTERNAL) Base_Matrix: Unable to find node on matrix"), msg.str());
  }
  pgassert(pos != m_ids.end());
 
  /*
   * return the index found
   */
  return static_cast<Idx>(pos - m_ids.begin());
}
 
Id
Base_Matrix::get_original_id(Idx index) const {
  /*
   * Go to the index in the identifiers vector
   */
 
  if (index >= m_ids.size()) {
    std::ostringstream msg;
    msg << *this << "\nOut of range" << index;
    pgassertwm(false, msg.str());
    throw std::make_pair(std::string("(INTERNAL) Base_Matrix: The given index is out of range"), msg.str());
  }
  pgassert(index < m_ids.size());
 
  /*
   * return the original id found
   */
  return static_cast<Id>(m_ids[index]);
}
 
Base_Matrix::Base_Matrix(
    Matrix_cell_t *data_costs, size_t size_matrix,
    const Identifiers<Id>& node_ids,
    Multiplier multiplier) {
  /*
   * Sets the selected nodes identifiers
   */
  m_ids.insert(m_ids.begin(), node_ids.begin(), node_ids.end());
 
  /*
   * Create matrix
   */
  m_time_matrix.resize(
      m_ids.size(),
      std::vector<TInterval>(
        m_ids.size(),
        /*
         * Set initial values to infinity
         */
        (std::numeric_limits<TInterval>::max)()));
 
  Identifiers<Idx> inserted;
  /*
   * Cycle the matrix data
   */
  for (size_t i = 0; i < size_matrix; ++i) {
    auto data = data_costs[i];
    /*
     * skip if row is not from selected nodes
     */
    if (!(has_id(data.from_vid) && has_id(data.to_vid))) continue;
 
    /*
     * Save the information
     */
    m_time_matrix[get_index(data.from_vid)][get_index(data.to_vid)] =
      static_cast<TInterval>(static_cast<Multiplier>(data.cost) * multiplier);
 
    /*
     * If the opposite direction is infinity insert the same cost
     */
    if (m_time_matrix[get_index(data.to_vid)][get_index(data.from_vid)] == (std::numeric_limits<TInterval>::max)()) {
      m_time_matrix[get_index(data.to_vid)][get_index(data.from_vid)] =
        m_time_matrix[get_index(data.from_vid)][get_index(data.to_vid)];
    }
  }
 
  /*
   * Set the diagonal values to 0
   */
  for (size_t i = 0; i < m_time_matrix.size(); ++i) {
    m_time_matrix[i][i] = 0;
  }
}
 
Base_Matrix::Base_Matrix(Vroom_matrix_t *matrix_rows, size_t total_matrix_rows,
                         const Identifiers<Id> &location_ids, double scaling_factor) {
  /*
   * Sets the selected nodes identifiers
   */
  m_ids.insert(m_ids.begin(), location_ids.begin(), location_ids.end());
 
  /*
   * Create matrix
   */
  m_time_matrix.resize(
      m_ids.size(),
      std::vector<TInterval>(m_ids.size(),
                             /*
                              * Set initial values to infinity
                              */
                             (std::numeric_limits<TInterval>::max)()));
  m_cost_matrix.resize(
      m_ids.size(),
      std::vector<TravelCost>(m_ids.size(), (std::numeric_limits<TravelCost>::max)()));
 
  Identifiers<Idx> inserted;
  /*
   * Cycle the matrix data
   */
  for (size_t i = 0; i < total_matrix_rows; ++i) {
    auto cell = matrix_rows[i];
    /*
     * skip if row is not from selected nodes
     */
    if (!(has_id(cell.start_id) && has_id(cell.end_id))) continue;
 
    /*
     * Save the information. Scale the time matrix according to scaling_factor
     */
    m_time_matrix[get_index(cell.start_id)][get_index(cell.end_id)] =
      static_cast<Duration>(std::round(cell.duration / scaling_factor));
    m_cost_matrix[get_index(cell.start_id)][get_index(cell.end_id)] =
      static_cast<Duration>(cell.cost);
 
    /*
     * If the opposite direction is infinity insert the same cost
     */
    if (m_time_matrix[get_index(cell.end_id)][get_index(cell.start_id)] ==
        (std::numeric_limits<TInterval>::max)()) {
      m_time_matrix[get_index(cell.end_id)][get_index(cell.start_id)] =
        m_time_matrix[get_index(cell.start_id)][get_index(cell.end_id)];
    }
    if (m_cost_matrix[get_index(cell.end_id)][get_index(cell.start_id)] ==
        (std::numeric_limits<TravelCost>::max)()) {
      m_cost_matrix[get_index(cell.end_id)][get_index(cell.start_id)] =
          m_cost_matrix[get_index(cell.start_id)][get_index(cell.end_id)];
    }
  }
 
  /*
   * Set the diagonal values to 0
   */
  for (size_t i = 0; i < m_time_matrix.size(); ++i) {
    m_time_matrix[i][i] = 0;
    m_cost_matrix[i][i] = 0;
  }
}
 
/*
 * constructor for euclidean
 */
Base_Matrix::Base_Matrix(const std::map<std::pair<Coordinate, Coordinate>, Id> &euclidean_data, Multiplier multiplier) {
  m_ids.reserve(euclidean_data.size());
  for (const auto &e : euclidean_data) {
    m_ids.push_back(e.second);
  }
  m_time_matrix.resize(
      m_ids.size(),
      std::vector<TInterval>(
        m_ids.size(),
        (std::numeric_limits<TInterval>::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);
      m_time_matrix[from_id][to_id] =
        static_cast<TInterval>(static_cast<Multiplier>(detail::get_distance(from.first, to.first)) * multiplier);
      m_time_matrix[to_id][from_id] = m_time_matrix[from_id][to_id];
    }
  }
 
  for (size_t i = 0; i < m_time_matrix.size(); ++i) {
    m_time_matrix[i][i] = 0;
  }
}
 
vroom::Matrix<vroom::Duration>
Base_Matrix::get_vroom_duration_matrix() const {
  size_t matrix_size = m_ids.size();
  vroom::Matrix<vroom::Cost> vroom_matrix(matrix_size);
  for (size_t i = 0; i < matrix_size; i++) {
    for (size_t j = 0; j < matrix_size; j++) {
      vroom_matrix[i][j] = static_cast<vroom::Duration>(m_time_matrix[i][j]);
    }
  }
  return vroom_matrix;
}
 
vroom::Matrix<vroom::Cost>
Base_Matrix::get_vroom_cost_matrix() const {
  size_t matrix_size = m_ids.size();
  vroom::Matrix<vroom::Cost> vroom_matrix(matrix_size);
  for (size_t i = 0; i < matrix_size; i++) {
    for (size_t j = 0; j < matrix_size; j++) {
      vroom_matrix[i][j] = static_cast<vroom::Cost>(m_cost_matrix[i][j]);
    }
  }
  return vroom_matrix;
}
 
bool
Base_Matrix::has_no_infinity() const {
  /*
   * Cycle the matrix
   */
  for (const auto &row : m_time_matrix) {
    for (const auto &val : row) {
      /*
       * found infinity?
       *
       * yes -> return false
       */
      if (val == (std::numeric_limits<TInterval>::max)()) return false;
    }
  }
  /*
   * return true
   */
  return true;
}
 
bool
Base_Matrix::obeys_triangle_inequality() const {
  for (size_t i = 0; i < m_time_matrix.size(); ++i) {
    for (size_t j = 0; j < m_time_matrix.size(); ++j) {
      for (size_t k = 0; k < m_time_matrix.size(); ++k) {
        if (m_time_matrix[i][k] > (m_time_matrix[i][j] + m_time_matrix[j][k])) {
          return false;
        }
      }
    }
  }
  return true;
}
 
size_t
Base_Matrix::fix_triangle_inequality(size_t depth) {
  if (depth > m_time_matrix.size()) return depth;
  for (auto & i : m_time_matrix) {
    for (size_t j = 0; j < m_time_matrix.size(); ++j) {
      for (size_t k = 0; k < m_time_matrix.size(); ++k) {
        if (i[k] > (i[j] + m_time_matrix[j][k])) {
          i[k] = i[j] + m_time_matrix[j][k];
          return fix_triangle_inequality(++depth);
        }
      }
    }
  }
  return depth;
}
 
bool
Base_Matrix::is_symmetric() const {
  for (size_t i = 0; i < m_time_matrix.size(); ++i) {
    for (size_t j = 0; j < m_time_matrix.size(); ++j) {
      if (0.000001 < std::fabs(m_time_matrix[i][j] - m_time_matrix[j][i])) {
        std::ostringstream log;
        log << "i \t" << i
          << "j \t" << j
          << "m_time_matrix[i][j] \t" << m_time_matrix[i][j]
          << "m_time_matrix[j][i] \t" << m_time_matrix[j][i]
          << "\n";
        log << (*this);
        pgassertwm(false, log.str());
        return false;
      }
    }
  }
  return true;
}
 
 
std::ostream& operator<<(std::ostream &log, const Base_Matrix &matrix) {
  /*
   * print the identifiers
   */
  for (const auto id : matrix.m_ids) {
    log << "\t" << id;
  }
  log << "\n";
  size_t i = 0;
 
  /*
   * Cycle the cells
   */
  for (const auto &row : matrix.m_time_matrix) {
    size_t j = 0;
    for (const auto cost : row) {
      /*
       * print the information
       */
      log << "Internal(" << i << "," << j << ")"
        << "\tOriginal(" << matrix.m_ids[i] << "," << matrix.m_ids[j] << ")"
        << "\t = " << cost
        << "\n";
      ++j;
    }
    ++i;
  }
  return log;
}
 
 
}  // namespace base
}  // namespace vrprouting