interpolation for ccd

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@190 253336fb-580f-4252-a368-f3cef5a2a82b

trunk/fcl/include/fcl/ccd/interpolation/interpolation.h

+#ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_H
+#define FCL_CCD_INTERPOLATION_INTERPOLATION_H
+
+#include "fcl/data_types.h"
+
+namespace fcl
+{
+
+enum InterpolationType
+{
+  LINEAR,
+  STANDARD
+};
+
+class Interpolation
+{
+public:
+  Interpolation();
+
+  virtual ~Interpolation() {}
+
+  Interpolation(FCL_REAL start_value, FCL_REAL end_value);
+
+  void setStartValue(FCL_REAL start_value);
+  void setEndValue(FCL_REAL end_value);
+
+  virtual FCL_REAL getValue(FCL_REAL time) const = 0;
+
+  /// @brief return the smallest value in time interval [0, 1]
+  virtual FCL_REAL getValueLowerBound() const = 0;
+
+  /// @brief return the biggest value in time interval [0, 1]
+  virtual FCL_REAL getValueUpperBound() const = 0;
+
+  virtual InterpolationType getType() const = 0;
+
+  bool operator == (const Interpolation& interpolation) const;
+  bool operator != (const Interpolation& interpolation) const;
+
+  virtual FCL_REAL getMovementLengthBound(FCL_REAL time) const = 0;
+
+  virtual FCL_REAL getVelocityBound(FCL_REAL time) const = 0;
+
+protected:
+  FCL_REAL value_0_; // value at time = 0.0
+  FCL_REAL value_1_; // value at time = 1.0
+
+};
+
+
+
+}
+
+#endif

trunk/fcl/include/fcl/ccd/interpolation/interpolation_factory.h

+#ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_FACTORY_H
+#define FCL_CCD_INTERPOLATION_INTERPOLATION_FACTORY_H
+
+#include "fcl/data_types.h"
+#include "fcl/ccd/interpolation/interpolation.h"
+
+#include <map>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/function.hpp>
+
+namespace fcl 
+{
+
+class InterpolationFactory
+{	
+public:
+  typedef boost::function<boost::shared_ptr<Interpolation>(FCL_REAL, FCL_REAL)> CreateFunction;
+
+public:
+  void registerClass(const InterpolationType type, const CreateFunction create_function);
+
+  boost::shared_ptr<Interpolation> create(const InterpolationType type, FCL_REAL start_value, FCL_REAL end_value);
+
+public:
+  static InterpolationFactory& instance();
+
+private:
+  InterpolationFactory();
+
+  InterpolationFactory(const InterpolationFactory&)
+  {}
+
+  InterpolationFactory& operator = (const InterpolationFactory&)
+  {
+    return *this;
+  }
+
+private:
+  std::map<InterpolationType, CreateFunction> creation_map_;
+};
+
+}
+
+#endif /* #ifndef FCL_INTERPOLATION_FACTORY_H */

trunk/fcl/include/fcl/ccd/interpolation/interpolation_linear.h

+#ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_LINEAR_H
+#define FCL_CCD_INTERPOLATION_INTERPOLATION_LINEAR_H
+
+#include "fcl/data_types.h"
+#include "fcl/ccd/interpolation/interpolation.h"
+
+#include <boost/shared_ptr.hpp>
+
+namespace fcl 
+{
+
+class InterpolationFactory;
+
+class InterpolationLinear : public Interpolation
+{
+public:
+  InterpolationLinear();
+
+  InterpolationLinear(FCL_REAL start_value, FCL_REAL end_value);
+
+  virtual FCL_REAL getValue(FCL_REAL time) const;
+
+  virtual FCL_REAL getValueLowerBound() const;
+  virtual FCL_REAL getValueUpperBound() const;
+
+  virtual InterpolationType getType() const;
+
+  virtual FCL_REAL getMovementLengthBound(FCL_REAL time) const;
+
+  virtual FCL_REAL getVelocityBound(FCL_REAL time) const;
+
+public:
+  static boost::shared_ptr<Interpolation> create(FCL_REAL start_value, FCL_REAL end_value);
+
+  static void registerToFactory();
+};
+
+}
+
+#endif

trunk/fcl/src/CMakeLists.txt

