interval-validation.hh

Go to the documentation of this file.

//
// Copyright (c) 2014,2015,2016,2018 CNRS
// Authors: Florent Lamiraux, Joseph Mirabel, Diane Bury
//
 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. 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.
//
// 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
// HOLDER 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 HPP_CORE_CONTINUOUS_VALIDATION_INTERVAL_VALIDATION_HH
#define HPP_CORE_CONTINUOUS_VALIDATION_INTERVAL_VALIDATION_HH
 
#include <coal/collision.h>
#include <coal/collision_data.h>
 
#include <boost/icl/continuous_interval.hpp>
#include <boost/icl/interval_set.hpp>
#include <hpp/core/deprecated.hh>
#include <hpp/core/fwd.hh>
#include <hpp/pinocchio/body.hh>
#include <hpp/pinocchio/collision-object.hh>
#include <hpp/pinocchio/joint.hh>
#include <iterator>
#include <limits>
 
namespace hpp {
namespace core {
namespace continuousValidation {
class IntervalValidation {
 public:
  virtual bool validateConfiguration(const value_type &t, interval_t &interval,
                                     ValidationReportPtr_t &report,
                                     const pinocchio::DeviceData &data) = 0;
 
  void path(const PathPtr_t &path, bool reverse) {
    path_ = path;
    reverse_ = reverse;
    valid_ = false;
    validInterval_ = interval_set();
    setupPath();
  }
 
  PathConstPtr_t path() const { return path_; }
 
  value_type tolerance() const { return tolerance_; }
 
  virtual std::string name() const = 0;
  virtual std::ostream &print(std::ostream &os) const = 0;
  virtual IntervalValidationPtr_t copy() const = 0;
 
 protected:
  typedef boost::icl::continuous_interval<value_type> continuous_interval;
  typedef boost::icl::interval_set<value_type> interval_set;
  PathPtr_t path_;
  value_type tolerance_;
  bool reverse_;
  bool refine_;
  bool valid_;
  interval_set validInterval_;
  IntervalValidation(value_type tolerance)
      : tolerance_(tolerance), reverse_(false), refine_(true) {
    if (tolerance < 0) {
      throw std::runtime_error("tolerance should be non-negative.");
    }
  }
 
  IntervalValidation(const IntervalValidation &other)
      : tolerance_(other.tolerance_), refine_(true) {
    if (tolerance_ < 0) {
      throw std::runtime_error("tolerance should be non-negative.");
    }
  }
 
 private:
  virtual void setupPath() = 0;
};  // class IntervalValidation
 
inline std::ostream &operator<<(std::ostream &os, const IntervalValidation &b) {
  return b.print(os);
}
}  // namespace continuousValidation
}  // namespace core
}  // namespace hpp
#endif  // HPP_CORE_CONTINUOUS_VALIDATION_INTERVAL_VALIDATION_HH