failed_at_this_configuration.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019. Toyota Research Institute
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of CNRS-LAAS and AIST nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef FCL_FAILED_AT_THIS_CONFIGURATION_H
#define FCL_FAILED_AT_THIS_CONFIGURATION_H

#include <exception>
#include <iomanip>
#include <ostream>
#include <sstream>
#include <stdexcept>
#include <string>

#include "fcl/common/types.h"
#include "fcl/export.h"

namespace fcl {
namespace detail {

class FCL_EXPORT FailedAtThisConfiguration final
    : public std::exception {
 public:
  FailedAtThisConfiguration(const std::string& message)
      : std::exception(), message_(message) {}

  const char* what() const noexcept final { return message_.c_str(); }

 private:
  std::string message_;
};

FCL_EXPORT void ThrowFailedAtThisConfiguration(
    const std::string& message, const char* func, const char* file, int line);

template <typename S>
void WriteCommaSeparated(std::stringstream* sstream, const Transform3<S>& p) {
  const auto& m = p.matrix();
  std::stringstream& ss = *sstream;
  for (int row = 0; row < 4; ++row) {
    for (int col = 0; col < 4; ++col) {
      ss << m(row, col);
      if (col < 3) {
        ss << ", ";
      }
    }
    if (row < 3) {
      ss << ",\n";
    } else {
      ss << ";";
    }
  }
}

template <typename Shape1, typename Shape2, typename Solver, typename S>
void ThrowDetailedConfiguration(const Shape1& s1, const Transform3<S>& X_FS1,
                                const Shape2& s2, const Transform3<S>& X_FS2,
                                const Solver& solver, const std::exception& e) {
  std::stringstream ss;
  const int digits = 20;
  ss << std::setprecision(digits);
  ss << "Error with configuration"
     << "\n  Original error message: " << e.what()
     << "\n  Shape 1:\n" << s1.representation(digits)
     << "\n  X_FS1\n";
  WriteCommaSeparated(&ss, X_FS1);
  ss << "\n  Shape 2:\n" << s2.representation(digits)
     << "\n  X_FS2\n";
  WriteCommaSeparated(&ss, X_FS2);
  ss << "\n  Solver: " << solver;
  throw std::logic_error(ss.str());
}

}  // namespace detail
}  // namespace fcl

#define FCL_THROW_FAILED_AT_THIS_CONFIGURATION(message)            \
  ::fcl::detail::ThrowFailedAtThisConfiguration(message, __func__, __FILE__, \
                                                __LINE__)

#endif  // FCL_FAILED_AT_THIS_CONFIGURATION_H