From f33d9fcf1fc3898f4a741180da6dc6153cac92ee Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Fri, 7 Sep 2012 20:20:06 +0000
Subject: [PATCH] interpolation for ccd

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@190 253336fb-580f-4252-a368-f3cef5a2a82b
---
 .../fcl/ccd/interpolation/interpolation.h     | 54 ++++++++++++++++++
 .../ccd/interpolation/interpolation_factory.h | 45 +++++++++++++++
 .../ccd/interpolation/interpolation_linear.h  | 40 ++++++++++++++
 trunk/fcl/src/CMakeLists.txt                  |  3 +
 .../src/ccd/interpolation/interpolation.cpp   | 39 +++++++++++++
 .../interpolation/interpolation_factory.cpp   | 36 ++++++++++++
 .../interpolation/interpolation_linear.cpp    | 55 +++++++++++++++++++
 7 files changed, 272 insertions(+)
 create mode 100644 trunk/fcl/include/fcl/ccd/interpolation/interpolation.h
 create mode 100644 trunk/fcl/include/fcl/ccd/interpolation/interpolation_factory.h
 create mode 100644 trunk/fcl/include/fcl/ccd/interpolation/interpolation_linear.h
 create mode 100644 trunk/fcl/src/ccd/interpolation/interpolation.cpp
 create mode 100644 trunk/fcl/src/ccd/interpolation/interpolation_factory.cpp
 create mode 100644 trunk/fcl/src/ccd/interpolation/interpolation_linear.cpp

diff --git a/trunk/fcl/include/fcl/ccd/interpolation/interpolation.h b/trunk/fcl/include/fcl/ccd/interpolation/interpolation.h
new file mode 100644
index 00000000..d52538f0
--- /dev/null
+++ b/trunk/fcl/include/fcl/ccd/interpolation/interpolation.h
@@ -0,0 +1,54 @@
+#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
diff --git a/trunk/fcl/include/fcl/ccd/interpolation/interpolation_factory.h b/trunk/fcl/include/fcl/ccd/interpolation/interpolation_factory.h
new file mode 100644
index 00000000..2729231a
--- /dev/null
+++ b/trunk/fcl/include/fcl/ccd/interpolation/interpolation_factory.h
@@ -0,0 +1,45 @@
+#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 */
diff --git a/trunk/fcl/include/fcl/ccd/interpolation/interpolation_linear.h b/trunk/fcl/include/fcl/ccd/interpolation/interpolation_linear.h
new file mode 100644
index 00000000..92f35a05
--- /dev/null
+++ b/trunk/fcl/include/fcl/ccd/interpolation/interpolation_linear.h
@@ -0,0 +1,40 @@
+#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
diff --git a/trunk/fcl/src/CMakeLists.txt b/trunk/fcl/src/CMakeLists.txt
index 288a1fc5..368da586 100644
--- a/trunk/fcl/src/CMakeLists.txt
+++ b/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)
 
diff --git a/trunk/fcl/src/ccd/interpolation/interpolation.cpp b/trunk/fcl/src/ccd/interpolation/interpolation.cpp
new file mode 100644
index 00000000..7305a1dd
--- /dev/null
+++ b/trunk/fcl/src/ccd/interpolation/interpolation.cpp
@@ -0,0 +1,39 @@
+#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);
+}
+
+}
diff --git a/trunk/fcl/src/ccd/interpolation/interpolation_factory.cpp b/trunk/fcl/src/ccd/interpolation/interpolation_factory.cpp
new file mode 100644
index 00000000..1ec4fd83
--- /dev/null
+++ b/trunk/fcl/src/ccd/interpolation/interpolation_factory.cpp
@@ -0,0 +1,36 @@
+#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);  
+}
+
+}
diff --git a/trunk/fcl/src/ccd/interpolation/interpolation_linear.cpp b/trunk/fcl/src/ccd/interpolation/interpolation_linear.cpp
new file mode 100644
index 00000000..8ae0f2c9
--- /dev/null
+++ b/trunk/fcl/src/ccd/interpolation/interpolation_linear.cpp
@@ -0,0 +1,55 @@
+#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_);
+}
+
+}
-- 
GitLab