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

trunk/fcl/CMakeLists.txt

@@ -32,7 +32,7 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 add_definitions(-DUSE_PQP=0)
 add_definitions(-DUSE_SVMLIGHT=0)
 
-rosbuild_add_library(${PROJECT_NAME} src/AABB.cpp src/OBB.cpp src/RSS.cpp src/vec_3f.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/geometric_shapes_intersect.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp)
+rosbuild_add_library(${PROJECT_NAME} src/AABB.cpp src/OBB.cpp src/RSS.cpp src/vec_3f.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/geometric_shapes_intersect.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp)
 
 rosbuild_add_gtest(test_core_collision test/test_core_collision.cpp)
 target_link_libraries(test_core_collision fcl)

trunk/fcl/include/fcl/broad_phase_collision.h

@@ -465,7 +465,7 @@ protected:
   };
 
   /** \brief Extention interval tree's interval to SAP interval, adding more information */
-  struct SAPInterval : public Interval
+  struct SAPInterval : public SimpleInterval
   {
     CollisionObject* obj;
     SAPInterval(double low_, double high_, CollisionObject* obj_)

trunk/fcl/include/fcl/interval.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ */
+
+/** \author Jia Pan */
+
+
+#ifndef FCL_INTERVAL_H
+#define FCL_INTERVAL_H
+
+#include "fcl/BVH_internal.h"
+#include <cstdlib>
+
+namespace fcl
+{
+
+struct Interval
+{
+  BVH_REAL i_[2];
+
+  Interval() {}
+  Interval(BVH_REAL v)
+  {
+    i_[0] = i_[1] = v;
+  }
+
+  Interval(BVH_REAL left, BVH_REAL right)
+  {
+    i_[0] = left; i_[1] = right;
+  }
+
+  inline void setValue(BVH_REAL a, BVH_REAL b)
+  {
+    i_[0] = a; i_[1] = b;
+  }
+
+  inline void setValue(BVH_REAL x)
+  {
+    i_[0] = i_[1] = x;
+  }
+
+  inline BVH_REAL operator [] (size_t i) const
+  {
+    return i_[i];
+  }
+
+  inline BVH_REAL& operator [] (size_t i)
+  {
+    return i_[i];
+  }
+
+  inline bool operator == (const Interval& other) const
+  {
+    if(i_[0] != other.i_[0]) return false;
+    if(i_[1] != other.i_[1]) return false;
+    return true;
+  }
+
+  inline Interval operator + (const Interval& other) const
+  {
+    return Interval(i_[0] + other.i_[0], i_[1] + other.i_[1]);
+  }
+
+  inline Interval operator - (const Interval& other) const
+  {
+    return Interval(i_[0] - other.i_[1], i_[1] - other.i_[0]);
+  }
+
+  inline Interval& operator += (const Interval& other)
+  {
+    i_[0] += other.i_[0];
+    i_[1] += other.i_[1];
+    return *this;
+  }
+
+  inline Interval& operator -= (const Interval& other)
+  {
+    i_[0] -= other.i_[1];
+    i_[1] -= other.i_[0];
+    return *this;
+  }
+
+  Interval operator * (const Interval& other) const;
+
+  inline Interval operator * (BVH_REAL d) const
+  {
+    if(d >= 0) return Interval(i_[0] * d, i_[1] * d);
+    return Interval(i_[1] * d, i_[0] * d);
+  }
+
+  inline Interval& operator *= (BVH_REAL d)
+  {
+    if(d >= 0)
+    {
+      i_[0] *= d;
+      i_[1] *= d;
+    }
+    else
+    {
+      BVH_REAL tmp = i_[0];
+      i_[0] = i_[1] * d;
+      i_[1] = tmp * d;
+    }
+
+    return *this;
+  }
+
+  /** \brief other must not contain 0 */
+  Interval operator / (const Interval& other) const;
+
+  /** \brief determine whether the intersection between intervals is empty */
+  inline bool overlap(const Interval& other) const
+  {
+    if(i_[1] < other.i_[0]) return false;
+    if(i_[0] > other.i_[1]) return false;
+    return true;
+  }
+
+  inline void intersect(const Interval& other)
+  {
+    if(i_[1] < other.i_[0]) return;
+    if(i_[0] > other.i_[1]) return;
+    if(i_[1] > other.i_[1]) i_[1] = other.i_[1];
+    if(i_[0] < other.i_[0]) i_[0] = other.i_[0];
+    return;
+  }
+
+  inline Interval operator - () const
+  {
+    return Interval(-i_[1], -i_[0]);
+  }
+
+  /** \brief Return the nearest distance for points within the interval to zero */
+  inline BVH_REAL getAbsLower() const
+  {
+    if(i_[0] >= 0) return i_[0];
+    if(i_[1] >= 0) return 0;
+    return -i_[1];
+  }
+
+  /** \brief Return the farthest distance for points within the interval to zero */
+  inline BVH_REAL getAbsUpper() const
+  {
+    if(i_[0] + i_[1] >= 0) return i_[1];
+    return i_[0];
+  }
+
+
+  inline bool contains(BVH_REAL v) const
+  {
+    if(v < i_[0]) return false;
+    if(v > i_[1]) return false;
+    return true;
+  }
+
+  /** \brief Compute the minimum interval contains v and original interval */
+  inline void bound(BVH_REAL v)
+  {
+    if(v < i_[0]) i_[0] = v;
+    if(v > i_[1]) i_[1] = v;
+  }
+
+  inline Interval bounded(BVH_REAL v) const
+  {
+    Interval res = *this;
+    if(v < res.i_[0]) res.i_[0] = v;
+    if(v > res.i_[1]) res.i_[1] = v;
+    return res;
+  }
+
+  /** \brief Compute the minimum interval contains other and original interval */
+  inline void bound(const Interval& other)
+  {
+    if(other.i_[0] < i_[0]) i_[0] = other.i_[0];
+    if(other.i_[1] > i_[1]) i_[1] = other.i_[1];
+  }
+
+  inline Interval bounded(const Interval& other) const
+  {
+    Interval res = *this;
+    if(other.i_[0] < res.i_[0]) res.i_[0] = other.i_[0];
+    if(other.i_[1] > res.i_[1]) res.i_[1] = other.i_[1];
+    return res;
+  }
+
+  void print() const;
+  inline BVH_REAL center() const { return 0.5 * (i_[0] + i_[1]); }
+  inline BVH_REAL diameter() const { return i_[1] -i_[0]; }
+};
+
+}
+#endif

trunk/fcl/include/fcl/interval_matrix.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ */
+
+/** \author Jia Pan */
+
+
+#ifndef FCL_INTERVAL_MATRIX_H
+#define FCL_INTERVAL_MATRIX_H
+
+#include "fcl/interval.h"
+#include "fcl/matrix_3f.h"
+#include "fcl/interval_vector.h"
+
+namespace fcl
+{
+
+struct IMatrix3
+{
+  Interval i_[3][3];
+
+  IMatrix3();
+  IMatrix3(BVH_REAL v);
+  IMatrix3(const Matrix3f& m);
+  IMatrix3(BVH_REAL m[3][3][2]);
+  IMatrix3(BVH_REAL m[3][3]);
+
+  void setIdentity();
+
+  IVector3 getColumn(size_t i) const;
+  IVector3 getRow(size_t i) const;
+
+  Vec3f getRealColumn(size_t i) const;
+  Vec3f getRealRow(size_t i) const;
+
+  IVector3 operator * (const Vec3f& v) const;
+  IVector3 operator * (const IVector3& v) const;
+  IMatrix3 operator * (const IMatrix3& m) const;
+  IMatrix3 operator * (const Matrix3f& m) const;
+  IMatrix3 operator + (const IMatrix3& m) const;
+  IMatrix3& operator += (const IMatrix3& m);
+
+  void print() const;
+
+  IMatrix3 nonIntervalAddMatrix(const IMatrix3& m) const;
+  IVector3 nonIntervalTimesVector(const IVector3& v) const;
+  IVector3 nonIntervalTimesVector(const Vec3f& v) const;
+
+};
+
+}
+
+#endif

trunk/fcl/include/fcl/interval_tree.h

@@ -50,11 +50,11 @@ namespace fcl
  * Can be replaced in part by boost::icl::interval_set, which is only supported after boost 1.46 and does not support delete node routine.
  */
 
-struct Interval
+struct SimpleInterval
 {
 public:
-  Interval() {}
-  virtual ~Interval() {}
+  SimpleInterval() {}
+  virtual ~SimpleInterval() {}
   virtual void print() {}
 
   double low, high;
@@ -69,13 +69,13 @@ public:
 
   IntervalTreeNode();
 
-  IntervalTreeNode(Interval* new_interval);
+  IntervalTreeNode(SimpleInterval* new_interval);
 
   ~IntervalTreeNode();
 
 protected:
 
-  Interval* stored_interval;
+  SimpleInterval* stored_interval;
 
   double key;
 
@@ -120,10 +120,10 @@ public:
   void print() const;
 
   /** \brief Delete one node of the interval tree */
-  Interval* deleteNode(IntervalTreeNode* node);
+  SimpleInterval* deleteNode(IntervalTreeNode* node);
 
   /** \brief Insert one node of the interval tree */
-  IntervalTreeNode* insert(Interval* new_interval);
+  IntervalTreeNode* insert(SimpleInterval* new_interval);
 
   /** \brief get the predecessor of a given node */
   IntervalTreeNode* getPredecessor(IntervalTreeNode* node) const;
@@ -132,7 +132,7 @@ public:
   IntervalTreeNode* getSuccessor(IntervalTreeNode* node) const;
 
   /** \brief Return result for a given query */
-  std::deque<Interval*> query(double low, double high);
+  std::deque<SimpleInterval*> query(double low, double high);
 
 protected:
 

trunk/fcl/include/fcl/interval_vector.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ */
+
+/** \author Jia Pan */
+
+
+#ifndef FCL_INTERVAL_VECTOR_H
+#define FCL_INTERVAL_VECTOR_H
+
+#include "fcl/interval.h"
+#include "fcl/vec_3f.h"
+
+namespace fcl
+{
+
+struct IVector3
+{
+  Interval i_[3];
+
+  IVector3();
+  IVector3(BVH_REAL v);
+  IVector3(BVH_REAL x, BVH_REAL y, BVH_REAL z);
+  IVector3(BVH_REAL xl, BVH_REAL xu, BVH_REAL yl, BVH_REAL yu, BVH_REAL zl, BVH_REAL zu);
+  IVector3(Interval v[3]);
+  IVector3(BVH_REAL v[3][2]);
+  IVector3(const Interval& v1, const Interval& v2, const Interval& v3);
+  IVector3(const Vec3f& v);
+
+  IVector3 operator + (const IVector3& other) const;
+  IVector3& operator += (const IVector3& other);
+  IVector3 operator - (const IVector3& other) const;
+  IVector3& operator -= (const IVector3& other);
+  IVector3& operator = (const Vec3f& other);
+  Interval dot(const IVector3& other) const;
+  IVector3 cross(const IVector3& other) const;
+
+  inline const Interval& operator [] (size_t i) const
+  {
+    return i_[i];
+  }
+
+  inline Interval& operator [] (size_t i)
+  {
+    return i_[i];
+  }
+
+  void print() const;
+  Vec3f center() const;
+  BVH_REAL volumn() const;
+  void setZero();
+
+  void bound(const Vec3f& v);
+  void bound(const IVector3& v);
+
+  IVector3 bounded(const Vec3f& v) const;
+  IVector3 bounded(const IVector3& v) const;
+
+  bool overlap(const IVector3& v) const;
+  bool contain(const IVector3& v) const;
+  void normalize();
+};
+
+}
+
+#endif

trunk/fcl/include/fcl/taylor_matrix.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ */
+
+/** \author Jia Pan */
+
+#ifndef FCL_TAYLOR_MATRIX_H
+#define FCL_TAYLOR_MATRIX_H
+
+#include "fcl/taylor_model.h"
+#include "fcl/matrix_3f.h"
+#include "fcl/taylor_vector.h"
+
+namespace fcl
+{
+
+struct TMatrix3
+{
+  TaylorModel i_[3][3];
+
+  TMatrix3();
+  TMatrix3(TaylorModel m[3][3]);
+  TMatrix3(const Matrix3f& m);
+
+  TVector3 getColumn(size_t i) const;
+  TVector3 getRow(size_t i) const;
+
+
+};
+
+}
+
+#endif

trunk/fcl/include/fcl/taylor_model.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ */
+
+/** \author Jia Pan */
+
+#ifndef FCL_TAYLOR_MODEL_H
+#define FCL_TAYLOR_MODEL_H
+
+#include "fcl/interval.h"
+
+namespace fcl
+{
+
+/** \brief TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function
+ * over a time interval, with an interval remainder.
+ * All the operations on two Taylor models assume their time intervals are the same.
+ */
+struct TaylorModel
+{
+  /** \brief Coefficients of the cubic polynomial approximation */
+  BVH_REAL coeffs_[4];
+
+  /** \brief interval remainder */
+  Interval r_;
+
+  void setTimeInterval(BVH_REAL l, BVH_REAL r);
+
+  TaylorModel();
+  TaylorModel(BVH_REAL coeff);
+  TaylorModel(BVH_REAL coeffs[3], const Interval& r);
+  TaylorModel(BVH_REAL c0, BVH_REAL c1, BVH_REAL c2, BVH_REAL c3, const Interval& r);
+
+  TaylorModel operator + (const TaylorModel& other) const;
+  TaylorModel operator - (const TaylorModel& other) const;
+  TaylorModel& operator += (const TaylorModel& other);
+  TaylorModel& operator -= (const TaylorModel& other);
+  TaylorModel operator * (const TaylorModel& other) const;
+  TaylorModel operator * (BVH_REAL d) const;
+  TaylorModel operator - () const;
+
+  void print() const;
+
+  Interval getBound() const;
+  Interval getBound(BVH_REAL l, BVH_REAL r) const;
+
+  Interval getTightBound() const;
+  Interval getTightBound(BVH_REAL l, BVH_REAL r) const;
+
+  Interval getBound(BVH_REAL t) const;
+
+  void setZero();
+
+  /** \brief time interval and different powers */
+  Interval t_; // [t1, t2]
+  Interval t2_; // [t1, t2]^2
+  Interval t3_; // [t1, t2]^3
+  Interval t4_; // [t1, t2]^4
+  Interval t5_; // [t1, t2]^5
+  Interval t6_; // [t1, t2]^6
+
+  static const BVH_REAL PI_;
+};
+
+void generateTaylorModelForCosFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0);
+void generateTaylorModelForSinFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0);
+void generateTaylorModelForLinearFunc(TaylorModel& tm, BVH_REAL p, BVH_REAL v);
+
+}
+
+#endif

