diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index d948e8b182b3182ed6e46e01680afae6f2b35804..fbe11232fc30236acebdc12ae8445128cbfd1267 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/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)
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index 087e90a38c4790416d124532f978899b1c7ec984..b99359cbc78ca0cd99dc0c5f9c18881456bba6b8 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/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_)
diff --git a/trunk/fcl/include/fcl/interval.h b/trunk/fcl/include/fcl/interval.h
new file mode 100644
index 0000000000000000000000000000000000000000..06eeb0d17cf02bc2e8a03b362a702e80ef6d62d8
--- /dev/null
+++ b/trunk/fcl/include/fcl/interval.h
@@ -0,0 +1,222 @@
+/*
+ * 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
diff --git a/trunk/fcl/include/fcl/interval_matrix.h b/trunk/fcl/include/fcl/interval_matrix.h
new file mode 100644
index 0000000000000000000000000000000000000000..c21f61a2d03c57e85d5762c73125b8430cbcccbb
--- /dev/null
+++ b/trunk/fcl/include/fcl/interval_matrix.h
@@ -0,0 +1,83 @@
+/*
+ * 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
diff --git a/trunk/fcl/include/fcl/interval_tree.h b/trunk/fcl/include/fcl/interval_tree.h
index 90763b07727268d5571212c48a5bd6afc8eb411d..94f290a571a3fbb786f0c21a1a947cf7789cd1dd 100644
--- a/trunk/fcl/include/fcl/interval_tree.h
+++ b/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:
diff --git a/trunk/fcl/include/fcl/interval_vector.h b/trunk/fcl/include/fcl/interval_vector.h
new file mode 100644
index 0000000000000000000000000000000000000000..dba1f789d920c632b1e14f6c327e27e9a747087c
--- /dev/null
+++ b/trunk/fcl/include/fcl/interval_vector.h
@@ -0,0 +1,96 @@
+/*
+ * 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
diff --git a/trunk/fcl/include/fcl/taylor_matrix.h b/trunk/fcl/include/fcl/taylor_matrix.h
new file mode 100644
index 0000000000000000000000000000000000000000..92d9f7766e02c7d2bc579648898226f8899eb868
--- /dev/null
+++ b/trunk/fcl/include/fcl/taylor_matrix.h
@@ -0,0 +1,63 @@
+/*
+ * 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
diff --git a/trunk/fcl/include/fcl/taylor_model.h b/trunk/fcl/include/fcl/taylor_model.h
new file mode 100644
index 0000000000000000000000000000000000000000..c7c414289062af77ee5ef763d4ad53851e91dcef
--- /dev/null
+++ b/trunk/fcl/include/fcl/taylor_model.h
@@ -0,0 +1,101 @@
+/*
+ * 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
diff --git a/trunk/fcl/include/fcl/taylor_vector.h b/trunk/fcl/include/fcl/taylor_vector.h
new file mode 100644
index 0000000000000000000000000000000000000000..46b69db1355c8054a369e029b8e9cc447a152766
--- /dev/null
+++ b/trunk/fcl/include/fcl/taylor_vector.h
@@ -0,0 +1,79 @@
+/*
+ * 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
diff --git a/trunk/fcl/include/fcl/vec_3f.h b/trunk/fcl/include/fcl/vec_3f.h
index 6534e9635493322852dc8521ce2c3090ed8f089d..1b927a2d480a47a904ee7d2bb7a5e38330e44440 100644
--- a/trunk/fcl/include/fcl/vec_3f.h
+++ b/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
{
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index 2bedad2ef41f848625df7dcb780201d6fa331ce4..d259f0a2cfd8db99fd4832efa3586714ae8da961 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/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)
diff --git a/trunk/fcl/src/interval.cpp b/trunk/fcl/src/interval.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7cb9a3e0971442191cfc28164e52272ff119d803
--- /dev/null
+++ b/trunk/fcl/src/interval.cpp
@@ -0,0 +1,87 @@
+/*
+ * 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;
+}
+
+}
diff --git a/trunk/fcl/src/interval_matrix.cpp b/trunk/fcl/src/interval_matrix.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1dab6143e80093f4f024f241e48c4c7cef724702
--- /dev/null
+++ b/trunk/fcl/src/interval_matrix.cpp
@@ -0,0 +1,746 @@
+/*
+ * 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_matrix.h"
+#include <iostream>
+
+namespace fcl
+{
+
+IMatrix3::IMatrix3()
+{
+ i_[0][0] = i_[0][1] = i_[0][2] = i_[1][0] = i_[1][1] = i_[1][2] = i_[2][0] = i_[2][1] = i_[2][2] = 0;
+}
+
+IMatrix3::IMatrix3(BVH_REAL v)
+{
+ i_[0][0] = i_[0][1] = i_[0][2] = i_[1][0] = i_[1][1] = i_[1][2] = i_[2][0] = i_[2][1] = i_[2][2] = v;
+}
+
+IMatrix3::IMatrix3(const Matrix3f& m)
+{
+ i_[0][0] = m[0][0];
+ i_[0][1] = m[0][1];
+ i_[0][2] = m[0][2];
+
+ i_[1][0] = m[1][0];
+ i_[1][1] = m[1][1];
+ i_[1][2] = m[1][2];
+
+ i_[2][0] = m[2][0];
+ i_[2][1] = m[2][1];
+ i_[2][2] = m[2][2];
+}
+
+IMatrix3::IMatrix3(BVH_REAL m[3][3][2])
+{
+ i_[0][0].setValue(m[0][0][0], m[0][0][1]);
+ i_[0][1].setValue(m[0][1][0], m[0][1][1]);
+ i_[0][2].setValue(m[0][2][0], m[0][2][1]);
+
+ i_[1][0].setValue(m[1][0][0], m[1][0][1]);
+ i_[1][1].setValue(m[1][1][0], m[1][1][1]);
+ i_[1][2].setValue(m[1][2][0], m[1][2][1]);
+
+ i_[2][0].setValue(m[2][0][0], m[2][0][1]);
+ i_[2][1].setValue(m[2][1][0], m[2][1][1]);
+ i_[2][2].setValue(m[2][2][0], m[2][2][1]);
+}
+
+IMatrix3::IMatrix3(BVH_REAL m[3][3])
+{
+ i_[0][0].setValue(m[0][0]);
+ i_[0][1].setValue(m[0][1]);
+ i_[0][2].setValue(m[0][2]);
+
+ i_[1][0].setValue(m[1][0]);
+ i_[1][1].setValue(m[1][1]);
+ i_[1][2].setValue(m[1][2]);
+
+ i_[2][0].setValue(m[2][0]);
+ i_[2][1].setValue(m[2][1]);
+ i_[2][2].setValue(m[2][2]);
+}
+
+void IMatrix3::setIdentity()
+{
+ i_[0][0] = 1;
+ i_[0][1] = 0;
+ i_[0][2] = 0;
+
+ i_[1][0] = 0;
+ i_[1][1] = 1;
+ i_[1][2] = 0;
+
+ i_[2][0] = 0;
+ i_[2][1] = 0;
+ i_[2][2] = 1;
+}
+
+IVector3 IMatrix3::getColumn(size_t i) const
+{
+ return IVector3(i_[0][i], i_[1][i], i_[2][i]);
+}
+
+IVector3 IMatrix3::getRow(size_t i) const
+{
+ return IVector3(i_[i][0], i_[i][1], i_[i][2]);
+}
+
+Vec3f IMatrix3::getRealColumn(size_t i) const
+{
+ return Vec3f(i_[0][i][0], i_[1][i][0], i_[2][i][0]);
+}
+
+Vec3f IMatrix3::getRealRow(size_t i) const
+{
+ return Vec3f(i_[i][0][0], i_[i][1][0], i_[i][2][0]);
+}
+
+IMatrix3 IMatrix3::operator * (const Matrix3f& m) const
+{
+ BVH_REAL res[3][3][2];
+
+ if(m[0][0] < 0)
+ {
+ res[0][0][0] = m[0][0] * i_[0][0][1];
+ res[0][0][1] = m[0][0] * i_[0][0][0];
+ res[1][0][0] = m[0][0] * i_[1][0][1];
+ res[1][0][1] = m[0][0] * i_[1][0][0];
+ res[2][0][0] = m[0][0] * i_[2][0][1];
+ res[2][0][1] = m[0][0] * i_[2][0][0];
+ }
+ else
+ {
+ res[0][0][0] = m[0][0] * i_[0][0][0];
+ res[0][0][1] = m[0][0] * i_[0][0][1];
+ res[1][0][0] = m[0][0] * i_[1][0][0];
+ res[1][0][1] = m[0][0] * i_[1][0][1];
+ res[2][0][0] = m[0][0] * i_[2][0][0];
+ res[2][0][1] = m[0][0] * i_[2][0][1];
+ }
+
+ if(m[0][1] < 0)
+ {
+ res[0][1][0] = m[0][1] * i_[0][0][1];
+ res[0][1][1] = m[0][1] * i_[0][0][0];
+ res[1][1][0] = m[0][1] * i_[1][0][1];
+ res[1][1][1] = m[0][1] * i_[1][0][0];
+ res[2][1][0] = m[0][1] * i_[2][0][1];
+ res[2][1][1] = m[0][1] * i_[2][0][0];
+ }
+ else
+ {
+ res[0][1][0] = m[0][1] * i_[0][0][0];
+ res[0][1][1] = m[0][1] * i_[0][0][1];
+ res[1][1][0] = m[0][1] * i_[1][0][0];
+ res[1][1][1] = m[0][1] * i_[1][0][1];
+ res[2][1][0] = m[0][1] * i_[2][0][0];
+ res[2][1][1] = m[0][1] * i_[2][0][1];
+ }
+
+ if(m[0][2] < 0)
+ {
+ res[0][2][0] = m[0][2] * i_[0][0][1];
+ res[0][2][1] = m[0][2] * i_[0][0][0];
+ res[1][2][0] = m[0][2] * i_[1][0][1];
+ res[1][2][1] = m[0][2] * i_[1][0][0];
+ res[2][2][0] = m[0][2] * i_[2][0][1];
+ res[2][2][1] = m[0][2] * i_[2][0][0];
+ }
+ else
+ {
+ res[0][2][0] = m[0][2] * i_[0][0][0];
+ res[0][2][1] = m[0][2] * i_[0][0][1];
+ res[1][2][0] = m[0][2] * i_[1][0][0];
+ res[1][2][1] = m[0][2] * i_[1][0][1];
+ res[2][2][0] = m[0][2] * i_[2][0][0];
+ res[2][2][1] = m[0][2] * i_[2][0][1];
+ }
+
+ if(m[1][0] < 0)
+ {
+ res[0][0][0] += m[1][0] * i_[0][1][1];
+ res[0][0][1] += m[1][0] * i_[0][1][0];
+ res[1][0][0] += m[1][0] * i_[1][1][1];
+ res[1][0][1] += m[1][0] * i_[1][1][0];
+ res[2][0][0] += m[1][0] * i_[2][1][1];
+ res[2][0][1] += m[1][0] * i_[2][1][0];
+ }
+ else
+ {
+ res[0][0][0] += m[1][0] * i_[0][1][0];
+ res[0][0][1] += m[1][0] * i_[0][1][1];
+ res[1][0][0] += m[1][0] * i_[1][1][0];
+ res[1][0][1] += m[1][0] * i_[1][1][1];
+ res[2][0][0] += m[1][0] * i_[2][1][0];
+ res[2][0][1] += m[1][0] * i_[2][1][1];
+ }
+
+ if(m[1][1] < 0)
+ {
+ res[0][1][0] += m[1][1] * i_[0][1][1];
+ res[0][1][1] += m[1][1] * i_[0][1][0];
+ res[1][1][0] += m[1][1] * i_[1][1][1];
+ res[1][1][1] += m[1][1] * i_[1][1][0];
+ res[2][1][0] += m[1][1] * i_[2][1][1];
+ res[2][1][1] += m[1][1] * i_[2][1][0];
+ }
+ else
+ {
+ res[0][1][0] += m[1][1] * i_[0][1][0];
+ res[0][1][1] += m[1][1] * i_[0][1][1];
+ res[1][1][0] += m[1][1] * i_[1][1][0];
+ res[1][1][1] += m[1][1] * i_[1][1][1];
+ res[2][1][0] += m[1][1] * i_[2][1][0];
+ res[2][1][1] += m[1][1] * i_[2][1][1];
+ }
+
+ if(m[1][2] < 0)
+ {
+ res[0][2][0] += m[1][2] * i_[0][1][1];
+ res[0][2][1] += m[1][2] * i_[0][1][0];
+ res[1][2][0] += m[1][2] * i_[1][1][1];
+ res[1][2][1] += m[1][2] * i_[1][1][0];
+ res[2][2][0] += m[1][2] * i_[2][1][1];
+ res[2][2][1] += m[1][2] * i_[2][1][0];
+ }
+ else
+ {
+ res[0][2][0] += m[1][2] * i_[0][1][0];
+ res[0][2][1] += m[1][2] * i_[0][1][1];
+ res[1][2][0] += m[1][2] * i_[1][1][0];
+ res[1][2][1] += m[1][2] * i_[1][1][1];
+ res[2][2][0] += m[1][2] * i_[2][1][0];
+ res[2][2][1] += m[1][2] * i_[2][1][1];
+ }
+
+ if(m[2][0] < 0)
+ {
+ res[0][0][0] += m[2][0] * i_[0][2][1];
+ res[0][0][1] += m[2][0] * i_[0][2][0];
+ res[1][0][0] += m[2][0] * i_[1][2][1];
+ res[1][0][1] += m[2][0] * i_[1][2][0];
+ res[2][0][0] += m[2][0] * i_[2][2][1];
+ res[2][0][1] += m[2][0] * i_[2][2][0];
+ }
+ else
+ {
+ res[0][0][0] += m[2][0] * i_[0][2][0];
+ res[0][0][1] += m[2][0] * i_[0][2][1];
+ res[1][0][0] += m[2][0] * i_[1][2][0];
+ res[1][0][1] += m[2][0] * i_[1][2][1];
+ res[2][0][0] += m[2][0] * i_[2][2][0];
+ res[2][0][1] += m[2][0] * i_[2][2][1];
+ }
+
+ if(m[2][1] < 0)
+ {
+ res[0][1][0] += m[2][1] * i_[0][2][1];
+ res[0][1][1] += m[2][1] * i_[0][2][0];
+ res[1][1][0] += m[2][1] * i_[1][2][1];
+ res[1][1][1] += m[2][1] * i_[1][2][0];
+ res[2][1][0] += m[2][1] * i_[2][2][1];
+ res[2][1][1] += m[2][1] * i_[2][2][0];
+ }
+ else
+ {
+ res[0][1][0] += m[2][1] * i_[0][2][0];
+ res[0][1][1] += m[2][1] * i_[0][2][1];
+ res[1][1][0] += m[2][1] * i_[1][2][0];
+ res[1][1][1] += m[2][1] * i_[1][2][1];
+ res[2][1][0] += m[2][1] * i_[2][2][0];
+ res[2][1][1] += m[2][1] * i_[2][2][1];
+ }
+
+ if(m[2][2] < 0)
+ {
+ res[0][2][0] += m[2][2] * i_[0][2][1];
+ res[0][2][1] += m[2][2] * i_[0][2][0];
+ res[1][2][0] += m[2][2] * i_[1][2][1];
+ res[1][2][1] += m[2][2] * i_[1][2][0];
+ res[2][2][0] += m[2][2] * i_[2][2][1];
+ res[2][2][1] += m[2][2] * i_[2][2][0];
+ }
+ else
+ {
+ res[0][2][0] += m[2][2] * i_[0][2][0];
+ res[0][2][1] += m[2][2] * i_[0][2][1];
+ res[1][2][0] += m[2][2] * i_[1][2][0];
+ res[1][2][1] += m[2][2] * i_[1][2][1];
+ res[2][2][0] += m[2][2] * i_[2][2][0];
+ res[2][2][1] += m[2][2] * i_[2][2][1];
+ }
+
+ return IMatrix3(res);
+}
+
+IVector3 IMatrix3::operator * (const Vec3f& v) const
+{
+ BVH_REAL res[3][2];
+
+ if(v[0] < 0)
+ {
+ res[0][0] = v[0] * i_[0][0][1];
+ res[0][1] = v[0] * i_[0][0][0];
+ res[1][0] = v[0] * i_[1][0][1];
+ res[1][1] = v[0] * i_[1][0][0];
+ res[2][0] = v[0] * i_[2][0][1];
+ res[2][1] = v[0] * i_[2][0][0];
+ }
+ else
+ {
+ res[0][0] = v[0] * i_[0][0][0];
+ res[0][1] = v[0] * i_[0][0][1];
+ res[1][0] = v[0] * i_[1][0][0];
+ res[1][1] = v[0] * i_[1][0][1];
+ res[2][0] = v[0] * i_[2][0][0];
+ res[2][1] = v[0] * i_[2][0][1];
+ }
+
+ if(v[1] < 0)
+ {
+ res[0][0] += v[1] * i_[0][1][1];
+ res[0][1] += v[1] * i_[0][1][0];
+ res[1][0] += v[1] * i_[1][1][1];
+ res[1][1] += v[1] * i_[1][1][0];
+ res[2][0] += v[1] * i_[2][1][1];
+ res[2][1] += v[1] * i_[2][1][0];
+ }
+ else
+ {
+ res[0][0] += v[1] * i_[0][1][0];
+ res[0][1] += v[1] * i_[0][1][1];
+ res[1][0] += v[1] * i_[1][1][0];
+ res[1][1] += v[1] * i_[1][1][1];
+ res[2][0] += v[1] * i_[2][1][0];
+ res[2][1] += v[1] * i_[2][1][1];
+ }
+
+ if(v[2] < 0)
+ {
+ res[0][0] += v[2] * i_[0][2][1];
+ res[0][1] += v[2] * i_[0][2][0];
+ res[1][0] += v[2] * i_[1][2][1];
+ res[1][1] += v[2] * i_[1][2][0];
+ res[2][0] += v[2] * i_[2][2][1];
+ res[2][1] += v[2] * i_[2][2][0];
+ }
+ else
+ {
+ res[0][0] += v[2] * i_[0][2][0];
+ res[0][1] += v[2] * i_[0][2][1];
+ res[1][0] += v[2] * i_[1][2][0];
+ res[1][1] += v[2] * i_[1][2][1];
+ res[2][0] += v[2] * i_[2][2][0];
+ res[2][1] += v[2] * i_[2][2][1];
+ }
+
+ return IVector3(res);
+}
+
+
+IMatrix3 IMatrix3::nonIntervalAddMatrix(const IMatrix3& m) const
+{
+ BVH_REAL res[3][3];
+
+ res[0][0] = i_[0][0][0] + m.i_[0][0][0];
+ res[0][1] = i_[0][1][0] + m.i_[0][1][0];
+ res[0][2] = i_[0][2][0] + m.i_[0][2][0];
+
+ res[1][0] = i_[1][0][0] + m.i_[1][0][0];
+ res[1][1] = i_[1][1][0] + m.i_[1][1][0];
+ res[1][2] = i_[1][2][0] + m.i_[1][2][0];
+
+ res[2][0] = i_[2][0][0] + m.i_[2][0][0];
+ res[2][1] = i_[2][1][0] + m.i_[2][1][0];
+ res[2][2] = i_[2][2][0] + m.i_[2][2][0];
+
+ return IMatrix3(res);
+}
+
+IVector3 IMatrix3::operator * (const IVector3& v) const
+{
+ BVH_REAL xl, xu, yl, yu, zl, zu;
+ register BVH_REAL temp, vmin, vmax;
+
+ // r.v.i_[0]
+ vmin = vmax = i_[0][0][0] * v.i_[0][0];
+ temp = i_[0][0][1] * v.i_[0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][0][1] * v.i_[0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ xl = vmin; xu = vmax;
+
+ vmin = vmax = i_[0][1][0] * v.i_[1][0];
+ temp = i_[0][1][0] * v.i_[1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][1][1] * v.i_[1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][1][1] * v.i_[1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ xl += vmin; xu += vmax;
+
+ vmin = vmax = i_[0][2][0] * v.i_[2][0];
+ temp = i_[0][2][0] * v.i_[2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][2][1] * v.i_[2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][2][1] * v.i_[2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ xl += vmin; xu += vmax;
+
+ // r.v.i_[1]
+
+ vmin = vmax = i_[1][0][0] * v.i_[0][0];
+ temp = i_[1][0][0] * v.i_[0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][0][1] * v.i_[0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][0][1] * v.i_[0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ yl = vmin; yu = vmax;
+
+ vmin = vmax = i_[1][1][0] * v.i_[1][0];
+ temp = i_[1][1][0] * v.i_[1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][1][1] * v.i_[1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][1][1] * v.i_[1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ yl += vmin; yu += vmax;
+
+ vmin = vmax = i_[1][2][0] * v.i_[2][0];
+ temp = i_[1][2][0] * v.i_[2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][2][1] * v.i_[2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][2][1] * v.i_[2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ yl += vmin; yu += vmax;
+
+ // r.v.i_[2]
+
+ vmin = vmax = i_[2][0][0] * v.i_[0][0];
+ temp = i_[2][0][0] * v.i_[0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][0][1] * v.i_[0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][0][1] * v.i_[0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ zl = vmin; zu = vmax;
+
+ vmin = vmax = i_[2][1][0] * v.i_[1][0];
+ temp = i_[2][1][0] * v.i_[1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][1][1] * v.i_[1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][1][1] * v.i_[1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ zl += vmin; zu += vmax;
+
+ vmin = vmax = i_[2][2][0] * v.i_[2][0];
+ temp = i_[2][2][0] * v.i_[2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][2][1] * v.i_[2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][2][1] * v.i_[2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ zl += vmin; zu += vmax; vmin = vmax = i_[0][0][0] * v.i_[0][0];
+ temp = i_[0][0][0] * v.i_[0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][0][1] * v.i_[0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][0][1] * v.i_[0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ xl = vmin; xu = vmax;
+
+ vmin = vmax = i_[0][1][0] * v.i_[1][0];
+ temp = i_[0][1][0] * v.i_[1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][1][1] * v.i_[1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][1][1] * v.i_[1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ xl += vmin; xu += vmax;
+
+ vmin = vmax = i_[0][2][0] * v.i_[2][0];
+ temp = i_[0][2][0] * v.i_[2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][2][1] * v.i_[2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][2][1] * v.i_[2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ xl += vmin; xu += vmax;
+
+ // r.v.i_[1]
+
+ vmin = vmax = i_[1][0][0] * v.i_[0][0];
+ temp = i_[1][0][0] * v.i_[0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][0][1] * v.i_[0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][0][1] * v.i_[0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ yl = vmin; yu = vmax;
+
+ vmin = vmax = i_[1][1][0] * v.i_[1][0];
+ temp = i_[1][1][0] * v.i_[1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][1][1] * v.i_[1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][1][1] * v.i_[1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ yl += vmin; yu += vmax;
+
+ vmin = vmax = i_[1][2][0] * v.i_[2][0];
+ temp = i_[1][2][0] * v.i_[2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][2][1] * v.i_[2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][2][1] * v.i_[2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ yl += vmin; yu += vmax;
+
+ // r.v.i_[2]
+
+ vmin = vmax = i_[2][0][0] * v.i_[0][0];
+ temp = i_[2][0][0] * v.i_[0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][0][1] * v.i_[0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][0][1] * v.i_[0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ zl = vmin; zu = vmax;
+
+ vmin = vmax = i_[2][1][0] * v.i_[1][0];
+ temp = i_[2][1][0] * v.i_[1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][1][1] * v.i_[1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][1][1] * v.i_[1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ zl += vmin; zu += vmax;
+
+ vmin = vmax = i_[2][2][0] * v.i_[2][0];
+ temp = i_[2][2][0] * v.i_[2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][2][1] * v.i_[2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][2][1] * v.i_[2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ zl += vmin; zu += vmax;
+
+ return IVector3(xl, xu, yl, yu, zl, zu);
+}
+
+IMatrix3 IMatrix3::operator * (const IMatrix3& m) const
+{
+
+ register BVH_REAL temp, vmin, vmax;
+ BVH_REAL res[3][3][2];
+
+ // res[0][0]
+
+ vmin = vmax = i_[0][0][0] * m.i_[0][0][0];
+ temp = i_[0][0][0] * m.i_[0][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][0][1] * m.i_[0][0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][0][1] * m.i_[0][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[0][0][0] = vmin; res[0][0][1] = vmax;
+
+ vmin = vmax = i_[0][1][0] * m.i_[1][0][0];
+ temp = i_[0][1][0] * m.i_[1][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][1][1] * m.i_[1][0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][1][1] * m.i_[1][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[0][0][0] += vmin; res[0][0][1] += vmax;
+
+ vmin = vmax = i_[0][2][0] * m.i_[2][0][0];
+ temp = i_[0][2][0] * m.i_[2][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][2][1] * m.i_[2][0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][2][1] * m.i_[2][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[0][0][0] += vmin; res[0][0][1] += vmax;
+
+ // res[1][0]
+
+ vmin = vmax = i_[1][0][0] * m.i_[0][0][0];
+ temp = i_[1][0][0] * m.i_[0][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][0][1] * m.i_[0][0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][0][1] * m.i_[0][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[1][0][0] = vmin; res[1][0][1] = vmax;
+
+ vmin = vmax = i_[1][1][0] * m.i_[1][0][0];
+ temp = i_[1][1][0] * m.i_[1][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][1][1] * m.i_[1][0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][1][1] * m.i_[1][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[1][0][0] += vmin; res[1][0][1] += vmax;
+
+ vmin = vmax = i_[1][2][0] * m.i_[2][0][0];
+ temp = i_[1][2][0] * m.i_[2][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][2][1] * m.i_[2][0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][2][1] * m.i_[2][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[1][0][0] += vmin; res[1][0][1] += vmax;
+
+ // res[2][0]
+
+ vmin = vmax = i_[2][0][0] * m.i_[0][0][0];
+ temp = i_[2][0][0] * m.i_[0][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][0][1] * m.i_[0][0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][0][1] * m.i_[0][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[2][0][0] = vmin; res[2][0][1] = vmax;
+
+ vmin = vmax = i_[2][1][0] * m.i_[1][0][0];
+ temp = i_[2][1][0] * m.i_[1][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][1][1] * m.i_[1][0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][1][1] * m.i_[1][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[2][0][0] += vmin; res[2][0][1] += vmax;
+
+ vmin = vmax = i_[2][2][0] * m.i_[2][0][0];
+ temp = i_[2][2][0] * m.i_[2][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][2][1] * m.i_[2][0][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][2][1] * m.i_[2][0][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[2][0][0] += vmin; res[2][0][1] += vmax;
+
+ // res[0][1]
+
+ vmin = vmax = i_[0][0][0] * m.i_[0][1][0];
+ temp = i_[0][0][0] * m.i_[0][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][0][1] * m.i_[0][1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][0][1] * m.i_[0][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[0][1][0] = vmin; res[0][1][1] = vmax;
+
+ vmin = vmax = i_[0][1][0] * m.i_[1][1][0];
+ temp = i_[0][1][0] * m.i_[1][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][1][1] * m.i_[1][1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][1][1] * m.i_[1][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[0][1][0] += vmin; res[0][1][1] += vmax;
+
+ vmin = vmax = i_[0][2][0] * m.i_[2][1][0];
+ temp = i_[0][2][0] * m.i_[2][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][2][1] * m.i_[2][1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][2][1] * m.i_[2][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[0][1][0] += vmin; res[0][1][1] += vmax;
+
+ // res[1][1]
+
+ vmin = vmax = i_[1][0][0] * m.i_[0][1][0];
+ temp = i_[1][0][0] * m.i_[0][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][0][1] * m.i_[0][1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][0][1] * m.i_[0][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[1][1][0] = vmin; res[1][1][1] = vmax;
+
+ vmin = vmax = i_[1][1][0] * m.i_[1][1][0];
+ temp = i_[1][1][0] * m.i_[1][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][1][1] * m.i_[1][1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][1][1] * m.i_[1][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[1][1][0] += vmin; res[1][1][1] += vmax;
+
+ vmin = vmax = i_[1][2][0] * m.i_[2][1][0];
+ temp = i_[1][2][0] * m.i_[2][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][2][1] * m.i_[2][1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][2][1] * m.i_[2][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[1][1][0] += vmin; res[1][1][1] += vmax;
+
+ // res[2][1]
+
+ vmin = vmax = i_[2][0][0] * m.i_[0][1][0];
+ temp = i_[2][0][0] * m.i_[0][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][0][1] * m.i_[0][1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][0][1] * m.i_[0][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[2][1][0] = vmin; res[2][1][1] = vmax;
+
+ vmin = vmax = i_[2][1][0] * m.i_[1][1][0];
+ temp = i_[2][1][0] * m.i_[1][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][1][1] * m.i_[1][1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][1][1] * m.i_[1][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[2][1][0] += vmin; res[2][1][1] += vmax;
+
+ vmin = vmax = i_[2][2][0] * m.i_[2][1][0];
+ temp = i_[2][2][0] * m.i_[2][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][2][1] * m.i_[2][1][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][2][1] * m.i_[2][1][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[2][1][0] += vmin; res[2][1][1] += vmax;
+
+ // res[0][2]
+
+ vmin = vmax = i_[0][0][0] * m.i_[0][2][0];
+ temp = i_[0][0][0] * m.i_[0][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][0][1] * m.i_[0][2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][0][1] * m.i_[0][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[0][2][0] = vmin; res[0][2][1] = vmax;
+
+ vmin = vmax = i_[0][1][0] * m.i_[1][2][0];
+ temp = i_[0][1][0] * m.i_[1][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][1][1] * m.i_[1][2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][1][1] * m.i_[1][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[0][2][0] += vmin; res[0][2][1] += vmax;
+
+ vmin = vmax = i_[0][2][0] * m.i_[2][2][0];
+ temp = i_[0][2][0] * m.i_[2][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][2][1] * m.i_[2][2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[0][2][1] * m.i_[2][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[0][2][0] += vmin; res[0][2][1] += vmax;
+
+ // res[1][2]
+
+ vmin = vmax = i_[1][0][0] * m.i_[0][2][0];
+ temp = i_[1][0][0] * m.i_[0][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][0][1] * m.i_[0][2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][0][1] * m.i_[0][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[1][2][0] = vmin; res[1][2][1] = vmax;
+
+ vmin = vmax = i_[1][1][0] * m.i_[1][2][0];
+ temp = i_[1][1][0] * m.i_[1][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][1][1] * m.i_[1][2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][1][1] * m.i_[1][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[1][2][0] += vmin; res[1][2][1] += vmax;
+
+ vmin = vmax = i_[1][2][0] * m.i_[2][2][0];
+ temp = i_[1][2][0] * m.i_[2][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][2][1] * m.i_[2][2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[1][2][1] * m.i_[2][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[1][2][0] += vmin; res[1][2][1] += vmax;
+
+ // res[2][2]
+
+ vmin = vmax = i_[2][0][0] * m.i_[0][2][0];
+ temp = i_[2][0][0] * m.i_[0][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][0][1] * m.i_[0][2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][0][1] * m.i_[0][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[2][2][0] = vmin; res[2][2][1] = vmax;
+
+ vmin = vmax = i_[2][1][0] * m.i_[1][2][0];
+ temp = i_[2][1][0] * m.i_[1][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][1][1] * m.i_[1][2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][1][1] * m.i_[1][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[2][2][0] += vmin; res[2][2][1] += vmax;
+
+ vmin = vmax = i_[2][2][0] * m.i_[2][2][0];
+ temp = i_[2][2][0] * m.i_[2][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][2][1] * m.i_[2][2][0]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ temp = i_[2][2][1] * m.i_[2][2][1]; if(temp < vmin) vmin = temp; else if(temp > vmax) vmax = temp;
+ res[2][2][0] += vmin; res[2][2][1] += vmax;
+
+ return IMatrix3(res);
+}
+
+IMatrix3 IMatrix3::operator + (const IMatrix3& m) const
+{
+ BVH_REAL res[3][3][2];
+ res[0][0][0] = i_[0][0][0] + m.i_[0][0][0]; res[0][0][1] = i_[0][0][1] + m.i_[0][0][1]; res[0][1][0] = i_[0][1][0] + m.i_[0][1][0]; res[0][1][1] = i_[0][1][1] + m.i_[0][1][1]; res[0][2][0] = i_[0][2][0] + m.i_[0][2][0]; res[0][2][1] = i_[0][2][1] + m.i_[0][2][1];
+ res[1][0][0] = i_[1][0][0] + m.i_[1][0][0]; res[1][0][1] = i_[1][0][1] + m.i_[1][0][1]; res[1][1][0] = i_[1][1][0] + m.i_[1][1][0]; res[1][1][1] = i_[1][1][1] + m.i_[1][1][1]; res[1][2][0] = i_[1][2][0] + m.i_[1][2][0]; res[1][2][1] = i_[1][2][1] + m.i_[1][2][1];
+ res[2][0][0] = i_[2][0][0] + m.i_[2][0][0]; res[2][0][1] = i_[2][0][1] + m.i_[2][0][1]; res[2][1][0] = i_[2][1][0] + m.i_[2][1][0]; res[2][1][1] = i_[2][1][1] + m.i_[2][1][1]; res[2][2][0] = i_[2][2][0] + m.i_[2][2][0]; res[2][2][1] = i_[2][2][1] + m.i_[2][2][1];
+
+ return IMatrix3(res);
+}
+
+IMatrix3& IMatrix3::operator += (const IMatrix3& m)
+{
+ i_[0][0][0] += m.i_[0][0][0]; i_[0][0][1] += m.i_[0][0][1]; i_[0][1][0] += m.i_[0][1][0]; i_[0][1][1] += m.i_[0][1][1]; i_[0][2][0] += m.i_[0][2][0]; i_[0][2][1] += m.i_[0][2][1];
+ i_[1][0][0] += m.i_[1][0][0]; i_[1][0][1] += m.i_[1][0][1]; i_[1][1][0] += m.i_[1][1][0]; i_[1][1][1] += m.i_[1][1][1]; i_[1][2][0] += m.i_[1][2][0]; i_[1][2][1] += m.i_[1][2][1];
+ i_[2][0][0] += m.i_[2][0][0]; i_[2][0][1] += m.i_[2][0][1]; i_[2][1][0] += m.i_[2][1][0]; i_[2][1][1] += m.i_[2][1][1]; i_[2][2][0] += m.i_[2][2][0]; i_[2][2][1] += m.i_[2][2][1];
+
+ return *this;
+}
+
+IVector3 IMatrix3::nonIntervalTimesVector(const Vec3f& v) const
+{
+ return IVector3(i_[0][0][0] * v[0] + i_[0][1][0] * v[1] + i_[0][2][0] * v[2],
+ i_[1][0][0] * v[0] + i_[1][1][0] * v[1] + i_[1][2][0] * v[2],
+ i_[2][0][0] * v[0] + i_[2][1][0] * v[1] + i_[2][2][0] * v[2]);
+}
+
+IVector3 IMatrix3::nonIntervalTimesVector(const IVector3& v) const
+{
+ return IVector3(i_[0][0][0] * v[0][0] + i_[0][1][0] * v[1][0] + i_[0][2][0] * v[2][0],
+ i_[1][0][0] * v[0][0] + i_[1][1][0] * v[1][0] + i_[1][2][0] * v[2][0],
+ i_[2][0][0] * v[0][0] + i_[2][1][0] * v[1][0] + i_[2][2][0] * v[2][0]);
+}
+
+void IMatrix3::print() const
+{
+ std::cout << "[" << i_[0][0][0] << "," << i_[0][0][1] << "]" << " [" << i_[0][1][0] << "," << i_[0][1][1] << "]" << " [" << i_[0][2][0] << "," << i_[0][2][1] << "]" << std::endl;
+ std::cout << "[" << i_[1][0][0] << "," << i_[1][0][1] << "]" << " [" << i_[1][1][0] << "," << i_[1][1][1] << "]" << " [" << i_[1][2][0] << "," << i_[1][2][1] << "]" << std::endl;
+ std::cout << "[" << i_[2][0][0] << "," << i_[2][0][1] << "]" << " [" << i_[2][1][0] << "," << i_[2][1][1] << "]" << " [" << i_[2][2][0] << "," << i_[2][2][1] << "]" << std::endl;
+}
+
+}
diff --git a/trunk/fcl/src/interval_tree.cpp b/trunk/fcl/src/interval_tree.cpp
index 5a106b50f646c301912056f2a59313859c2888d6..397cd16beeba6dc3884bdd851273bb269698203a 100644
--- a/trunk/fcl/src/interval_tree.cpp
+++ b/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);
diff --git a/trunk/fcl/src/interval_vector.cpp b/trunk/fcl/src/interval_vector.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5074850d33e4aa422decf5f5bca1239a749faffb
--- /dev/null
+++ b/trunk/fcl/src/interval_vector.cpp
@@ -0,0 +1,239 @@
+/*
+ * 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;
+}
+
+}
diff --git a/trunk/fcl/src/taylor_model.cpp b/trunk/fcl/src/taylor_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d512f5189fd8cfc438ca57073220f095fa71ebbf
--- /dev/null
+++ b/trunk/fcl/src/taylor_model.cpp
@@ -0,0 +1,439 @@
+/*
+ * 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;
+}
+
+}