@@ -46,6 +46,9 @@ add_library(${PROJECT_NAME} SHARED
   articulated_model/joint.cpp
   articulated_model/model_config.cpp
   articulated_model/model.cpp
+  ccd/interpolation/interpolation.cpp
+  ccd/interpolation/interpolation_factory.cpp
+  ccd/interpolation/interpolation_linear.cpp
   profile.cpp
   collision_data.cpp)
 

trunk/fcl/src/ccd/interpolation/interpolation.cpp

+#include "fcl/ccd/interpolation/interpolation.h"
+
+namespace fcl 
+{
+
+Interpolation::Interpolation() :
+  value_0_(0.0),
+  value_1_(1.0) 
+{}
+
+Interpolation::Interpolation(FCL_REAL start_value, FCL_REAL end_value) :
+  value_0_(start_value),
+  value_1_(end_value)
+{}
+
+void Interpolation::setStartValue(FCL_REAL start_value)
+{
+  value_0_ = start_value;
+}
+
+void Interpolation::setEndValue(FCL_REAL end_value)
+{
+  value_1_ = end_value;
+}
+
+bool Interpolation::operator == (const Interpolation& interpolation) const
+{
+  return 
+    (this->getType() == interpolation.getType()) &&
+    (this->value_0_ == interpolation.value_0_) &&
+    (this->value_1_ == interpolation.value_1_);
+}
+
+bool Interpolation::operator != (const Interpolation& interpolation) const
+{
+  return !(*this == interpolation);
+}
+
+}

trunk/fcl/src/ccd/interpolation/interpolation_factory.cpp

+#include "fcl/ccd/interpolation/interpolation_factory.h"
+#include "fcl/ccd/interpolation/interpolation_linear.h"
+
+#include <boost/assert.hpp>
+
+namespace fcl 
+{
+
+InterpolationFactory::InterpolationFactory()
+{
+  InterpolationLinear::registerToFactory();
+}
+
+InterpolationFactory& InterpolationFactory::instance()
+{
+  static InterpolationFactory instance;
+
+  return instance;
+}
+
+void InterpolationFactory::registerClass(const InterpolationType type, const CreateFunction create_function)
+{
+  this->creation_map_[type] = create_function;
+}
+
+boost::shared_ptr<Interpolation> 
+InterpolationFactory::create(const InterpolationType type, const FCL_REAL start_value, const FCL_REAL end_value)
+{
+  std::map<InterpolationType, CreateFunction>::const_iterator it = creation_map_.find(type);
+
+  BOOST_ASSERT_MSG((it != creation_map_.end()), "CreateFunction wasn't found.");
+
+  return (it->second)(start_value, end_value);  
+}
+
+}

trunk/fcl/src/ccd/interpolation/interpolation_linear.cpp

+#include "fcl/ccd/interpolation/interpolation_linear.h"
+#include "fcl/ccd/interpolation/interpolation_factory.h"
+
+namespace fcl 
+{
+
+InterpolationType interpolation_linear_type = LINEAR;
+
+InterpolationLinear::InterpolationLinear() : Interpolation(0.0, 1.0)
+{}
+
+InterpolationLinear::InterpolationLinear(FCL_REAL start_value, FCL_REAL end_value) : Interpolation(start_value, end_value)
+{}
+
+FCL_REAL InterpolationLinear::getValue(FCL_REAL time) const
+{
+  return value_0_ + (value_1_ - value_0_) * time;
+}
+
+FCL_REAL InterpolationLinear::getValueLowerBound() const
+{
+  return value_0_;
+}
+
+FCL_REAL InterpolationLinear::getValueUpperBound() const
+{
+  return value_1_;
+}
+
+InterpolationType InterpolationLinear::getType() const
+{
+  return interpolation_linear_type;
+}
+
+boost::shared_ptr<Interpolation> InterpolationLinear::create(FCL_REAL start_value, FCL_REAL end_value)
+{
+  return boost::shared_ptr<Interpolation>(new InterpolationLinear(start_value, end_value) );
+}
+
+void InterpolationLinear::registerToFactory()
+{
+  InterpolationFactory::instance().registerClass(interpolation_linear_type, create);
+}
+
+FCL_REAL InterpolationLinear::getMovementLengthBound(FCL_REAL time) const
+{
+  return getValueUpperBound() - getValue(time);
+}
+
+FCL_REAL InterpolationLinear::getVelocityBound(FCL_REAL time) const
+{
+  return (value_1_ - value_0_);
+}
+
+}