trunk/fcl/include/fcl/taylor_vector.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ */
+
+/** \author Jia Pan */
+
+#ifndef FCL_TAYLOR_VECTOR_H
+#define FCL_TAYLOR_VECTOR_H
+
+#include "fcl/taylor_model.h"
+
+namespace fcl
+{
+
+struct TVector3
+{
+  TaylorModel i_[3];
+
+  TVector3();
+  TVector3(TaylorModel v[3]);
+  TVector3(const TaylorModel& v0, const TaylorModel& v1, const TaylorModel& v2);
+  TVector3(const Vec3f& v);
+
+  TVector3 operator + (const TVector3& other) const;
+  TVector3 operator + (BVH_REAL d) const;
+  TVector3& operator += (const TVector3& other);
+  TVector3 operator - (const TVector3& other) const;
+  TVector3& operator -= (const TVector3& other);
+  TVector3& operator = (const Vec3f& u);
+
+  TaylorModel dot(const TVector3& other) const;
+  TVector3 cross(const TVector3& other) const;
+
+  IVector3 getBound() const;
+  IVector3 getBound(BVH_REAL t) const;
+
+  void print() const;
+  BVH_REAL volumn() const;
+  void setZero();
+
+  TaylorModel squareLength() const;
+};
+
+void generateTVector3lForLinearFunc(TVector3& v, const Vec3f& position, const Vec3f& velocity);
+
+
+}
+
+#endif

trunk/fcl/include/fcl/vec_3f.h

@@ -212,6 +212,20 @@ namespace fcl
       return false;
     }
 
+    inline Vec3f normalized() const
+    {
+      BVH_REAL sqr_length = v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2];
+      if(sqr_length > EPSILON * EPSILON)
+      {
+        BVH_REAL inv_length = (BVH_REAL)1.0 / (BVH_REAL)sqrt(sqr_length);
+        return *this * inv_length;
+      }
+      else
+      {
+        return *this;
+      }
+    }
+
     /** \brief Return vector length */
     inline float length() const
     {
@@ -412,6 +426,21 @@ namespace fcl
       return false;
     }
 
+    inline Vec3f normalized() const
+    {
+      BVH_REAL sqr_length = v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2];
+      if(sqr_length > EPSILON * EPSILON)
+      {
+        BVH_REAL inv_length = (BVH_REAL)1.0 / (BVH_REAL)sqrt(sqr_length);
+        return *this * inv_length;
+      }
+      else
+      {
+        return *this;
+      }
+    }
+
+
     /** \brief Return vector length */
     inline BVH_REAL length() const
     {

trunk/fcl/src/broad_phase_collision.cpp

@@ -917,7 +917,7 @@ void IntervalTreeCollisionManager::collide(CollisionObject* obj, void* cdata, Co
 {
   static const unsigned int CUTOFF = 100;
 
-  std::deque<Interval*> results0, results1, results2;
+  std::deque<SimpleInterval*> results0, results1, results2;
 
   results0 = interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]);
   if(results0.size() > CUTOFF)

trunk/fcl/src/interval.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ */
+
+/** \author Jia Pan */
+
+#include "fcl/interval.h"
+#include <iostream>
+
+namespace fcl
+{
+
+Interval Interval::operator * (const Interval& other) const
+{
+  if(other.i_[0] >= 0)
+  {
+    if(i_[0] >= 0) return Interval(i_[0] * other.i_[0], i_[1] * other.i_[1]);
+    if(i_[1] <= 0) return Interval(i_[0] * other.i_[1], i_[1] * other.i_[0]);
+    return Interval(i_[0] * other.i_[1], i_[1] * other.i_[1]);
+  }
+  if(other.i_[1] <= 0)
+  {
+    if(i_[0] >= 0) return Interval(i_[1] * other.i_[0], i_[0] * other.i_[1]);
+    if(i_[1] <= 0) return Interval(i_[1] * other.i_[1], i_[0] * other.i_[0]);
+    return Interval(i_[1] * other.i_[0], i_[0] * other.i_[0]);
+  }
+
+  if(i_[0] >= 0) return Interval(i_[1] * other.i_[0], i_[1] * other.i_[1]);
+  if(i_[1] <= 0) return Interval(i_[0] * other.i_[1], i_[0] * other.i_[0]);
+
+  BVH_REAL v00 = i_[0] * other.i_[0];
+  BVH_REAL v11 = i_[1] * other.i_[1];
+  if(v00 <= v11)
+  {
+    BVH_REAL v01 = i_[0] * other.i_[1];
+    BVH_REAL v10 = i_[1] * other.i_[0];
+    if(v01 < v10) return Interval(v01, v11);
+    return Interval(v10, v11);
+  }
+
+  BVH_REAL v01 = i_[0] * other.i_[1];
+  BVH_REAL v10 = i_[1] * other.i_[0];
+  if(v01 < v10) return Interval(v01, v00);
+  return Interval(v10, v00);
+}
+
+Interval Interval::operator / (const Interval& other) const
+{
+  return *this * Interval(1.0 / other.i_[1], 1.0 / other.i_[0]);
+}
+
+void Interval::print() const
+{
+  std::cout << "[" << i_[0] << ", " << i_[1] << "]" << std::endl;
+}
+
+}

trunk/fcl/src/interval_tree.cpp

@@ -44,7 +44,7 @@ namespace fcl
 
 IntervalTreeNode::IntervalTreeNode(){}
 
-IntervalTreeNode::IntervalTreeNode(Interval* new_interval) :
+IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval) :
     stored_interval (new_interval),
     key(new_interval->low),
     high(new_interval->high),
@@ -157,7 +157,7 @@ void IntervalTree::fixupMaxHigh(IntervalTreeNode* x)
   }
 }
 
-IntervalTreeNode* IntervalTree::insert(Interval* new_interval)
+IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval)
 {
   IntervalTreeNode* y;
   IntervalTreeNode* x;
@@ -406,11 +406,11 @@ void IntervalTree::deleteFixup(IntervalTreeNode* x)
   x->red = false;
 }
 
-Interval* IntervalTree::deleteNode(IntervalTreeNode* z)
+SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z)
 {
   IntervalTreeNode* y;
   IntervalTreeNode* x;
-  Interval* node_to_delete = z->stored_interval;
+  SimpleInterval* node_to_delete = z->stored_interval;
 
   y= ((z->left == nil) || (z->right == nil)) ? z : getSuccessor(z);
   x= (y->left == nil) ? y->right : y->left;
@@ -477,9 +477,9 @@ bool overlap(double a1, double a2, double b1, double b2)
   }
 }
 
-std::deque<Interval*> IntervalTree::query(double low, double high)
+std::deque<SimpleInterval*> IntervalTree::query(double low, double high)
 {
-  std::deque<Interval*> result_stack;
+  std::deque<SimpleInterval*> result_stack;
   IntervalTreeNode* x = root->left;
   bool run = (x != nil);
 

trunk/fcl/src/interval_vector.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ */
+
+/** \author Jia Pan */
+
+#include "fcl/interval_vector.h"
+#include <iostream>
+
+namespace fcl
+{
+
+
+IVector3::IVector3() {}
+
+IVector3::IVector3(BVH_REAL v) { i_[0] = i_[1] = i_[2] = v; }
+
+IVector3::IVector3(BVH_REAL x, BVH_REAL y, BVH_REAL z)
+{
+  i_[0].setValue(x);
+  i_[1].setValue(y);
+  i_[2].setValue(z);
+}
+
+IVector3::IVector3(BVH_REAL xl, BVH_REAL xu, BVH_REAL yl, BVH_REAL yu, BVH_REAL zl, BVH_REAL zu)
+{
+  i_[0].setValue(xl, xu);
+  i_[1].setValue(yl, yu);
+  i_[2].setValue(zl, zu);
+}
+
+IVector3::IVector3(BVH_REAL v[3][2])
+{
+  i_[0].setValue(v[0][0], v[0][1]);
+  i_[1].setValue(v[1][0], v[1][1]);
+  i_[2].setValue(v[2][0], v[2][1]);
+}
+
+IVector3::IVector3(Interval v[3])
+{
+  i_[0] = v[0];
+  i_[1] = v[1];
+  i_[2] = v[2];
+}
+
+IVector3::IVector3(const Interval& v1, const Interval& v2, const Interval& v3)
+{
+  i_[0] = v1;
+  i_[1] = v2;
+  i_[2] = v3;
+}
+
+IVector3::IVector3(const Vec3f& v)
+{
+  i_[0].setValue(v[0]);
+  i_[1].setValue(v[1]);
+  i_[2].setValue(v[2]);
+}
+
+void IVector3::setZero()
+{
+  i_[0].setValue(0);
+  i_[1].setValue(0);
+  i_[2].setValue(0);
+}
+
+IVector3 IVector3::operator + (const IVector3& other) const
+{
+  Interval res[3];
+  res[0] = i_[0] + other[0];
+  res[1] = i_[1] + other[1];
+  res[2] = i_[2] + other[2];
+  return IVector3(res);
+}
+
+IVector3& IVector3::operator += (const IVector3& other)
+{
+  i_[0] += other[0];
+  i_[1] += other[1];
+  i_[2] += other[2];
+  return *this;
+}
+
+IVector3 IVector3::operator - (const IVector3& other) const
+{
+  Interval res[3];
+  res[0] = i_[0] - other[0];
+  res[1] = i_[1] - other[1];
+  res[2] = i_[2] - other[2];
+  return IVector3(res);
+}
+
+IVector3& IVector3::operator -= (const IVector3& other)
+{
+  i_[0] -= other[0];
+  i_[1] -= other[1];
+  i_[2] -= other[2];
+  return *this;
+}
+
+Interval IVector3::dot(const IVector3& other) const
+{
+  return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2];
+}
+
+IVector3 IVector3::cross(const IVector3& other) const
+{
+  Interval res[3];
+  res[0] = i_[1] * other[2] - i_[2] * other[1];
+  res[1] = i_[2] * other[0] - i_[0] * other[2];
+  res[2] = i_[0] * other[1] - i_[1] * other[0];
+  return IVector3(res);
+}
+
+BVH_REAL IVector3::volumn() const
+{
+  return i_[0].diameter() * i_[1].diameter() * i_[2].diameter();
+}
+
+void IVector3::print() const
+{
+  std::cout << "[" << i_[0][0] << "," << i_[0][1] << "]" << std::endl;
+  std::cout << "[" << i_[1][0] << "," << i_[1][1] << "]" << std::endl;
+  std::cout << "[" << i_[2][0] << "," << i_[2][1] << "]" << std::endl;
+}
+
+Vec3f IVector3::center() const
+{
+  return Vec3f(i_[0].center(), i_[1].center(), i_[2].center());
+}
+
+void IVector3::bound(const IVector3& v)
+{
+  if(v[0][0] < i_[0][0]) i_[0][0] = v[0][0];
+  if(v[1][0] < i_[1][0]) i_[1][0] = v[1][0];
+  if(v[2][0] < i_[2][0]) i_[2][0] = v[2][0];
+
+  if(v[0][1] > i_[0][1]) i_[0][1] = v[0][1];
+  if(v[1][1] > i_[1][1]) i_[1][1] = v[1][1];
+  if(v[2][1] > i_[2][1]) i_[2][1] = v[2][1];
+}
+
+void IVector3::bound(const Vec3f& v)
+{
+  if(v[0] < i_[0][0]) i_[0][0] = v[0];
+  if(v[1] < i_[1][0]) i_[1][0] = v[1];
+  if(v[2] < i_[2][0]) i_[2][0] = v[2];
+
+  if(v[0] > i_[0][1]) i_[0][1] = v[0];
+  if(v[1] > i_[1][1]) i_[1][1] = v[1];
+  if(v[2] > i_[2][1]) i_[2][1] = v[2];
+}
+
+
+IVector3 IVector3::bounded(const IVector3& v) const
+{
+  IVector3 res = *this;
+  if(v[0][0] < res.i_[0][0]) res.i_[0][0] = v[0][0];
+  if(v[1][0] < res.i_[1][0]) res.i_[1][0] = v[1][0];
+  if(v[2][0] < res.i_[2][0]) res.i_[2][0] = v[2][0];
+
+  if(v[0][1] > res.i_[0][1]) res.i_[0][1] = v[0][1];
+  if(v[1][1] > res.i_[1][1]) res.i_[1][1] = v[1][1];
+  if(v[2][1] > res.i_[2][1]) res.i_[2][1] = v[2][1];
+
+  return res;
+}
+
+IVector3 IVector3::bounded(const Vec3f& v) const
+{
+  IVector3 res = *this;
+  if(v[0] < res.i_[0][0]) res.i_[0][0] = v[0];
+  if(v[1] < res.i_[1][0]) res.i_[1][0] = v[1];
+  if(v[2] < res.i_[2][0]) res.i_[2][0] = v[2];
+
+  if(v[0] > res.i_[0][1]) res.i_[0][1] = v[0];
+  if(v[1] > res.i_[1][1]) res.i_[1][1] = v[1];
+  if(v[2] > res.i_[2][1]) res.i_[2][1] = v[2];
+
+  return res;
+}
+
+bool IVector3::overlap(const IVector3& v) const
+{
+  if(v[0][1] < i_[0][0]) return false;
+  if(v[1][1] < i_[1][0]) return false;
+  if(v[2][1] < i_[2][0]) return false;
+
+  if(v[0][0] > i_[0][1]) return false;
+  if(v[1][0] > i_[1][1]) return false;
+  if(v[2][0] > i_[2][1]) return false;
+
+  return true;
+}
+
+bool IVector3::contain(const IVector3& v) const
+{
+  if(v[0][0] < i_[0][0]) return false;
+  if(v[1][0] < i_[1][0]) return false;
+  if(v[2][0] < i_[2][0]) return false;
+
+  if(v[0][1] > i_[0][1]) return false;
+  if(v[1][1] > i_[1][1]) return false;
+  if(v[2][1] > i_[2][1]) return false;
+
+  return true;
+}
+
+}

trunk/fcl/src/taylor_model.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ */
+
+/** \author Jia Pan */
+
+#include "fcl/taylor_model.h"
+#include <cassert>
+#include <iostream>
+#include <cmath>
+
+namespace fcl
+{
+
+const BVH_REAL TaylorModel::PI_ = 3.1415626535;
+
+TaylorModel::TaylorModel() {}
+
+TaylorModel::TaylorModel(BVH_REAL coeff)
+{
+  coeffs_[0] = coeff;
+  coeffs_[1] = coeffs_[2] = coeffs_[3] = r_[0] = r_[1] = 0;
+}
+
+TaylorModel::TaylorModel(BVH_REAL coeffs[3], const Interval& r)
+{
+  coeffs_[0] = coeffs[0];
+  coeffs_[1] = coeffs[1];
+  coeffs_[2] = coeffs[2];
+  coeffs_[3] = coeffs[3];
+
+  r_ = r;
+}
+
+TaylorModel::TaylorModel(BVH_REAL c0, BVH_REAL c1, BVH_REAL c2, BVH_REAL c3, const Interval& r)
+{
+  coeffs_[0] = c0;
+  coeffs_[1] = c1;
+  coeffs_[2] = c2;
+  coeffs_[3] = c3;
+
+  r_ = r;
+}
+
+void TaylorModel::setTimeInterval(BVH_REAL l, BVH_REAL r)
+{
+  t_.setValue(l, r);
+  t2_.setValue(l * t_[0], r * t_[1]);
+  t3_.setValue(l * t2_[0], r * t2_[1]);
+  t4_.setValue(l * t3_[0], r * t3_[1]);
+  t5_.setValue(l * t4_[0], r * t4_[1]);
+  t6_.setValue(l * t5_[0], r * t5_[1]);
+}
+
+TaylorModel TaylorModel::operator + (const TaylorModel& other) const
+{
+  assert(other.t_ == t_);
+  return TaylorModel(coeffs_[0] + other.coeffs_[0], coeffs_[1] + other.coeffs_[1], coeffs_[2] + other.coeffs_[2], coeffs_[3] + other.coeffs_[3], r_ + other.r_);
+}
+
+TaylorModel TaylorModel::operator - (const TaylorModel& other) const
+{
+  assert(other.t_ == t_);
+  return TaylorModel(coeffs_[0] - other.coeffs_[0], coeffs_[1] - other.coeffs_[1], coeffs_[2] - other.coeffs_[2], coeffs_[3] - other.coeffs_[3], r_ - other.r_);
+}
+TaylorModel& TaylorModel::operator += (const TaylorModel& other)
+{
+  assert(other.t_ == t_);
+  coeffs_[0] += other.coeffs_[0];
+  coeffs_[1] += other.coeffs_[1];
+  coeffs_[2] += other.coeffs_[2];
+  coeffs_[3] += other.coeffs_[3];
+  r_ += other.r_;
+  return *this;
+}
+
+TaylorModel& TaylorModel::operator -= (const TaylorModel& other)
+{
+  assert(other.t_ == t_);
+  coeffs_[0] -= other.coeffs_[0];
+  coeffs_[1] -= other.coeffs_[1];
+  coeffs_[2] -= other.coeffs_[2];
+  coeffs_[3] -= other.coeffs_[3];
+  r_ -= other.r_;
+  return *this;
+}
+
+/** \brief Taylor model multiplication:
+ * f(t) = c0+c1*t+c2*t^2+c3*t^3+[a,b]
+ * g(t) = c0'+c1'*t+c2'*t^2+c3'*t^2+[c,d]
+ * f(t)g(t)= c0c0'+
+ *           (c0c1'+c1c0')t+
+ *           (c0c2'+c1c1'+c2c0')t^2+
+ *           (c0c3'+c1c2'+c2c1'+c3c0')t^3+
+ *           [a,b][c,d]+
+ *           (c1c3'+c2c2'+c3c1')t^4+
+ *           (c2c3'+c3c2')t^5+
+ *           (c3c3')t^6+
+ *           (c0+c1*t+c2*t^2+c3*t^3)[c,d]+
+ *           (c0'+c1'*t+c2'*t^2+c3'*c^3)[a,b]
+ */
+
+TaylorModel TaylorModel::operator * (const TaylorModel& other) const
+{
+  assert(other.t_ == t_);
+  register BVH_REAL c0, c1, c2, c3;
+  register BVH_REAL c0b = other.coeffs_[0], c1b = other.coeffs_[1], c2b = other.coeffs_[2], c3b = other.coeffs_[3];
+
+  const Interval& rb = other.r_;
+
+  c0 = coeffs_[0] * c0b;
+  c1 = coeffs_[0] * c1b + coeffs_[1] * c0b;
+  c2 = coeffs_[0] * c2b + coeffs_[1] * c1b + coeffs_[2] * c0b;
+  c3 = coeffs_[0] * c3b + coeffs_[1] * c2b + coeffs_[2] * c1b + coeffs_[3] * c0b;
+
+  Interval remainder(r_ * rb);
+  register BVH_REAL tempVal = coeffs_[1] * c3b + coeffs_[2] * c2b + coeffs_[3] * c1b;
+  remainder += t4_ * tempVal;
+
+  tempVal = coeffs_[2] * c3b + coeffs_[3] * c2b;
+  remainder += t5_ * tempVal;
+
+  tempVal = coeffs_[3] * c3b;
+  remainder += t6_ * tempVal;
+
+  remainder += ((Interval(coeffs_[0]) + t_ * coeffs_[1] + t2_ * coeffs_[2] + t3_ * coeffs_[3]) * rb +
+      (Interval(c0b) + t_ * c1b + t2_ * c2b + t3_ * c3b) * r_);
+
+  return TaylorModel(c0, c1, c2, c3, remainder);
+}
+
+TaylorModel TaylorModel::operator * (BVH_REAL d) const
+{
+  return TaylorModel(coeffs_[0] * d, coeffs_[1] * d, coeffs_[2] * d, coeffs_[3] * d,  r_ * d);
+}
+
+
+TaylorModel TaylorModel::operator - () const
+{
+  return TaylorModel(-coeffs_[0], -coeffs_[1], -coeffs_[2], -coeffs_[3], -r_);
+}
+
+void TaylorModel::print() const
+{
+  std::cout << coeffs_[0] << "+" << coeffs_[1] << "*t+" << coeffs_[2] << "*t^2+" << coeffs_[3] << "*t^3+[" << r_[0] << "," << r_[1] << "]" << std::endl;
+}
+
+Interval TaylorModel::getBound(BVH_REAL t) const
+{
+  return Interval(coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]))) + r_;
+}
+
+Interval TaylorModel::getBound(BVH_REAL t0, BVH_REAL t1) const
+{
+  Interval t(t0, t1);
+  Interval t2(t0 * t0, t1 * t1);
+  Interval t3(t0 * t2[0], t1 * t2[1]);
+
+  return Interval(coeffs_[0]) + t * coeffs_[1] + t2 * coeffs_[2] + t3 * coeffs_[3] + r_;
+}
+
+Interval TaylorModel::getBound() const
+{
+  return Interval(coeffs_[0] + r_[0], coeffs_[1] + r_[1]) + t_ * coeffs_[1] + t2_ * coeffs_[2] + t3_ * coeffs_[3];
+}
+
+Interval TaylorModel::getTightBound(BVH_REAL t0, BVH_REAL t1) const
+{
+
+  if(t0 < t_[0]) t0 = t_[0];
+  if(t1 > t_[1]) t1 = t_[1];
+
+  if(coeffs_[3] == 0)
+  {
+    register BVH_REAL a = -coeffs_[1] / (2 * coeffs_[2]);
+    Interval polybounds;
+    if(a <= t1 && a >= t0)
+    {
+      BVH_REAL AQ = coeffs_[0] + a * (coeffs_[1] + a * coeffs_[2]);
+      register BVH_REAL t = t0;
+      BVH_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
+      t = t1;
+      BVH_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
+
+      BVH_REAL minQ = LQ, maxQ = RQ;
+      if(LQ > RQ)
+      {
+        minQ = RQ;
+        maxQ = LQ;
+      }
+
+      if(minQ > AQ) minQ = AQ;
+      if(maxQ < AQ) maxQ = AQ;
+
+      polybounds.setValue(minQ, maxQ);
+    }
+    else
+    {
+      register BVH_REAL t = t0;
+      BVH_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
+      t = t1;
+      BVH_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
+
+      if(LQ > RQ) polybounds.setValue(RQ, LQ);
+      else polybounds.setValue(LQ, RQ);
+    }
+
+    return polybounds + r_;
+  }
+  else
+  {
+    register BVH_REAL t = t0;
+    BVH_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
+    t = t1;
+    BVH_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
+
+    if(LQ > RQ)
+    {
+      BVH_REAL tmp = LQ;
+      LQ = RQ;
+      RQ = tmp;
+    }
+
+    // derivative: c1+2*c2*t+3*c3*t^2
+
+    BVH_REAL delta = coeffs_[2] * coeffs_[2] - 3 * coeffs_[1] * coeffs_[3];
+    if(delta < 0)
+      return Interval(LQ, RQ) + r_;
+
+    BVH_REAL r1=(-coeffs_[2]-sqrt(delta))/(3*coeffs_[3]);
+    BVH_REAL r2=(-coeffs_[2]+sqrt(delta))/(3*coeffs_[3]);
+
+    if(r1 <= t1 && r1 >= t0)
+    {
+      BVH_REAL Q = coeffs_[0] + r1 * (coeffs_[1] + r1 * (coeffs_[2] + r1 * coeffs_[3]));
+      if(Q < LQ) LQ = Q;
+      else if(Q > RQ) RQ = Q;
+    }
+
+    if(r2 <= t1 && r2 >= t0)
+    {
+      BVH_REAL Q = coeffs_[0] + r2 * (coeffs_[1] + r2 * (coeffs_[2] + r2 * coeffs_[3]));
+      if(Q < LQ) LQ = Q;
+      else if(Q > RQ) RQ = Q;
+    }
+
+    return Interval(LQ, RQ) + r_;
+  }
+}
+
+Interval TaylorModel::getTightBound() const
+{
+  return getTightBound(t_[0], t_[1]);
+}
+
+void TaylorModel::setZero()
+{
+  coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
+  r_.setValue(0);
+}
+
+
+void generateTaylorModelForCosFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0)
+{
+  BVH_REAL a = tm.t_.center();
+  BVH_REAL t = w * a + q0;
+  BVH_REAL w2 = w * w;
+  BVH_REAL fa = cos(t);
+  BVH_REAL fda = -w*sin(t);
+  BVH_REAL fdda = -w2*fa;
+  BVH_REAL fddda = -w2*fda;
+
+  tm.coeffs_[0] = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda));
+  tm.coeffs_[1] = fda-a*fdda+0.5*a*a*fddda;
+  tm.coeffs_[2] = 0.5*(fdda-a*fddda);
+  tm.coeffs_[3] = 1.0/6.0*fddda;
+
+  // compute bounds for w^3 cos(wt+q0)/16, t \in [t0, t1]
+  Interval fddddBounds;
+  if(w == 0) fddddBounds.setValue(0);
+  else
+  {
+    BVH_REAL cosQL = cos(tm.t_[0] * w + q0);
+    BVH_REAL cosQR = cos(tm.t_[1] * w + q0);
+
+    if(cosQL < cosQR) fddddBounds.setValue(cosQL, cosQR);
+    else fddddBounds.setValue(cosQR, cosQL);
+
+    // enlarge to handle round-off errors
+    fddddBounds[0] -= 1e-15;
+    fddddBounds[1] += 1e-15;
+
+    // cos reaches maximum if there exists an integer k in [(w*t0+q0)/2pi, (w*t1+q0)/2pi];
+    // cos reaches minimum if there exists an integer k in [(w*t0+q0-pi)/2pi, (w*t1+q0-pi)/2pi]
+
+    BVH_REAL k1 = (tm.t_[0] * w + q0) / (2 * TaylorModel::PI_);
+    BVH_REAL k2 = (tm.t_[1] * w + q0) / (2 * TaylorModel::PI_);
+
+
+    if(w > 0)
+    {
+      if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1;
+      k1 -= 0.5;
+      k2 -= 0.5;
+      if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1;
+    }
+    else
+    {
+      if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1;
+      k1 -= 0.5;
+      k2 -= 0.5;
+      if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1;
+    }
+  }
+
+  BVH_REAL w4 = w2 * w2;
+  fddddBounds *= w4;
+
+  BVH_REAL midSize = 0.5 * (tm.t_[1] - tm.t_[0]);
+  BVH_REAL midSize2 = midSize * midSize;
+  BVH_REAL midSize4 = midSize2 * midSize2;
+
+  // [0, midSize4] * fdddBounds
+  if(fddddBounds[0] > 0)
+    tm.r_.setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24));
+  else if(fddddBounds[0] < 0)
+    tm.r_.setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0);
+  else
+    tm.r_.setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24));
+}
+
+void generateTaylorModelForSinFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0)
+{
+  BVH_REAL a = tm.t_.center();
+  BVH_REAL t = w * a + q0;
+  BVH_REAL w2 = w * w;
+  BVH_REAL fa = sin(t);
+  BVH_REAL fda = w*cos(t);
+  BVH_REAL fdda = -w2*fa;
+  BVH_REAL fddda = -w2*fda;
+
+  tm.coeffs_[0] = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda));
+  tm.coeffs_[1] = fda-a*fdda+0.5*a*a*fddda;
+  tm.coeffs_[2] = 0.5*(fdda-a*fddda);
+  tm.coeffs_[3] = 1.0/6.0*fddda;
+
+  // compute bounds for w^3 sin(wt+q0)/16, t \in [t0, t1]
+
+  Interval fddddBounds;
+
+  if(w == 0) fddddBounds.setValue(0);
+  else
+  {
+    BVH_REAL sinQL = sin(w * tm.t_[0] + q0);
+    BVH_REAL sinQR = sin(w * tm.t_[1] + q0);
+
+    if(sinQL < sinQR) fddddBounds.setValue(sinQL, sinQR);
+    else fddddBounds.setValue(sinQR, sinQL);
+
+    // enlarge to handle round-off errors
+    fddddBounds[0] -= 1e-15;
+    fddddBounds[1] += 1e-15;
+
+    // sin reaches maximum if there exists an integer k in [(w*t0+q0-pi/2)/2pi, (w*t1+q0-pi/2)/2pi];
+    // sin reaches minimum if there exists an integer k in [(w*t0+q0-pi-pi/2)/2pi, (w*t1+q0-pi-pi/2)/2pi]
+
+    BVH_REAL k1 = (tm.t_[0] * w + q0) / (2 * TaylorModel::PI_) - 0.25;
+    BVH_REAL k2 = (tm.t_[1] * w + q0) / (2 * TaylorModel::PI_) - 0.25;
+
+    if(w > 0)
+    {
+      if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1;
+      k1 -= 0.5;
+      k2 -= 0.5;
+      if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1;
+    }
+    else
+    {
+      if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1;
+      k1 -= 0.5;
+      k2 -= 0.5;
+      if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1;
+    }
+
+    BVH_REAL w4 = w2 * w2;
+    fddddBounds *= w4;
+
+    BVH_REAL midSize = 0.5 * (tm.t_[1] - tm.t_[0]);
+    BVH_REAL midSize2 = midSize * midSize;
+    BVH_REAL midSize4 = midSize2 * midSize2;
+
+    // [0, midSize4] * fdddBounds
+    if(fddddBounds[0] > 0)
+      tm.r_.setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24));
+    else if(fddddBounds[0] < 0)
+      tm.r_.setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0);
+    else
+      tm.r_.setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24));
+  }
+}
+
+void generateTaylorModelForLinearFunc(TaylorModel& tm, BVH_REAL p, BVH_REAL v)
+{
+  tm.coeffs_[0] = p;
+  tm.coeffs_[1] = v;
+  tm.coeffs_[2] = tm.coeffs_[3] = tm.r_[0] = tm.r_[1] = 0;
+}
+
+}