From b5646a33148696c692c7a8eaae4a9c3d7108b4ac Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Mon, 16 Jul 2012 08:52:58 +0000
Subject: [PATCH] add octomap extension, still need test; fix bug in sse matrix
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@128 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/CMakeLists.txt | 11 +-
trunk/fcl/include/fcl/BV_node.h | 12 +-
trunk/fcl/include/fcl/broad_phase_collision.h | 4 +
trunk/fcl/include/fcl/ccd/interval.h | 40 +-
trunk/fcl/include/fcl/ccd/interval_matrix.h | 41 +-
trunk/fcl/include/fcl/ccd/interval_vector.h | 80 +-
trunk/fcl/include/fcl/ccd/taylor_matrix.h | 24 +-
trunk/fcl/include/fcl/ccd/taylor_model.h | 46 +-
trunk/fcl/include/fcl/ccd/taylor_vector.h | 18 +-
trunk/fcl/include/fcl/math_details.h | 328 ++++++++
trunk/fcl/include/fcl/math_simd_details.h | 517 +++++++++---
trunk/fcl/include/fcl/matrix_3f.h | 456 ++++++++---
trunk/fcl/include/fcl/octomap_extension.h | 68 ++
trunk/fcl/include/fcl/primitive.h | 8 +-
trunk/fcl/include/fcl/transform.h | 9 +-
trunk/fcl/include/fcl/vec_3f.h | 39 +-
trunk/fcl/manifest.xml | 2 +-
trunk/fcl/src/BV/OBB.cpp | 68 +-
trunk/fcl/src/BV/RSS.cpp | 282 +++----
trunk/fcl/src/BVH_utility.cpp | 137 +---
trunk/fcl/src/BV_fitter.cpp | 14 +-
trunk/fcl/src/ccd/interval.cpp | 116 +++
trunk/fcl/src/ccd/interval_matrix.cpp | 741 +++---------------
trunk/fcl/src/ccd/interval_vector.cpp | 94 +--
trunk/fcl/src/ccd/taylor_matrix.cpp | 377 +++------
trunk/fcl/src/ccd/taylor_model.cpp | 133 ++--
trunk/fcl/src/ccd/taylor_vector.cpp | 110 ++-
trunk/fcl/src/geometric_shapes_utility.cpp | 24 +-
trunk/fcl/src/intersect.cpp | 34 +-
trunk/fcl/src/matrix_3f.cpp | 240 ------
trunk/fcl/src/narrowphase/gjk.cpp | 2 +-
trunk/fcl/src/narrowphase/narrowphase.cpp | 71 +-
trunk/fcl/src/octomap_extension.cpp | 351 +++++++++
trunk/fcl/src/transform.cpp | 20 +-
trunk/fcl/src/traversal_recurse.cpp | 12 +-
35 files changed, 2552 insertions(+), 1977 deletions(-)
create mode 100644 trunk/fcl/include/fcl/octomap_extension.h
delete mode 100644 trunk/fcl/src/matrix_3f.cpp
create mode 100644 trunk/fcl/src/octomap_extension.cpp
diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index 02d18510..ef2e6599 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -12,7 +12,7 @@ set(CMAKE_BUILD_TYPE Release)
set(FCL_VERSION "0.1.1")
set(PKG_DESC "Fast Collision Library")
-set(PKG_EXTERNAL_DEPS "ccd flann")
+set(PKG_EXTERNAL_DEPS "ccd flann octomap")
set(CMAKE_SKIP_BUILD_RPATH FALSE)
set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
@@ -35,11 +35,16 @@ pkg_check_modules(CCD REQUIRED ccd)
include_directories(${CCD_INCLUDE_DIRS})
link_directories(${CCD_LIBRARY_DIRS})
+pkg_check_modules(OCTOMAP REQUIRED octomap)
+include_directories(${OCTOMAP_INCLUDE_DIRS})
+link_directories(${OCTOMAP_LIBRARY_DIRS})
+
add_definitions(-DUSE_SVMLIGHT=0)
-add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.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/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/ccd/interval.cpp src/ccd/interval_vector.cpp src/ccd/interval_matrix.cpp src/ccd/taylor_model.cpp src/ccd/taylor_vector.cpp src/ccd/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/narrowphase/gjk.cpp src/narrowphase/gjk_libccd.cpp src/narrowphase/narrowphase.cpp src/hierarchy_tree.cpp)
+add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.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/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/ccd/interval.cpp src/ccd/interval_vector.cpp src/ccd/interval_matrix.cpp src/ccd/taylor_model.cpp src/ccd/taylor_vector.cpp src/ccd/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/narrowphase/gjk.cpp src/narrowphase/gjk_libccd.cpp src/narrowphase/narrowphase.cpp src/hierarchy_tree.cpp src/octomap_extension.cpp)
+
-target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES})
set(pkg_conf_file "${CMAKE_CURRENT_SOURCE_DIR}/fcl.pc")
configure_file("${pkg_conf_file}.in" "${pkg_conf_file}" @ONLY)
diff --git a/trunk/fcl/include/fcl/BV_node.h b/trunk/fcl/include/fcl/BV_node.h
index 1341a623..28ab9fe4 100644
--- a/trunk/fcl/include/fcl/BV_node.h
+++ b/trunk/fcl/include/fcl/BV_node.h
@@ -130,9 +130,9 @@ struct BVNode<OBB> : public BVNodeBase
}
void setOrientation(const Matrix3f& m)
{
- bv.axis[0].setValue(m[0][0], m[1][0], m[2][0]);
- bv.axis[1].setValue(m[0][1], m[1][1], m[2][1]);
- bv.axis[2].setValue(m[0][2], m[1][2], m[2][2]);
+ bv.axis[0].setValue(m(0, 0), m(1, 0), m(2, 0));
+ bv.axis[1].setValue(m(0, 1), m(1, 1), m(2, 1));
+ bv.axis[2].setValue(m(0, 2), m(1, 2), m(2, 2));
}
};
@@ -165,9 +165,9 @@ struct BVNode<RSS> : public BVNodeBase
void setOrientation(const Matrix3f& m)
{
- bv.axis[0].setValue(m[0][0], m[1][0], m[2][0]);
- bv.axis[1].setValue(m[0][1], m[1][1], m[2][1]);
- bv.axis[2].setValue(m[0][2], m[1][2], m[2][2]);
+ bv.axis[0].setValue(m(0, 0), m(1, 0), m(2, 0));
+ bv.axis[1].setValue(m(0, 1), m(1, 1), m(2, 1));
+ bv.axis[2].setValue(m(0, 2), m(1, 2), m(2, 2));
}
};
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index e1e05634..1402f5d1 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -1390,6 +1390,8 @@ public:
return dtree.size();
}
+ const HierarchyTree<AABB>& getTree() const { return dtree; }
+
private:
HierarchyTree<AABB> dtree;
@@ -1533,6 +1535,8 @@ public:
}
+ const alternative::HierarchyTree<AABB>& getTree() const { return dtree; }
+
private:
alternative::HierarchyTree<AABB> dtree;
boost::unordered_map<CollisionObject*, size_t> table;
diff --git a/trunk/fcl/include/fcl/ccd/interval.h b/trunk/fcl/include/fcl/ccd/interval.h
index fe6a3b02..6a4b697f 100644
--- a/trunk/fcl/include/fcl/ccd/interval.h
+++ b/trunk/fcl/include/fcl/ccd/interval.h
@@ -48,8 +48,8 @@ struct Interval
{
FCL_REAL i_[2];
- Interval() {}
- Interval(FCL_REAL v)
+ Interval() { i_[0] = i_[1] = 0; }
+ explicit Interval(FCL_REAL v)
{
i_[0] = i_[1] = v;
}
@@ -112,6 +112,8 @@ struct Interval
Interval operator * (const Interval& other) const;
+ Interval& operator *= (const Interval& other);
+
inline Interval operator * (FCL_REAL d) const
{
if(d >= 0) return Interval(i_[0] * d, i_[1] * d);
@@ -138,6 +140,8 @@ struct Interval
/** \brief other must not contain 0 */
Interval operator / (const Interval& other) const;
+ Interval& operator /= (const Interval& other);
+
/** \brief determine whether the intersection between intervals is empty */
inline bool overlap(const Interval& other) const
{
@@ -146,13 +150,13 @@ struct Interval
return true;
}
- inline void intersect(const Interval& other)
+ inline bool intersect(const Interval& other)
{
- if(i_[1] < other.i_[0]) return;
- if(i_[0] > other.i_[1]) return;
+ if(i_[1] < other.i_[0]) return false;
+ if(i_[0] > other.i_[1]) return false;
if(i_[1] > other.i_[1]) i_[1] = other.i_[1];
if(i_[0] < other.i_[0]) i_[0] = other.i_[0];
- return;
+ return true;
}
inline Interval operator - () const
@@ -184,39 +188,31 @@ struct Interval
}
/** \brief Compute the minimum interval contains v and original interval */
- inline void bound(FCL_REAL v)
+ inline Interval& bound(FCL_REAL v)
{
if(v < i_[0]) i_[0] = v;
if(v > i_[1]) i_[1] = v;
+ return *this;
}
- inline Interval bounded(FCL_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)
+ inline Interval& 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];
+ return *this;
}
- 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 FCL_REAL center() const { return 0.5 * (i_[0] + i_[1]); }
inline FCL_REAL diameter() const { return i_[1] -i_[0]; }
};
+Interval bound(const Interval& i, FCL_REAL v);
+
+Interval bound(const Interval& i, const Interval& other);
+
}
#endif
diff --git a/trunk/fcl/include/fcl/ccd/interval_matrix.h b/trunk/fcl/include/fcl/ccd/interval_matrix.h
index 1c0a01a5..657616cb 100644
--- a/trunk/fcl/include/fcl/ccd/interval_matrix.h
+++ b/trunk/fcl/include/fcl/ccd/interval_matrix.h
@@ -47,7 +47,7 @@ namespace fcl
struct IMatrix3
{
- Interval i_[3][3];
+ IVector3 v_[3];
IMatrix3();
IMatrix3(FCL_REAL v);
@@ -55,31 +55,48 @@ struct IMatrix3
IMatrix3(FCL_REAL m[3][3][2]);
IMatrix3(FCL_REAL m[3][3]);
IMatrix3(Interval m[3][3]);
+ IMatrix3(const IVector3& v1, const IVector3& v2, const IVector3& v3);
void setIdentity();
IVector3 getColumn(size_t i) const;
- IVector3 getRow(size_t i) const;
+ const IVector3& getRow(size_t i) const;
- Vec3f getRealColumn(size_t i) const;
- Vec3f getRealRow(size_t i) const;
+ Vec3f getColumnLow(size_t i) const;
+ Vec3f getRowLow(size_t i) const;
- const Interval& operator () (size_t i, size_t j) const;
- Interval& operator () (size_t i, size_t j);
+ Vec3f getColumnHigh(size_t i) const;
+ Vec3f getRowHigh(size_t i) const;
+
+ Matrix3f getLow() const;
+ Matrix3f getHigh() const;
+
+ inline const IVector3& operator [] (size_t i) const
+ {
+ return v_[i];
+ }
+
+ inline IVector3& operator [] (size_t i)
+ {
+ return v_[i];
+ }
+
+ IMatrix3 operator + (const IMatrix3& m) const;
+ IMatrix3& operator += (const IMatrix3& m);
+
+ IMatrix3 operator - (const IMatrix3& m) const;
+ IMatrix3& operator -= (const IMatrix3& m);
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& operator *= (const IMatrix3& m);
+ IMatrix3& operator *= (const Matrix3f& m);
- IMatrix3 nonIntervalAddMatrix(const IMatrix3& m) const;
- IVector3 nonIntervalTimesVector(const IVector3& v) const;
- IVector3 nonIntervalTimesVector(const Vec3f& v) const;
+ void print() const;
};
}
diff --git a/trunk/fcl/include/fcl/ccd/interval_vector.h b/trunk/fcl/include/fcl/ccd/interval_vector.h
index c66911fe..8f2f7074 100644
--- a/trunk/fcl/include/fcl/ccd/interval_vector.h
+++ b/trunk/fcl/include/fcl/ccd/interval_vector.h
@@ -57,14 +57,74 @@ struct IVector3
IVector3(const Interval& v1, const Interval& v2, const Interval& v3);
IVector3(const Vec3f& v);
+ inline void setValue(FCL_REAL v)
+ {
+ i_[0].setValue(v);
+ i_[1].setValue(v);
+ i_[2].setValue(v);
+ }
+
+ inline void setValue(FCL_REAL x, FCL_REAL y, FCL_REAL z)
+ {
+ i_[0].setValue(x);
+ i_[1].setValue(y);
+ i_[2].setValue(z);
+ }
+
+ inline void setValue(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu)
+ {
+ i_[0].setValue(xl, xu);
+ i_[1].setValue(yl, yu);
+ i_[2].setValue(zl, zu);
+ }
+
+ inline void setValue(FCL_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]);
+ }
+
+ inline void setValue(Interval v[3])
+ {
+ i_[0] = v[0];
+ i_[1] = v[1];
+ i_[2] = v[2];
+ }
+
+ inline void setValue(const Interval& v1, const Interval& v2, const Interval& v3)
+ {
+ i_[0] = v1;
+ i_[1] = v2;
+ i_[2] = v3;
+ }
+
+ inline void setValue(const Vec3f& v)
+ {
+ i_[0].setValue(v[0]);
+ i_[1].setValue(v[1]);
+ i_[2].setValue(v[2]);
+ }
+
+ inline void setValue(FCL_REAL v[3])
+ {
+ i_[0].setValue(v[0]);
+ i_[1].setValue(v[1]);
+ i_[2].setValue(v[2]);
+ }
+
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;
+ Interval dot(const Vec3f& other) const;
+ IVector3 cross(const Vec3f& other) const;
+
inline const Interval& operator [] (size_t i) const
{
return i_[i];
@@ -75,6 +135,16 @@ struct IVector3
return i_[i];
}
+ inline Vec3f getLow() const
+ {
+ return Vec3f(i_[0][0], i_[1][0], i_[2][0]);
+ }
+
+ inline Vec3f getHigh() const
+ {
+ return Vec3f(i_[0][1], i_[1][1], i_[2][1]);
+ }
+
void print() const;
Vec3f center() const;
FCL_REAL volumn() const;
@@ -83,14 +153,14 @@ struct IVector3
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();
};
+IVector3 bound(const IVector3& i, const Vec3f& v);
+
+IVector3 bound(const IVector3& i, const IVector3& v);
+
}
#endif
diff --git a/trunk/fcl/include/fcl/ccd/taylor_matrix.h b/trunk/fcl/include/fcl/ccd/taylor_matrix.h
index 2f4f560c..f3e2ddd8 100644
--- a/trunk/fcl/include/fcl/ccd/taylor_matrix.h
+++ b/trunk/fcl/include/fcl/ccd/taylor_matrix.h
@@ -47,31 +47,45 @@ namespace fcl
struct TMatrix3
{
- TaylorModel i_[3][3];
+ TVector3 v_[3];
TMatrix3();
+ TMatrix3(const boost::shared_ptr<TimeInterval>& time_interval);
TMatrix3(TaylorModel m[3][3]);
- TMatrix3(const Matrix3f& m);
+ TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3);
+ TMatrix3(const Matrix3f& m, const boost::shared_ptr<TimeInterval>& time_interval);
TVector3 getColumn(size_t i) const;
- TVector3 getRow(size_t i) const;
+ const TVector3& getRow(size_t i) const;
- const TaylorModel& operator () (size_t i, size_t j) const;
- TaylorModel& operator () (size_t i, size_t j);
+ const TVector3& operator [] (size_t i) const;
+ TVector3& operator [] (size_t i);
TVector3 operator * (const Vec3f& v) const;
TVector3 operator * (const TVector3& v) const;
TMatrix3 operator * (const Matrix3f& m) const;
TMatrix3 operator * (const TMatrix3& m) const;
TMatrix3 operator * (const TaylorModel& d) const;
+ TMatrix3 operator * (FCL_REAL d) const;
+
+ TMatrix3& operator *= (const Matrix3f& m);
+ TMatrix3& operator *= (const TMatrix3& m);
+ TMatrix3& operator *= (const TaylorModel& d);
+ TMatrix3& operator *= (FCL_REAL d);
+
TMatrix3 operator + (const TMatrix3& m) const;
TMatrix3& operator += (const TMatrix3& m);
+ TMatrix3 operator - (const TMatrix3& m) const;
+ TMatrix3& operator -= (const TMatrix3& m);
+
IMatrix3 getBound() const;
void print() const;
void setIdentity();
void setZero();
FCL_REAL diameter() const;
+
+ void setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval);
};
}
diff --git a/trunk/fcl/include/fcl/ccd/taylor_model.h b/trunk/fcl/include/fcl/ccd/taylor_model.h
index ff29711f..3249067f 100644
--- a/trunk/fcl/include/fcl/ccd/taylor_model.h
+++ b/trunk/fcl/include/fcl/ccd/taylor_model.h
@@ -38,16 +38,31 @@
#define FCL_TAYLOR_MODEL_H
#include "fcl/ccd/interval.h"
+#include <boost/shared_ptr.hpp>
namespace fcl
{
+struct TimeInterval
+{
+ /** \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
+};
+
/** \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 time interval */
+ boost::shared_ptr<TimeInterval> time_interval_;
+
/** \brief Coefficients of the cubic polynomial approximation */
FCL_REAL coeffs_[4];
@@ -55,18 +70,31 @@ struct TaylorModel
Interval r_;
void setTimeInterval(FCL_REAL l, FCL_REAL r);
+ void setTimeInterval(const boost::shared_ptr<TimeInterval> time_interval)
+ {
+ time_interval_ = time_interval;
+ }
TaylorModel();
- TaylorModel(FCL_REAL coeff);
- TaylorModel(FCL_REAL coeffs[3], const Interval& r);
- TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r);
+ TaylorModel(const boost::shared_ptr<TimeInterval>& time_interval);
+ TaylorModel(FCL_REAL coeff, const boost::shared_ptr<TimeInterval>& time_interval);
+ TaylorModel(FCL_REAL coeffs[3], const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval);
+ TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval);
TaylorModel operator + (const TaylorModel& other) const;
- TaylorModel operator - (const TaylorModel& other) const;
TaylorModel& operator += (const TaylorModel& other);
+
+ TaylorModel operator - (const TaylorModel& other) const;
TaylorModel& operator -= (const TaylorModel& other);
+
+ TaylorModel operator + (FCL_REAL d) const;
+ TaylorModel& operator += (FCL_REAL d);
+
TaylorModel operator * (const TaylorModel& other) const;
TaylorModel operator * (FCL_REAL d) const;
+ TaylorModel& operator *= (const TaylorModel& other);
+ TaylorModel& operator *= (FCL_REAL d);
+
TaylorModel operator - () const;
void print() const;
@@ -80,16 +108,6 @@ struct TaylorModel
Interval getBound(FCL_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 FCL_REAL PI_;
};
void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0);
diff --git a/trunk/fcl/include/fcl/ccd/taylor_vector.h b/trunk/fcl/include/fcl/ccd/taylor_vector.h
index 5ba4a8d2..8771feec 100644
--- a/trunk/fcl/include/fcl/ccd/taylor_vector.h
+++ b/trunk/fcl/include/fcl/ccd/taylor_vector.h
@@ -48,22 +48,32 @@ struct TVector3
TaylorModel i_[3];
TVector3();
+ TVector3(const boost::shared_ptr<TimeInterval>& time_interval);
TVector3(TaylorModel v[3]);
TVector3(const TaylorModel& v0, const TaylorModel& v1, const TaylorModel& v2);
- TVector3(const Vec3f& v);
+ TVector3(const Vec3f& v, const boost::shared_ptr<TimeInterval>& time_interval);
TVector3 operator + (const TVector3& other) const;
- TVector3 operator + (FCL_REAL d) const;
TVector3& operator += (const TVector3& other);
+
+ TVector3 operator + (FCL_REAL d) const;
+ TVector3& operator += (FCL_REAL d);
+
TVector3 operator - (const TVector3& other) const;
TVector3& operator -= (const TVector3& other);
- TVector3& operator = (const Vec3f& u);
+
+ TVector3 operator * (const TaylorModel& d) const;
+ TVector3& operator *= (const TaylorModel& d);
+ TVector3 operator * (FCL_REAL d) const;
+ TVector3& operator *= (FCL_REAL d);
const TaylorModel& operator [] (size_t i) const;
TaylorModel& operator [] (size_t i);
TaylorModel dot(const TVector3& other) const;
TVector3 cross(const TVector3& other) const;
+ TaylorModel dot(const Vec3f& other) const;
+ TVector3 cross(const Vec3f& other) const;
IVector3 getBound() const;
IVector3 getBound(FCL_REAL t) const;
@@ -73,6 +83,8 @@ struct TVector3
void setZero();
TaylorModel squareLength() const;
+
+ void setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval);
};
void generateTVector3ForLinearFunc(TVector3& v, const Vec3f& position, const Vec3f& velocity);
diff --git a/trunk/fcl/include/fcl/math_details.h b/trunk/fcl/include/fcl/math_details.h
index 5726d369..8dde27b7 100644
--- a/trunk/fcl/include/fcl/math_details.h
+++ b/trunk/fcl/include/fcl/math_details.h
@@ -167,6 +167,334 @@ static inline bool equal(const Vec3Data<T>& x, const Vec3Data<T>& y, T epsilon)
(x.vs[2] - y.vs[2] > -epsilon));
}
+
+template<typename T>
+struct Matrix3Data
+{
+ typedef T meta_type;
+ typedef Vec3Data<T> vector_type;
+
+ Vec3Data<T> rs[3];
+ Matrix3Data() {};
+
+ Matrix3Data(T xx, T xy, T xz,
+ T yx, T yy, T yz,
+ T zx, T zy, T zz)
+ {
+ setValue(xx, xy, xz,
+ yx, yy, yz,
+ zx, zy, zz);
+ }
+
+ Matrix3Data(const Vec3Data<T>& v1, const Vec3Data<T>& v2, const Vec3Data<T>& v3)
+ {
+ rs[0] = v1;
+ rs[1] = v2;
+ rs[2] = v3;
+ }
+
+ Matrix3Data(const Matrix3Data<T>& other)
+ {
+ rs[0] = other.rs[0];
+ rs[1] = other.rs[1];
+ rs[2] = other.rs[2];
+ }
+
+ inline Vec3Data<T> getColumn(size_t i) const
+ {
+ return Vec3Data<T>(rs[0][i], rs[1][i], rs[2][i]);
+ }
+
+ inline const Vec3Data<T>& getRow(size_t i) const
+ {
+ return rs[i];
+ }
+
+ inline T operator() (size_t i, size_t j) const
+ {
+ return rs[i][j];
+ }
+
+ inline T& operator() (size_t i, size_t j)
+ {
+ return rs[i][j];
+ }
+
+ inline Vec3Data<T> operator * (const Vec3Data<T>& v) const
+ {
+ return Vec3Data<T>(dot_prod3(rs[0], v), dot_prod3(rs[1], v), dot_prod3(rs[2], v));
+ }
+
+ inline Matrix3Data<T> operator * (const Matrix3Data<T>& other) const
+ {
+ return Matrix3Data<T>(other.transposeDotX(rs[0]), other.transposeDotY(rs[0]), other.transposeDotZ(rs[0]),
+ other.transposeDotX(rs[1]), other.transposeDotY(rs[1]), other.transposeDotZ(rs[1]),
+ other.transposeDotX(rs[2]), other.transposeDotY(rs[2]), other.transposeDotZ(rs[2]));
+ }
+
+ inline Matrix3Data<T> operator + (const Matrix3Data<T>& other) const
+ {
+ return Matrix3Data<T>(rs[0] + other.rs[0], rs[1] + other.rs[1], rs[2] + other.rs[2]);
+ }
+
+ inline Matrix3Data<T> operator - (const Matrix3Data<T>& other) const
+ {
+ return Matrix3Data<T>(rs[0] - other.rs[0], rs[1] - other.rs[1], rs[2] - other.rs[2]);
+ }
+
+ inline Matrix3Data<T> operator + (T c) const
+ {
+ return Matrix3Data<T>(rs[0] + c, rs[1] + c, rs[2] + c);
+ }
+
+ inline Matrix3Data<T> operator - (T c) const
+ {
+ return Matrix3Data<T>(rs[0] - c, rs[1] - c, rs[2] - c);
+ }
+
+ inline Matrix3Data<T> operator * (T c) const
+ {
+ return Matrix3Data<T>(rs[0] * c, rs[1] * c, rs[2] * c);
+ }
+
+ inline Matrix3Data<T> operator / (T c) const
+ {
+ return Matrix3Data<T>(rs[0] / c, rs[1] / c, rs[2] / c);
+ }
+
+ inline Matrix3Data<T>& operator *= (const Matrix3Data<T>& other)
+ {
+ rs[0].setValue(other.transposeDotX(rs[0]), other.transposeDotY(rs[0]), other.transposeDotZ(rs[0]));
+ rs[1].setValue(other.transposeDotX(rs[1]), other.transposeDotY(rs[1]), other.transposeDotZ(rs[1]));
+ rs[2].setValue(other.transposeDotX(rs[2]), other.transposeDotY(rs[2]), other.transposeDotZ(rs[2]));
+ return *this;
+ }
+
+ inline Matrix3Data<T>& operator += (const Matrix3Data<T>& other)
+ {
+ rs[0] += other.rs[0];
+ rs[1] += other.rs[1];
+ rs[2] += other.rs[2];
+ return *this;
+ }
+
+ inline Matrix3Data<T>& operator -= (const Matrix3Data<T>& other)
+ {
+ rs[0] -= other.rs[0];
+ rs[1] -= other.rs[1];
+ rs[2] -= other.rs[2];
+ return *this;
+ }
+
+ inline Matrix3Data<T>& operator += (T c)
+ {
+ rs[0] += c;
+ rs[1] += c;
+ rs[2] += c;
+ return *this;
+ }
+
+ inline Matrix3Data<T>& operator - (T c)
+ {
+ rs[0] -= c;
+ rs[1] -= c;
+ rs[2] -= c;
+ return *this;
+ }
+
+ inline Matrix3Data<T>& operator * (T c)
+ {
+ rs[0] *= c;
+ rs[1] *= c;
+ rs[2] *= c;
+ return *this;
+ }
+
+ inline Matrix3Data<T>& operator / (T c)
+ {
+ rs[0] /= c;
+ rs[1] /= c;
+ rs[2] /= c;
+ return *this;
+ }
+
+
+ void setIdentity()
+ {
+ setValue((T)1, (T)0, (T)0,
+ (T)0, (T)1, (T)0,
+ (T)0, (T)0, (T)1);
+ }
+
+ void setZero()
+ {
+ setValue((T)0);
+ }
+
+ static const Matrix3Data<T>& getIdentity()
+ {
+ static const Matrix3Data<T> I((T)1, (T)0, (T)0,
+ (T)0, (T)1, (T)0,
+ (T)0, (T)0, (T)1);
+ return I;
+ }
+
+ T determinant() const
+ {
+ return dot_prod3(rs[0], cross_prod(rs[1], rs[2]));
+ }
+
+ Matrix3Data<T>& transpose()
+ {
+ register T tmp = rs[0][1];
+ rs[0][1] = rs[1][0];
+ rs[1][0] = tmp;
+
+ tmp = rs[0][2];
+ rs[0][2] = rs[2][0];
+ rs[2][0] = tmp;
+
+ tmp = rs[2][1];
+ rs[2][1] = rs[1][2];
+ rs[1][2] = tmp;
+ return *this;
+ }
+
+ Matrix3Data<T>& inverse()
+ {
+ T det = determinant();
+ register T inrsdet = 1 / det;
+
+ setValue((rs[1][1] * rs[2][2] - rs[1][2] * rs[2][1]) * inrsdet,
+ (rs[0][2] * rs[2][1] - rs[0][1] * rs[2][2]) * inrsdet,
+ (rs[0][1] * rs[1][2] - rs[0][2] * rs[1][1]) * inrsdet,
+ (rs[1][2] * rs[2][0] - rs[1][0] * rs[2][2]) * inrsdet,
+ (rs[0][0] * rs[2][2] - rs[0][2] * rs[2][0]) * inrsdet,
+ (rs[0][2] * rs[1][0] - rs[0][0] * rs[1][2]) * inrsdet,
+ (rs[1][0] * rs[2][1] - rs[1][1] * rs[2][0]) * inrsdet,
+ (rs[0][1] * rs[2][0] - rs[0][0] * rs[2][1]) * inrsdet,
+ (rs[0][0] * rs[1][1] - rs[0][1] * rs[1][0]) * inrsdet);
+
+ return *this;
+ }
+
+ Matrix3Data<T> transposeTimes(const Matrix3Data<T>& m) const
+ {
+ return Matrix3Data<T>(rs[0][0] * m.rs[0][0] + rs[1][0] * m.rs[1][0] + rs[2][0] * m.rs[2][0],
+ rs[0][0] * m.rs[0][1] + rs[1][0] * m.rs[1][1] + rs[2][0] * m.rs[2][1],
+ rs[0][0] * m.rs[0][2] + rs[1][0] * m.rs[1][2] + rs[2][0] * m.rs[2][2],
+ rs[0][1] * m.rs[0][0] + rs[1][1] * m.rs[1][0] + rs[2][1] * m.rs[2][0],
+ rs[0][1] * m.rs[0][1] + rs[1][1] * m.rs[1][1] + rs[2][1] * m.rs[2][1],
+ rs[0][1] * m.rs[0][2] + rs[1][1] * m.rs[1][2] + rs[2][1] * m.rs[2][2],
+ rs[0][2] * m.rs[0][0] + rs[1][2] * m.rs[1][0] + rs[2][2] * m.rs[2][0],
+ rs[0][2] * m.rs[0][1] + rs[1][2] * m.rs[1][1] + rs[2][2] * m.rs[2][1],
+ rs[0][2] * m.rs[0][2] + rs[1][2] * m.rs[1][2] + rs[2][2] * m.rs[2][2]);
+ }
+
+ Matrix3Data<T> timesTranspose(const Matrix3Data<T>& m) const
+ {
+ return Matrix3Data<T>(dot_prod3(rs[0], m[0]), dot_prod3(rs[0], m[1]), dot_prod3(rs[0], m[2]),
+ dot_prod3(rs[1], m[0]), dot_prod3(rs[1], m[1]), dot_prod3(rs[1], m[2]),
+ dot_prod3(rs[2], m[0]), dot_prod3(rs[2], m[1]), dot_prod3(rs[2], m[2]));
+ }
+
+
+ Vec3Data<T> transposeTimes(const Vec3Data<T>& v) const
+ {
+ return Vec3Data<T>(transposeDotX(v), transposeDotY(v), transposeDotZ(v));
+ }
+
+ inline T transposeDotX(const Vec3Data<T>& v) const
+ {
+ return rs[0][0] * v[0] + rs[1][0] * v[1] + rs[2][0] * v[2];
+ }
+
+ inline T transposeDotY(const Vec3Data<T>& v) const
+ {
+ return rs[0][1] * v[0] + rs[1][1] * v[1] + rs[2][1] * v[2];
+ }
+
+ inline T transposeDotZ(const Vec3Data<T>& v) const
+ {
+ return rs[0][2] * v[0] + rs[1][2] * v[1] + rs[2][2] * v[2];
+ }
+
+ inline T transposeDot(size_t i, const Vec3Data<T>& v) const
+ {
+ return rs[0][i] * v[0] + rs[1][i] * v[1] + rs[2][i] * v[2];
+ }
+
+ inline T dotX(const Vec3Data<T>& v) const
+ {
+ return rs[0][0] * v[0] + rs[0][1] * v[1] + rs[0][2] * v[2];
+ }
+
+ inline T dotY(const Vec3Data<T>& v) const
+ {
+ return rs[1][0] * v[0] + rs[1][1] * v[1] + rs[1][2] * v[2];
+ }
+
+ inline T dotZ(const Vec3Data<T>& v) const
+ {
+ return rs[2][0] * v[0] + rs[2][1] * v[1] + rs[2][2] * v[2];
+ }
+
+ inline T dot(size_t i, const Vec3Data<T>& v) const
+ {
+ return rs[i][0] * v[0] + rs[i][1] * v[1] + rs[i][2] * v[2];
+ }
+
+ inline void setValue(T xx, T xy, T xz,
+ T yx, T yy, T yz,
+ T zx, T zy, T zz)
+ {
+ rs[0].setValue(xx, xy, xz);
+ rs[1].setValue(yx, yy, yz);
+ rs[2].setValue(zx, zy, zz);
+ }
+
+ inline void setValue(T x)
+ {
+ rs[0].setValue(x);
+ rs[1].setValue(x);
+ rs[2].setValue(x);
+ }
+};
+
+
+
+template<typename T>
+Matrix3Data<T> abs(const Matrix3Data<T>& m)
+{
+ return Matrix3Data<T>(abs(m.rs[0]), abs(m.rs[1]), abs(m.rs[2]));
+}
+
+template<typename T>
+Matrix3Data<T> transpose(const Matrix3Data<T>& m)
+{
+ return Matrix3Data<T>(m.rs[0][0], m.rs[1][0], m.rs[2][0],
+ m.rs[0][1], m.rs[1][1], m.rs[2][1],
+ m.rs[0][2], m.rs[1][2], m.rs[2][2]);
+}
+
+
+template<typename T>
+Matrix3Data<T> inverse(const Matrix3Data<T>& m)
+{
+ T det = m.determinant();
+ T inrsdet = 1 / det;
+
+ return Matrix3Data<T>((m.rs[1][1] * m.rs[2][2] - m.rs[1][2] * m.rs[2][1]) * inrsdet,
+ (m.rs[0][2] * m.rs[2][1] - m.rs[0][1] * m.rs[2][2]) * inrsdet,
+ (m.rs[0][1] * m.rs[1][2] - m.rs[0][2] * m.rs[1][1]) * inrsdet,
+ (m.rs[1][2] * m.rs[2][0] - m.rs[1][0] * m.rs[2][2]) * inrsdet,
+ (m.rs[0][0] * m.rs[2][2] - m.rs[0][2] * m.rs[2][0]) * inrsdet,
+ (m.rs[0][2] * m.rs[1][0] - m.rs[0][0] * m.rs[1][2]) * inrsdet,
+ (m.rs[1][0] * m.rs[2][1] - m.rs[1][1] * m.rs[2][0]) * inrsdet,
+ (m.rs[0][1] * m.rs[2][0] - m.rs[0][0] * m.rs[2][1]) * inrsdet,
+ (m.rs[0][0] * m.rs[1][1] - m.rs[0][1] * m.rs[1][0]) * inrsdet);
+}
+
}
}
diff --git a/trunk/fcl/include/fcl/math_simd_details.h b/trunk/fcl/include/fcl/math_simd_details.h
index 37ea501d..404f9381 100644
--- a/trunk/fcl/include/fcl/math_simd_details.h
+++ b/trunk/fcl/include/fcl/math_simd_details.h
@@ -63,6 +63,15 @@ static inline __m128 vec_sel(__m128 a, __m128 b, __m128 mask)
{
return _mm_or_ps(_mm_and_ps(mask, b), _mm_andnot_ps(mask, a));
}
+static inline __m128 vec_sel(__m128 a, __m128 b, const unsigned int* mask)
+{
+ return vec_sel(a, b, _mm_load_ps((float*)mask));
+}
+
+static inline __m128 vec_sel(__m128 a, __m128 b, unsigned int mask)
+{
+ return vec_sel(a, b, _mm_set1_ps(*(float*)&mask));
+}
static inline __m128 vec_splat(__m128 a, int e)
{
@@ -444,15 +453,48 @@ static inline sse_meta_f4 normalize3_approx(const sse_meta_f4& x)
}
+static inline void transpose(__m128 c0, __m128 c1, __m128 c2, __m128* r0, __m128* r1, __m128* r2)
+{
+ static const union { unsigned int i[4]; __m128 m; } selectmask __attribute__ ((aligned(16))) = {{0, 0xffffffff, 0, 0}};
+ register __m128 t0, t1;
+ t0 = _mm_unpacklo_ps(c0, c2);
+ t1 = _mm_unpackhi_ps(c0, c2);
+ *r0 = _mm_unpacklo_ps(t0, c1);
+ *r1 = _mm_shuffle_ps(t0, t0, _MM_SHUFFLE(0, 3, 2, 2));
+ *r1 = vec_sel(*r1, c1, selectmask.i);
+ *r2 = _mm_shuffle_ps(t1, t1, _MM_SHUFFLE(0, 1, 1, 0));
+ *r2 = vec_sel(*r2, vec_splat(c1, 2), selectmask.i);
+}
+
+
+static inline void inverse(__m128 c0, __m128 c1, __m128 c2, __m128* i0, __m128* i1, __m128* i2)
+{
+ __m128 t0, t1, t2, d, invd;
+ t2 = cross_prod(c0, c1);
+ t0 = cross_prod(c1, c2);
+ t1 = cross_prod(c2, c0);
+ d = dot_prod3(t2, c2);
+ d = vec_splat(d, 0);
+ invd = _mm_rcp_ps(d); // approximate inverse
+ transpose(t0, t1, t2, i0, i1, i2);
+ *i0 = _mm_mul_ps(*i0, invd);
+ *i1 = _mm_mul_ps(*i1, invd);
+ *i2 = _mm_mul_ps(*i2, invd);
+}
+
+
struct sse_meta_f12
{
typedef float meta_type;
+ typedef sse_meta_f4 vector_type;
sse_meta_f4 c[3];
+ sse_meta_f12() { setZero(); }
+
sse_meta_f12(float xx, float xy, float xz,
float yx, float yy, float yz,
float zx, float zy, float zz)
- { setValue(xx, xy, xz, yz, yy, yz, zx, zy, zz); }
+ { setValue(xx, xy, xz, yx, yy, yz, zx, zy, zz); }
sse_meta_f12(const sse_meta_f4& x, const sse_meta_f4& y, const sse_meta_f4& z)
{ setColumn(x, y, z); }
@@ -469,6 +511,20 @@ struct sse_meta_f12
c[2].setValue(xz, yz, zz, 0);
}
+ inline void setIdentity()
+ {
+ c[0].setValue(1, 0, 0, 0);
+ c[1].setValue(0, 1, 0, 0);
+ c[2].setValue(0, 0, 1, 0);
+ }
+
+ inline void setZero()
+ {
+ c[0].setValue(0);
+ c[1].setValue(0);
+ c[2].setValue(0);
+ }
+
inline void setColumn(const sse_meta_f4& x, const sse_meta_f4& y, const sse_meta_f4& z)
{
c[0] = x; c[1] = y; c[2] = z;
@@ -514,6 +570,16 @@ struct sse_meta_f12
return sse_meta_f12((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2]);
}
+ inline sse_meta_f12 operator + (const sse_meta_f12& mat) const
+ {
+ return sse_meta_f12(c[0] + mat.c[0], c[1] + mat.c[1], c[2] + mat.c[2]);
+ }
+
+ inline sse_meta_f12 operator - (const sse_meta_f12& mat) const
+ {
+ return sse_meta_f12(c[0] - mat.c[0], c[1] - mat.c[1], c[2] - mat.c[2]);
+ }
+
inline sse_meta_f12 operator + (float t_) const
{
sse_meta_f4 t(t_);
@@ -538,6 +604,27 @@ struct sse_meta_f12
return sse_meta_f12(c[0] / t, c[1] / t, c[2] / t);
}
+ inline sse_meta_f12& operator *= (const sse_meta_f12& mat)
+ {
+ setColumn((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2]);
+ return *this;
+ }
+
+ inline sse_meta_f12& operator += (const sse_meta_f12& mat)
+ {
+ c[0] += mat.c[0];
+ c[1] += mat.c[1];
+ c[2] += mat.c[2];
+ return *this;
+ }
+
+ inline sse_meta_f12& operator -= (const sse_meta_f12& mat)
+ {
+ c[0] -= mat.c[0];
+ c[1] -= mat.c[1];
+ c[2] -= mat.c[2];
+ return *this;
+ }
inline sse_meta_f12& operator += (float t_)
{
@@ -575,20 +662,68 @@ struct sse_meta_f12
return *this;
}
-};
+ inline sse_meta_f12& inverse()
+ {
+ __m128 inv0, inv1, inv2;
+ details::inverse(c[0].v, c[1].v, c[2].v, &inv0, &inv1, &inv2);
+ setColumn(inv0, inv1, inv2);
+ return *this;
+ }
+ inline sse_meta_f12& transpose()
+ {
+ __m128 r0, r1, r2;
+ details::transpose(c[0].v, c[1].v, c[2].v, &r0, &r1, &r2);
+ setColumn(r0, r1, r2);
+ return *this;
+ }
-static inline void transpose(__m128 c0, __m128 c1, __m128 c2, __m128* r0, __m128* r1, __m128* r2)
+ inline sse_meta_f12& abs()
+ {
+ c[0] = details::abs(c[0]);
+ c[1] = details::abs(c[1]);
+ c[2] = details::abs(c[2]);
+ return *this;
+ }
+
+ inline float determinant() const
+ {
+ return _mm_cvtss_f32(dot_prod3(c[2].v, cross_prod(c[0].v, c[1].v)));
+ }
+
+ inline sse_meta_f12 transposeTimes(const sse_meta_f12& other) const
+ {
+ return sse_meta_f12(dot_prod3(c[0], other.c[0]), dot_prod3(c[0], other.c[1]), dot_prod3(c[0], other.c[2]),
+ dot_prod3(c[1], other.c[0]), dot_prod3(c[1], other.c[1]), dot_prod3(c[1], other.c[2]),
+ dot_prod3(c[2], other.c[0]), dot_prod3(c[2], other.c[1]), dot_prod3(c[2], other.c[2]));
+ }
+
+ inline sse_meta_f12 timesTranspose(const sse_meta_f12& m) const
+ {
+ sse_meta_f12 tmp(m);
+ return (*this) * tmp.transpose();
+ }
+
+ inline sse_meta_f4 transposeTimes(const sse_meta_f4& v) const
+ {
+ return sse_meta_f4(dot_prod3(c[0], v), dot_prod3(c[1], v), dot_prod3(c[2], v));
+ }
+
+ inline float transposeDot(size_t i, const sse_meta_f4& v) const
+ {
+ return dot_prod3(c[i], v);
+ }
+
+ inline float dot(size_t i, const sse_meta_f4& v) const
+ {
+ return v[0] * c[0][i] + v[1] * c[1][i] + v[2] * c[2][i];
+ }
+
+};
+
+static inline sse_meta_f12 abs(const sse_meta_f12& mat)
{
- static const union { int i[4]; __m128 m; } selectmask __attribute__ ((aligned(16))) = {{0, 0xffffffff, 0, 0}};
- register __m128 t0, t1;
- t0 = _mm_unpackhi_ps(c0, c2);
- t1 = _mm_unpacklo_ps(c0, c2);
- *r0 = _mm_unpackhi_ps(t0, c1);
- *r1 = _mm_shuffle_ps(t0, t0, _MM_SHUFFLE(0, 3, 2, 2));
- *r1 = vec_sel(*r1, c1, selectmask.m);
- *r2 = _mm_shuffle_ps(t1, t1, _MM_SHUFFLE(0, 1, 1, 0));
- *r2 = vec_sel(*r2, vec_splat(c1, 1), selectmask.m);
+ return sse_meta_f12(abs(mat.getColumn(0)), abs(mat.getColumn(1)), abs(mat.getColumn(2)));
}
static inline sse_meta_f12 transpose(const sse_meta_f12& mat)
@@ -598,20 +733,6 @@ static inline sse_meta_f12 transpose(const sse_meta_f12& mat)
return sse_meta_f12(r0, r1, r2);
}
-static inline void inverse(__m128 c0, __m128 c1, __m128 c2, __m128* i0, __m128* i1, __m128* i2)
-{
- __m128 t0, t1, t2, d, invd;
- t2 = cross_prod(c0, c1);
- t0 = cross_prod(c1, c2);
- t1 = cross_prod(c2, c0);
- d = dot_prod3(t2, c2);
- d = _mm_shuffle_ps(d, d, _MM_SHUFFLE(0, 0, 0, 0));
- invd = _mm_rcp_ps(d); // approximate inverse
- transpose(t0, t1, t2, i0, i1, i2);
- *i0 = _mm_mul_ps(*i0, invd);
- *i1 = _mm_mul_ps(*i1, invd);
- *i2 = _mm_mul_ps(*i2, invd);
-}
static inline sse_meta_f12 inverse(const sse_meta_f12& mat)
{
@@ -620,16 +741,114 @@ static inline sse_meta_f12 inverse(const sse_meta_f12& mat)
return sse_meta_f12(inv0, inv1, inv2);
}
-static inline float determinant(const sse_meta_f12& mat)
+
+static inline void transpose(__m128 c0, __m128 c1, __m128 c2, __m128 c3,
+ __m128* r0, __m128* r1, __m128* r2, __m128* r3)
{
- return _mm_cvtss_f32(dot_prod3(mat.getColumn(2).v, cross_prod(mat.getColumn(0).v, mat.getColumn(1).v)));
+ __m128 tmp0 = _mm_unpacklo_ps(c0, c2);
+ __m128 tmp1 = _mm_unpacklo_ps(c1, c3);
+ __m128 tmp2 = _mm_unpackhi_ps(c0, c2);
+ __m128 tmp3 = _mm_unpackhi_ps(c1, c3);
+ *r0 = _mm_unpacklo_ps(tmp0, tmp1);
+ *r1 = _mm_unpackhi_ps(tmp0, tmp1);
+ *r2 = _mm_unpacklo_ps(tmp2, tmp3);
+ *r3 = _mm_unpackhi_ps(tmp2, tmp3);
+}
+
+
+static inline void inverse(__m128 c0, __m128 c1, __m128 c2, __m128 c3,
+ __m128* res0, __m128* res1, __m128* res2, __m128* res3)
+{
+ __m128 Va, Vb, Vc;
+ __m128 r1, r2, r3, tt, tt2;
+ __m128 sum, Det, RDet;
+ __m128 trns0, trns1, trns2, trns3;
+
+ // Calculating the minterms for the first line.
+
+ tt = c3; tt2 = _mm_ror_ps(c2,1);
+ Vc = _mm_mul_ps(tt2,_mm_ror_ps(tt,0)); // V3'\B7V4
+ Va = _mm_mul_ps(tt2,_mm_ror_ps(tt,2)); // V3'\B7V4"
+ Vb = _mm_mul_ps(tt2,_mm_ror_ps(tt,3)); // V3'\B7V4^
+
+ r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2)); // V3"\B7V4^ - V3^\B7V4"
+ r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0)); // V3^\B7V4' - V3'\B7V4^
+ r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1)); // V3'\B7V4" - V3"\B7V4'
+
+ tt = c1;
+ Va = _mm_ror_ps(tt,1); sum = _mm_mul_ps(Va,r1);
+ Vb = _mm_ror_ps(tt,2); sum = _mm_add_ps(sum,_mm_mul_ps(Vb,r2));
+ Vc = _mm_ror_ps(tt,3); sum = _mm_add_ps(sum,_mm_mul_ps(Vc,r3));
+
+ // Calculating the determinant.
+ Det = _mm_mul_ps(sum,c0);
+ Det = _mm_add_ps(Det,_mm_movehl_ps(Det,Det));
+
+ static const union { int i[4]; __m128 m; } Sign_PNPN __attribute__ ((aligned(16))) = {{0x00000000, 0x80000000, 0x00000000, 0x80000000}};
+ static const union { int i[4]; __m128 m; } Sign_NPNP __attribute__ ((aligned(16))) = {{0x80000000, 0x00000000, 0x80000000, 0x00000000}};
+ static const union { float i[4]; __m128 m; } ZERONE __attribute__ ((aligned(16))) = {{1.0f, 0.0f, 0.0f, 1.0f}};
+
+ __m128 mtL1 = _mm_xor_ps(sum,Sign_PNPN.m);
+
+ // Calculating the minterms of the second line (using previous results).
+ tt = _mm_ror_ps(c0,1); sum = _mm_mul_ps(tt,r1);
+ tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
+ tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
+ __m128 mtL2 = _mm_xor_ps(sum,Sign_NPNP.m);
+
+ // Testing the determinant.
+ Det = _mm_sub_ss(Det,_mm_shuffle_ps(Det,Det,1));
+
+ // Calculating the minterms of the third line.
+ tt = _mm_ror_ps(c0,1);
+ Va = _mm_mul_ps(tt,Vb); // V1'\B7V2"
+ Vb = _mm_mul_ps(tt,Vc); // V1'\B7V2^
+ Vc = _mm_mul_ps(tt,c1); // V1'\B7V2
+
+ r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2)); // V1"\B7V2^ - V1^\B7V2"
+ r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0)); // V1^\B7V2' - V1'\B7V2^
+ r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1)); // V1'\B7V2" - V1"\B7V2'
+
+ tt = _mm_ror_ps(c3,1); sum = _mm_mul_ps(tt,r1);
+ tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
+ tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
+ __m128 mtL3 = _mm_xor_ps(sum,Sign_PNPN.m);
+
+ // Dividing is FASTER than rcp_nr! (Because rcp_nr causes many register-memory RWs).
+ RDet = _mm_div_ss(ZERONE.m, Det); // TODO: just 1.0f?
+ RDet = _mm_shuffle_ps(RDet,RDet,0x00);
+
+ // Devide the first 12 minterms with the determinant.
+ mtL1 = _mm_mul_ps(mtL1, RDet);
+ mtL2 = _mm_mul_ps(mtL2, RDet);
+ mtL3 = _mm_mul_ps(mtL3, RDet);
+
+ // Calculate the minterms of the forth line and devide by the determinant.
+ tt = _mm_ror_ps(c2,1); sum = _mm_mul_ps(tt,r1);
+ tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
+ tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
+ __m128 mtL4 = _mm_xor_ps(sum,Sign_NPNP.m);
+ mtL4 = _mm_mul_ps(mtL4, RDet);
+
+ // Now we just have to transpose the minterms matrix.
+ trns0 = _mm_unpacklo_ps(mtL1,mtL2);
+ trns1 = _mm_unpacklo_ps(mtL3,mtL4);
+ trns2 = _mm_unpackhi_ps(mtL1,mtL2);
+ trns3 = _mm_unpackhi_ps(mtL3,mtL4);
+ *res0 = _mm_movelh_ps(trns0,trns1);
+ *res1 = _mm_movehl_ps(trns1,trns0);
+ *res2 = _mm_movelh_ps(trns2,trns3);
+ *res3 = _mm_movehl_ps(trns3,trns2);
}
struct sse_meta_f16
{
typedef float meta_type;
+ typedef sse_meta_f4 vector_type;
sse_meta_f4 c[4];
+
+ sse_meta_f16() { setZero(); }
sse_meta_f16(float xx, float xy, float xz, float xw,
float yx, float yy, float yz, float yw,
@@ -648,7 +867,7 @@ struct sse_meta_f16
float zx, float zy, float zz, float zw,
float wx, float wy, float wz, float ww)
{
- c[0].setValue(xx, yz, zx, wx);
+ c[0].setValue(xx, yx, zx, wx);
c[1].setValue(xy, yy, zy, wy);
c[2].setValue(xz, yz, zz, wz);
c[3].setValue(xw, yw, zw, ww);
@@ -664,6 +883,22 @@ struct sse_meta_f16
c[0].setValue(x); c[1].setValue(y); c[2].setValue(z); c[3].setValue(w);
}
+ inline void setIdentity()
+ {
+ c[0].setValue(1, 0, 0, 0);
+ c[1].setValue(0, 1, 0, 0);
+ c[2].setValue(0, 0, 1, 0);
+ c[3].setValue(0, 0, 0, 1);
+ }
+
+ inline void setZero()
+ {
+ c[0].setValue(0);
+ c[1].setValue(0);
+ c[2].setValue(0);
+ c[3].setValue(0);
+ }
+
inline const sse_meta_f4& getColumn(size_t i) const
{
return c[i];
@@ -701,6 +936,17 @@ struct sse_meta_f16
return sse_meta_f16((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2], (*this) * mat.c[3]);
}
+
+ inline sse_meta_f16 operator + (const sse_meta_f16& mat) const
+ {
+ return sse_meta_f16(c[0] + mat.c[0], c[1] + mat.c[1], c[2] + mat.c[2], c[3] + mat.c[3]);
+ }
+
+ inline sse_meta_f16 operator - (const sse_meta_f16& mat) const
+ {
+ return sse_meta_f16(c[0] - mat.c[0], c[1] - mat.c[1], c[2] - mat.c[2], c[3] - mat.c[3]);
+ }
+
inline sse_meta_f16 operator + (float t_) const
{
sse_meta_f4 t(t_);
@@ -725,6 +971,29 @@ struct sse_meta_f16
return sse_meta_f16(c[0] / t, c[1] / t, c[2] / t, c[3] / t);
}
+ inline sse_meta_f16& operator *= (const sse_meta_f16& mat)
+ {
+ setColumn((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2], (*this) * mat.c[3]);
+ return *this;
+ }
+
+ inline sse_meta_f16& operator += (const sse_meta_f16& mat)
+ {
+ c[0] += mat.c[0];
+ c[1] += mat.c[1];
+ c[2] += mat.c[2];
+ c[3] += mat.c[3];
+ return *this;
+ }
+
+ inline sse_meta_f16& operator -= (const sse_meta_f16& mat)
+ {
+ c[0] -= mat.c[0];
+ c[1] -= mat.c[1];
+ c[2] -= mat.c[2];
+ c[3] -= mat.c[3];
+ return *this;
+ }
inline sse_meta_f16& operator += (float t_)
{
@@ -766,107 +1035,121 @@ struct sse_meta_f16
return *this;
}
+ inline sse_meta_f16& abs()
+ {
+ c[0] = details::abs(c[0]);
+ c[1] = details::abs(c[1]);
+ c[2] = details::abs(c[2]);
+ c[3] = details::abs(c[3]);
+ return *this;
+ }
-};
-
-sse_meta_f16 transpose(const sse_meta_f16& mat)
-{
- __m128 tmp0 = _mm_unpackhi_ps(mat.getColumn(0).v, mat.getColumn(2).v);
- __m128 tmp1 = _mm_unpackhi_ps(mat.getColumn(1).v, mat.getColumn(3).v);
- __m128 tmp2 = _mm_unpacklo_ps(mat.getColumn(0).v, mat.getColumn(2).v);
- __m128 tmp3 = _mm_unpacklo_ps(mat.getColumn(1).v, mat.getColumn(3).v);
- return sse_meta_f16(_mm_unpackhi_ps(tmp0, tmp1), _mm_unpacklo_ps(tmp0, tmp1), _mm_unpackhi_ps(tmp2, tmp3), _mm_unpacklo_ps(tmp2, tmp3));
-}
-
-
-sse_meta_f16 inverse(const sse_meta_f16& mat)
-{
- __m128 Va, Vb, Vc;
- __m128 r1, r2, r3, tt, tt2;
- __m128 sum, Det, RDet;
- __m128 trns0, trns1, trns2, trns3;
-
- __m128 _L1 = mat.getColumn(0).v;
- __m128 _L2 = mat.getColumn(1).v;
- __m128 _L3 = mat.getColumn(2).v;
- __m128 _L4 = mat.getColumn(3).v;
- // Calculating the minterms for the first line.
-
- tt = _L4; tt2 = _mm_ror_ps(_L3,1);
- Vc = _mm_mul_ps(tt2,_mm_ror_ps(tt,0)); // V3'\B7V4
- Va = _mm_mul_ps(tt2,_mm_ror_ps(tt,2)); // V3'\B7V4"
- Vb = _mm_mul_ps(tt2,_mm_ror_ps(tt,3)); // V3'\B7V4^
-
- r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2)); // V3"\B7V4^ - V3^\B7V4"
- r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0)); // V3^\B7V4' - V3'\B7V4^
- r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1)); // V3'\B7V4" - V3"\B7V4'
-
- tt = _L2;
- Va = _mm_ror_ps(tt,1); sum = _mm_mul_ps(Va,r1);
- Vb = _mm_ror_ps(tt,2); sum = _mm_add_ps(sum,_mm_mul_ps(Vb,r2));
- Vc = _mm_ror_ps(tt,3); sum = _mm_add_ps(sum,_mm_mul_ps(Vc,r3));
-
- // Calculating the determinant.
- Det = _mm_mul_ps(sum,_L1);
- Det = _mm_add_ps(Det,_mm_movehl_ps(Det,Det));
+ inline sse_meta_f16& inverse()
+ {
+ __m128 r0, r1, r2, r3;
+ details::inverse(c[0].v, c[1].v, c[2].v, c[3].v, &r0, &r1, &r2, &r3);
+ setColumn(r0, r1, r2, r3);
+ return *this;
+ }
- static const union { int i[4]; __m128 m; } Sign_PNPN __attribute__ ((aligned(16))) = {{0x00000000, 0x80000000, 0x00000000, 0x80000000}};
- static const union { int i[4]; __m128 m; } Sign_NPNP __attribute__ ((aligned(16))) = {{0x80000000, 0x00000000, 0x80000000, 0x00000000}};
- static const union { float i[4]; __m128 m; } ZERONE __attribute__ ((aligned(16))) = {{1.0f, 0.0f, 0.0f, 1.0f}};
+ inline sse_meta_f16& transpose()
+ {
+ __m128 r0, r1, r2, r3;
+ details::transpose(c[0].v, c[1].v, c[2].v, c[3].v, &r0, &r1, &r2, &r3);
+ setColumn(r0, r1, r2, r3);
+ return *this;
+ }
- __m128 mtL1 = _mm_xor_ps(sum,Sign_PNPN.m);
+ inline float determinant() const
+ {
+ __m128 Va, Vb, Vc;
+ __m128 r1, r2, r3, tt, tt2;
+ __m128 sum, Det;
+
+ __m128 _L1 = c[0].v;
+ __m128 _L2 = c[1].v;
+ __m128 _L3 = c[2].v;
+ __m128 _L4 = c[3].v;
+ // Calculating the minterms for the first line.
+
+ // _mm_ror_ps is just a macro using _mm_shuffle_ps().
+ tt = _L4; tt2 = _mm_ror_ps(_L3,1);
+ Vc = _mm_mul_ps(tt2,_mm_ror_ps(tt,0)); // V3'·V4
+ Va = _mm_mul_ps(tt2,_mm_ror_ps(tt,2)); // V3'·V4"
+ Vb = _mm_mul_ps(tt2,_mm_ror_ps(tt,3)); // V3'·V4^
+
+ r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2)); // V3"·V4^ - V3^·V4"
+ r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0)); // V3^·V4' - V3'·V4^
+ r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1)); // V3'·V4" - V3"·V4'
+
+ tt = _L2;
+ Va = _mm_ror_ps(tt,1); sum = _mm_mul_ps(Va,r1);
+ Vb = _mm_ror_ps(tt,2); sum = _mm_add_ps(sum,_mm_mul_ps(Vb,r2));
+ Vc = _mm_ror_ps(tt,3); sum = _mm_add_ps(sum,_mm_mul_ps(Vc,r3));
+
+ // Calculating the determinant.
+ Det = _mm_mul_ps(sum,_L1);
+ Det = _mm_add_ps(Det,_mm_movehl_ps(Det,Det));
+
+ // Calculating the minterms of the second line (using previous results).
+ tt = _mm_ror_ps(_L1,1); sum = _mm_mul_ps(tt,r1);
+ tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
+ tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
+
+ // Testing the determinant.
+ Det = _mm_sub_ss(Det,_mm_shuffle_ps(Det,Det,1));
+ return _mm_cvtss_f32(Det);
+ }
- // Calculating the minterms of the second line (using previous results).
- tt = _mm_ror_ps(_L1,1); sum = _mm_mul_ps(tt,r1);
- tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
- tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
- __m128 mtL2 = _mm_xor_ps(sum,Sign_NPNP.m);
+ inline sse_meta_f16 transposeTimes(const sse_meta_f16& other) const
+ {
+ return sse_meta_f16(dot_prod4(c[0], other.c[0]), dot_prod4(c[0], other.c[1]), dot_prod4(c[0], other.c[2]), dot_prod4(c[0], other.c[3]),
+ dot_prod4(c[1], other.c[0]), dot_prod4(c[1], other.c[1]), dot_prod4(c[1], other.c[2]), dot_prod4(c[1], other.c[3]),
+ dot_prod4(c[2], other.c[0]), dot_prod4(c[2], other.c[1]), dot_prod4(c[2], other.c[2]), dot_prod4(c[2], other.c[3]),
+ dot_prod4(c[3], other.c[0]), dot_prod4(c[3], other.c[1]), dot_prod4(c[3], other.c[2]), dot_prod4(c[3], other.c[3]));
+ }
- // Testing the determinant.
- Det = _mm_sub_ss(Det,_mm_shuffle_ps(Det,Det,1));
+ inline sse_meta_f16 timesTranspose(const sse_meta_f16& m) const
+ {
+ sse_meta_f16 tmp(m);
+ return (*this) * tmp.transpose();
+ }
- // Calculating the minterms of the third line.
- tt = _mm_ror_ps(_L1,1);
- Va = _mm_mul_ps(tt,Vb); // V1'\B7V2"
- Vb = _mm_mul_ps(tt,Vc); // V1'\B7V2^
- Vc = _mm_mul_ps(tt,_L2); // V1'\B7V2
+ inline sse_meta_f4 transposeTimes(const sse_meta_f4& v) const
+ {
+ return sse_meta_f4(dot_prod4(c[0], v), dot_prod4(c[1], v), dot_prod4(c[2], v), dot_prod4(c[3], v));
+ }
- r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2)); // V1"\B7V2^ - V1^\B7V2"
- r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0)); // V1^\B7V2' - V1'\B7V2^
- r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1)); // V1'\B7V2" - V1"\B7V2'
+ inline float transposeDot(size_t i, const sse_meta_f4& v) const
+ {
+ return dot_prod4(c[i], v);
+ }
- tt = _mm_ror_ps(_L4,1); sum = _mm_mul_ps(tt,r1);
- tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
- tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
- __m128 mtL3 = _mm_xor_ps(sum,Sign_PNPN.m);
+ inline float dot(size_t i, const sse_meta_f4& v) const
+ {
+ return v[0] * c[0][i] + v[1] * c[1][i] + v[2] * c[2][i] + v[3] * c[3][i];
+ }
- // Dividing is FASTER than rcp_nr! (Because rcp_nr causes many register-memory RWs).
- RDet = _mm_div_ss(ZERONE.m, Det); // TODO: just 1.0f?
- RDet = _mm_shuffle_ps(RDet,RDet,0x00);
+};
- // Devide the first 12 minterms with the determinant.
- mtL1 = _mm_mul_ps(mtL1, RDet);
- mtL2 = _mm_mul_ps(mtL2, RDet);
- mtL3 = _mm_mul_ps(mtL3, RDet);
+static inline sse_meta_f16 abs(const sse_meta_f16& mat)
+{
+ return sse_meta_f16(abs(mat.getColumn(0)), abs(mat.getColumn(1)), abs(mat.getColumn(2)), abs(mat.getColumn(3)));
+}
- // Calculate the minterms of the forth line and devide by the determinant.
- tt = _mm_ror_ps(_L3,1); sum = _mm_mul_ps(tt,r1);
- tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
- tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
- __m128 mtL4 = _mm_xor_ps(sum,Sign_NPNP.m);
- mtL4 = _mm_mul_ps(mtL4, RDet);
+static inline sse_meta_f16 transpose(const sse_meta_f16& mat)
+{
+ __m128 r0, r1, r2, r3;
+ transpose(mat.getColumn(0).v, mat.getColumn(1).v, mat.getColumn(2).v, mat.getColumn(3).v, &r0, &r1, &r2, &r3);
+ return sse_meta_f16(r0, r1, r2, r3);
+}
- // Now we just have to transpose the minterms matrix.
- trns0 = _mm_unpacklo_ps(mtL1,mtL2);
- trns1 = _mm_unpacklo_ps(mtL3,mtL4);
- trns2 = _mm_unpackhi_ps(mtL1,mtL2);
- trns3 = _mm_unpackhi_ps(mtL3,mtL4);
- _L1 = _mm_movelh_ps(trns0,trns1);
- _L2 = _mm_movehl_ps(trns1,trns0);
- _L3 = _mm_movelh_ps(trns2,trns3);
- _L4 = _mm_movehl_ps(trns3,trns2);
- return sse_meta_f16(_L1, _L2, _L3, _L4);
+static inline sse_meta_f16 inverse(const sse_meta_f16& mat)
+{
+ __m128 r0, r1, r2, r3;
+ inverse(mat.getColumn(0).v, mat.getColumn(1).v, mat.getColumn(2).v, mat.getColumn(3).v, &r0, &r1, &r2, &r3);
+ return sse_meta_f16(r0, r1, r2, r3);
}
diff --git a/trunk/fcl/include/fcl/matrix_3f.h b/trunk/fcl/include/fcl/matrix_3f.h
index 4e6e3be2..e42af05c 100644
--- a/trunk/fcl/include/fcl/matrix_3f.h
+++ b/trunk/fcl/include/fcl/matrix_3f.h
@@ -41,162 +41,376 @@
namespace fcl
{
- class Matrix3f
+
+
+template<typename T>
+class Matrix3fX
+{
+public:
+ typedef typename T::meta_type U;
+ typedef typename T::vector_type S;
+ T data;
+
+ Matrix3fX() {}
+ Matrix3fX(U xx, U xy, U xz,
+ U yx, U yy, U yz,
+ U zx, U zy, U zz) : data(xx, xy, xz, yx, yy, yz, zx, zy, zz)
+ {}
+
+ Matrix3fX(const Vec3fX<S>& v1, const Vec3fX<S>& v2, const Vec3fX<S>& v3)
+ : data(v1.data, v2.data, v3.data) {}
+
+ Matrix3fX(const Matrix3fX<T>& other) : data(other.data) {}
+
+ Matrix3fX(const T& data_) : data(data_) {}
+
+ inline Vec3fX<S> getColumn(size_t i) const
{
- public:
- Vec3f v_[3];
+ return Vec3fX<S>(data.getColumn(i));
+ }
- /** \brief All zero matrix */
- Matrix3f() {}
+ inline Vec3fX<S> getRow(size_t i) const
+ {
+ return Vec3fX<S>(data.getRow(i));
+ }
- Matrix3f(FCL_REAL xx, FCL_REAL xy, FCL_REAL xz,
- FCL_REAL yx, FCL_REAL yy, FCL_REAL yz,
- FCL_REAL zx, FCL_REAL zy, FCL_REAL zz)
- {
- setValue(xx, xy, xz,
- yx, yy, yz,
- zx, zy, zz);
- }
+ inline U operator () (size_t i, size_t j) const
+ {
+ return data(i, j);
+ }
- Matrix3f(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3)
- {
- v_[0] = v1;
- v_[1] = v2;
- v_[2] = v3;
- }
+ inline U& operator () (size_t i, size_t j)
+ {
+ return data(i, j);
+ }
- Matrix3f(const Matrix3f& other)
- {
- v_[0] = other.v_[0];
- v_[1] = other.v_[1];
- v_[2] = other.v_[2];
- }
+ inline Vec3fX<S> operator * (const Vec3fX<S>& v) const
+ {
+ return Vec3fX<S>(data * v.data);
+ }
- Matrix3f& operator = (const Matrix3f& other)
- {
- v_[0] = other.v_[0];
- v_[1] = other.v_[1];
- v_[2] = other.v_[2];
- return *this;
- }
+ inline Matrix3fX<T> operator * (const Matrix3fX<T>& m) const
+ {
+ return Matrix3fX<T>(data * m.data);
+ }
- inline Vec3f getColumn(size_t i) const
- {
- return Vec3f(v_[0][i], v_[1][i], v_[2][i]);
- }
+ inline Matrix3fX<T> operator + (const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data + other.data);
+ }
- inline const Vec3f& getRow(size_t i) const
- {
- return v_[i];
- }
+ inline Matrix3fX<T> operator - (const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data - other.data);
+ }
- inline Vec3f& operator [](size_t i)
- {
- return v_[i];
- }
+ inline Matrix3fX<T> operator + (U c) const
+ {
+ return Matrix3fX<T>(data + c);
+ }
- inline const Vec3f& operator [](size_t i) const
- {
- return v_[i];
- }
+ inline Matrix3fX<T> operator - (U c) const
+ {
+ return Matrix3fX<T>(data - c);
+ }
- inline FCL_REAL operator () (size_t i, size_t j) const
- {
- return v_[i][j];
- }
+ inline Matrix3fX<T> operator * (U c) const
+ {
+ return Matrix3fX<T>(data * c);
+ }
- inline FCL_REAL& operator() (size_t i, size_t j)
- {
- return v_[i][j];
- }
+ inline Matrix3fX<T> operator / (U c) const
+ {
+ return Matrix3fX<T>(data / c);
+ }
- Matrix3f& operator *= (const Matrix3f& other);
+ inline Matrix3fX<T>& operator *= (const Matrix3fX<T>& other)
+ {
+ data *= other.data;
+ return *this;
+ }
- Matrix3f& operator += (FCL_REAL c);
+ inline Matrix3fX<T>& operator += (const Matrix3fX<T>& other)
+ {
+ data += other.data;
+ return *this;
+ }
- void setIdentity()
- {
- setValue((FCL_REAL)1.0, (FCL_REAL)0.0, (FCL_REAL)0.0,
- (FCL_REAL)0.0, (FCL_REAL)1.0, (FCL_REAL)0.0,
- (FCL_REAL)0.0, (FCL_REAL)0.0, (FCL_REAL)1.0);
- }
+ inline Matrix3fX<T>& operator -= (const Matrix3fX<T>& other)
+ {
+ data -= other.data;
+ return *this;
+ }
- void setZero()
- {
- setValue((FCL_REAL)0.0);
- }
+ inline Matrix3fX<T>& operator += (U c)
+ {
+ data += c;
+ return *this;
+ }
- static const Matrix3f& getIdentity()
- {
- static const Matrix3f I((FCL_REAL)1.0, (FCL_REAL)0.0, (FCL_REAL)0.0,
- (FCL_REAL)0.0, (FCL_REAL)1.0, (FCL_REAL)0.0,
- (FCL_REAL)0.0, (FCL_REAL)0.0, (FCL_REAL)1.0);
- return I;
- }
+ inline Matrix3fX<T>& operator -= (U c)
+ {
+ data -= c;
+ return *this;
+ }
- FCL_REAL determinant() const;
- Matrix3f transpose() const;
- Matrix3f inverse() const;
+ inline Matrix3fX<T>& operator *= (U c)
+ {
+ data *= c;
+ return *this;
+ }
- Matrix3f transposeTimes(const Matrix3f& m) const;
- Matrix3f timesTranspose(const Matrix3f& m) const;
- Matrix3f operator * (const Matrix3f& m) const;
+ inline Matrix3fX<T>& operator /= (U c)
+ {
+ data /= c;
+ return *this;
+ }
- Vec3f operator * (const Vec3f& v) const;
- Vec3f transposeTimes(const Vec3f& v) const;
+ inline void setIdentity()
+ {
+ data.setIdentity();
+ }
- inline FCL_REAL quadraticForm(const Vec3f& v) const
- {
- return v[0] * v[0] * v_[0][0] + v[0] * v[1] * v_[0][1] + v[0] * v[2] * v_[0][2] +
- v[1] * v[0] * v_[1][0] + v[1] * v[1] * v_[1][1] + v[1] * v[2] * v_[1][2] +
- v[2] * v[0] * v_[2][0] + v[2] * v[1] * v_[2][1] + v[2] * v[2] * v_[2][2];
- }
+ inline void setZero()
+ {
+ data.setZero();
+ }
- /** S * M * S' */
- Matrix3f tensorTransform(const Matrix3f& m) const;
+ inline U determinant() const
+ {
+ return data.determinant();
+ }
- inline FCL_REAL transposeDotX(const Vec3f& v) const
- {
- return v_[0][0] * v[0] + v_[1][0] * v[1] + v_[2][0] * v[2];
- }
+ Matrix3fX<T>& transpose()
+ {
+ data.transpose();
+ return *this;
+ }
- inline FCL_REAL transposeDotY(const Vec3f& v) const
- {
- return v_[0][1] * v[0] + v_[1][1] * v[1] + v_[2][1] * v[2];
- }
+ Matrix3fX<T>& inverse()
+ {
+ data.inverse();
+ return *this;
+ }
- inline FCL_REAL transposeDotZ(const Vec3f& v) const
- {
- return v_[0][2] * v[0] + v_[1][2] * v[1] + v_[2][2] * v[2];
- }
+ Matrix3fX<T>& abs()
+ {
+ data.abs();
+ return *this;
+ }
+
+ static const Matrix3fX<T>& getIdentity()
+ {
+ static const Matrix3fX<T> I((U)1, (U)0, (U)0,
+ (U)0, (U)1, (U)0,
+ (U)0, (U)0, (U)1);
+ return I;
+ }
+
+ Matrix3fX<T> transposeTimes(const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data.transposeTimes(other.data));
+ }
+
+ Matrix3fX<T> timesTranspose(const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data.timesTranspose(other.data));
+ }
+
+ Vec3fX<S> transposeTimes(const Vec3fX<S>& v) const
+ {
+ return Vec3fX<S>(data.transposeTimes(v.data));
+ }
+
+ Matrix3fX<T> tensorTransform(const Matrix3fX<T>& m) const
+ {
+ Matrix3fX<T> res(*this);
+ res *= m;
+ return res.timesTranspose(*this);
+ }
+
+ inline U transposeDotX(const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(0, v.data);
+ }
+
+ inline U transposeDotY(const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(1, v.data);
+ }
+
+ inline U transposeDotZ(const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(2, v.data);
+ }
+
+ inline U transposeDot(size_t i, const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(i, v.data);
+ }
+
+ inline U dotX(const Vec3fX<S>& v) const
+ {
+ return data.dot(0, v.data);
+ }
+
+ inline U dotY(const Vec3fX<S>& v) const
+ {
+ return data.dot(1, v.data);
+ }
+
+ inline U dotZ(const Vec3fX<S>& v) const
+ {
+ return data.dot(2, v.data);
+ }
+
+ inline U dot(size_t i, const Vec3fX<S>& v) const
+ {
+ return data.dot(i, v.data);
+ }
+
+ inline void setValue(U xx, U xy, U xz,
+ U yx, U yy, U yz,
+ U zx, U zy, U zz)
+ {
+ data.setValue(xx, xy, xz,
+ yx, yy, yz,
+ zx, zy, zz);
+ }
+
+ inline void setValue(U x)
+ {
+ data.setValue(x);
+ }
+};
+
+template<typename T>
+void relativeTransform(const Matrix3fX<T>& R1, const Vec3fX<typename T::vector_type>& t1,
+ const Matrix3fX<T>& R2, const Vec3fX<typename T::vector_type>& t2,
+ Matrix3fX<T>& R, Vec3fX<typename T::vector_type>& t)
+{
+ R = R1.transposeTimes(R2);
+ t = R1.transposeTimes(t2 - t1);
+}
+
+template<typename T>
+void eigen(const Matrix3fX<T>& m, typename T::meta_type dout[3], Vec3fX<typename T::vector_type> vout[3])
+{
+ Matrix3fX<T> R(m);
+ int n = 3;
+ int j, iq, ip, i;
+ typename T::meta_type tresh, theta, tau, t, sm, s, h, g, c;
+ int nrot;
+ typename T::meta_type b[3];
+ typename T::meta_type z[3];
+ typename T::meta_type v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+ typename T::meta_type d[3];
+
+ for(ip = 0; ip < n; ++ip)
+ {
+ b[ip] = d[ip] = R(ip, ip);
+ z[ip] = 0;
+ }
- inline FCL_REAL transposeDot(size_t i, const Vec3f& v) const
+ nrot = 0;
+
+ for(i = 0; i < 50; ++i)
+ {
+ sm = 0;
+ for(ip = 0; ip < n; ++ip)
+ for(iq = ip + 1; iq < n; ++iq)
+ sm += std::abs(R(ip, iq));
+ if(sm == 0.0)
{
- return v_[0][i] * v[0] + v_[1][i] * v[1] + v_[2][i] * v[2];
+ vout[0].setValue(v[0][0], v[0][1], v[0][2]);
+ vout[1].setValue(v[1][0], v[1][1], v[1][2]);
+ vout[2].setValue(v[2][0], v[2][1], v[2][2]);
+ dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2];
+ return;
}
-
- inline void setValue(FCL_REAL xx, FCL_REAL xy, FCL_REAL xz,
- FCL_REAL yx, FCL_REAL yy, FCL_REAL yz,
- FCL_REAL zx, FCL_REAL zy, FCL_REAL zz)
+
+ if(i < 3) tresh = 0.2 * sm / (n * n);
+ else tresh = 0.0;
+
+ for(ip = 0; ip < n; ++ip)
{
- v_[0].setValue(xx, xy, xz);
- v_[1].setValue(yx, yy, yz);
- v_[2].setValue(zx, zy, zz);
+ for(iq= ip + 1; iq < n; ++iq)
+ {
+ g = 100.0 * std::abs(R(ip, iq));
+ if(i > 3 &&
+ std::abs(d[ip]) + g == std::abs(d[ip]) &&
+ std::abs(d[iq]) + g == std::abs(d[iq]))
+ R(ip, iq) = 0.0;
+ else if(std::abs(R(ip, iq)) > tresh)
+ {
+ h = d[iq] - d[ip];
+ if(std::abs(h) + g == std::abs(h)) t = (R(ip, iq)) / h;
+ else
+ {
+ theta = 0.5 * h / (R(ip, iq));
+ t = 1.0 /(std::abs(theta) + std::sqrt(1.0 + theta * theta));
+ if(theta < 0.0) t = -t;
+ }
+ c = 1.0 / std::sqrt(1 + t * t);
+ s = t * c;
+ tau = s / (1.0 + c);
+ h = t * R(ip, iq);
+ z[ip] -= h;
+ z[iq] += h;
+ d[ip] -= h;
+ d[iq] += h;
+ R(ip, iq) = 0.0;
+ for(j = 0; j < ip; ++j) { g = R(j, ip); h = R(j, iq); R(j, ip) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
+ for(j = ip + 1; j < iq; ++j) { g = R(ip, j); h = R(j, iq); R(ip, j) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
+ for(j = iq + 1; j < n; ++j) { g = R(ip, j); h = R(iq, j); R(ip, j) = g - s * (h + g * tau); R(iq, j) = h + s * (g - h * tau); }
+ for(j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); }
+ nrot++;
+ }
+ }
}
-
- inline void setValue(FCL_REAL x)
+ for(ip = 0; ip < n; ++ip)
{
- v_[0].setValue(x);
- v_[1].setValue(x);
- v_[2].setValue(x);
+ b[ip] += z[ip];
+ d[ip] = b[ip];
+ z[ip] = 0.0;
}
- };
+ }
- void relativeTransform(const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, Matrix3f& R, Vec3f& T);
+ std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
- void matEigen(const Matrix3f& R, FCL_REAL dout[3], Vec3f vout[3]);
+ return;
+}
- Matrix3f abs(const Matrix3f& R);
+template<typename T>
+Matrix3fX<T> abs(const Matrix3fX<T>& R)
+{
+ return Matrix3fX<T>(abs(R.data));
}
+template<typename T>
+Matrix3fX<T> transpose(const Matrix3fX<T>& R)
+{
+ return Matrix3fX<T>(transpose(R.data));
+}
+
+template<typename T>
+Matrix3fX<T> inverse(const Matrix3fX<T>& R)
+{
+ return Matrix3fX<T>(inverse(R.data));
+}
+
+template<typename T>
+typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename T::vector_type>& v)
+{
+ return v.dot(R * v);
+}
+
+
+typedef Matrix3fX<details::Matrix3Data<FCL_REAL> > Matrix3f;
+//typedef Matrix3fX<details::sse_meta_f12> Matrix3f;
+
+}
+
+
+
#endif
diff --git a/trunk/fcl/include/fcl/octomap_extension.h b/trunk/fcl/include/fcl/octomap_extension.h
new file mode 100644
index 00000000..6b45b42b
--- /dev/null
+++ b/trunk/fcl/include/fcl/octomap_extension.h
@@ -0,0 +1,68 @@
+/*
+ * 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_OCTOMAP_EXTENSION_H
+#define FCL_OCTOMAP_EXTENSION_H
+
+#include <set>
+#include <octomap/octomap.h>
+#include "fcl/broad_phase_collision.h"
+
+namespace fcl
+{
+
+
+bool defaultCollisionCostFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost);
+
+bool defaultCollisionCostExtFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost, std::set<octomap::OcTreeNode*>& nodes);
+
+
+typedef bool (*CollisionCostCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost);
+
+typedef bool (*CollisionCostCallBackExt)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost, std::set<octomap::OcTreeNode*>& nodes);
+
+
+void collide(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCallBack callback);
+
+FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostCallBack callback);
+
+FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostCallBackExt callback, std::set<octomap::OcTreeNode*>& nodes);
+
+
+}
+
+#endif
diff --git a/trunk/fcl/include/fcl/primitive.h b/trunk/fcl/include/fcl/primitive.h
index 9b25878b..f5396422 100644
--- a/trunk/fcl/include/fcl/primitive.h
+++ b/trunk/fcl/include/fcl/primitive.h
@@ -58,7 +58,7 @@ struct Uncertainty
/** preprocess performs the eigen decomposition on the Sigma matrix */
void preprocess()
{
- matEigen(Sigma, sigma, axis);
+ eigen(Sigma, sigma, axis);
}
/** sqrt performs the sqrt of Sigma matrix based on the eigen decomposition result, this is useful when the uncertainty matrix is initialized
@@ -78,9 +78,9 @@ struct Uncertainty
{
for(int j = 0; j < 3; ++j)
{
- Sigma[i][j] += sigma[0] * axis[0][i] * axis[0][j];
- Sigma[i][j] += sigma[1] * axis[1][i] * axis[1][j];
- Sigma[i][j] += sigma[2] * axis[2][i] * axis[2][j];
+ Sigma(i, j) += sigma[0] * axis[0][i] * axis[0][j];
+ Sigma(i, j) += sigma[1] * axis[1][i] * axis[1][j];
+ Sigma(i, j) += sigma[2] * axis[2][i] * axis[2][j];
}
}
}
diff --git a/trunk/fcl/include/fcl/transform.h b/trunk/fcl/include/fcl/transform.h
index 260af6db..a5e9f7c9 100644
--- a/trunk/fcl/include/fcl/transform.h
+++ b/trunk/fcl/include/fcl/transform.h
@@ -223,7 +223,7 @@ public:
SimpleTransform inverse() const
{
- Matrix3f Rinv = R.transpose();
+ Matrix3f Rinv = transpose(R);
return SimpleTransform(Rinv, Rinv * (-T));
}
@@ -254,8 +254,11 @@ public:
bool isIdentity() const
{
- return (R[0][0] == 1) && (R[0][1] == 0) && (R[0][2] == 0) && (R[1][0] == 0) && (R[1][1] == 1) && (R[1][2] == 0) && (R[2][0] == 0) && (R[2][1] == 0) && (R[2][2] == 1)
- && (T[0] == 0) && (T[1] == 0) && (T[2] == 0);
+ return
+ (R(0, 0) == 1) && (R(0, 1) == 0) && (R(0, 2) == 0) &&
+ (R(1, 0) == 0) && (R(1, 1) == 1) && (R(1, 2) == 0) &&
+ (R(2, 0) == 0) && (R(2, 1) == 0) && (R(2, 2) == 1) &&
+ (T[0] == 0) && (T[1] == 0) && (T[2] == 0);
}
void setIdentity()
diff --git a/trunk/fcl/include/fcl/vec_3f.h b/trunk/fcl/include/fcl/vec_3f.h
index d4d202f0..83a5a02a 100644
--- a/trunk/fcl/include/fcl/vec_3f.h
+++ b/trunk/fcl/include/fcl/vec_3f.h
@@ -39,6 +39,7 @@
#include "fcl/BVH_internal.h"
#include "fcl/math_details.h"
+#include "fcl/math_simd_details.h"
#include <cmath>
#include <iostream>
#include <limits>
@@ -81,25 +82,31 @@ public:
inline Vec3fX operator - () const { return Vec3fX(-data); }
inline Vec3fX cross(const Vec3fX& other) const { return Vec3fX(details::cross_prod(data, other.data)); }
inline U dot(const Vec3fX& other) const { return details::dot_prod3(data, other.data); }
- inline bool normalize()
+ inline Vec3fX& normalize()
{
U sqr_length = details::dot_prod3(data, data);
if(sqr_length > 0)
- {
*this /= (U)sqrt(sqr_length);
- return true;
- }
- else
- return false;
+ return *this;
}
- inline Vec3fX normalized() const
+ inline Vec3fX& normalize(bool* signal)
{
U sqr_length = details::dot_prod3(data, data);
if(sqr_length > 0)
- return *this / (U)sqrt(sqr_length);
+ {
+ *this /= (U)sqrt(sqr_length);
+ *signal = true;
+ }
else
- return *this;
+ *signal = false;
+ return *this;
+ }
+
+ inline Vec3fX& abs()
+ {
+ data = abs(data);
+ return *this;
}
inline U length() const { return sqrt(details::dot_prod3(data, data)); }
@@ -107,7 +114,7 @@ public:
inline void setValue(U x, U y, U z) { data.setValue(x, y, z); }
inline void setValue(U x) { data.setValue(x); }
inline bool equal(const Vec3fX& other, U epsilon = std::numeric_limits<U>::epsilon() * 100) const { return details::equal(data, other.data, epsilon); }
- inline void negate() { data.negate(); }
+ inline Vec3fX<T>& negate() { data.negate(); return *this; }
inline Vec3fX<T>& ubound(const Vec3fX<T>& u)
{
@@ -120,8 +127,19 @@ public:
data.lbound(l.data);
return *this;
}
+
};
+template<typename T>
+static inline Vec3fX<T> normalize(const Vec3fX<T>& v)
+{
+ typename T::meta_type sqr_length = details::dot_prod3(v.data, v.data);
+ if(sqr_length > 0)
+ return v / (typename T::meta_type)sqrt(sqr_length);
+ else
+ return v;
+}
+
template <typename T>
static inline typename T::meta_type triple(const Vec3fX<T>& x, const Vec3fX<T>& y, const Vec3fX<T>& z)
{
@@ -182,6 +200,7 @@ void generateCoordinateSystem(const Vec3fX<T>& w, Vec3fX<T>& u, Vec3fX<T>& v)
typedef Vec3fX<details::Vec3Data<FCL_REAL> > Vec3f;
+//typedef Vec3fX<details::sse_meta_f4> Vec3f;
}
diff --git a/trunk/fcl/manifest.xml b/trunk/fcl/manifest.xml
index d7c37367..0ad5ac31 100644
--- a/trunk/fcl/manifest.xml
+++ b/trunk/fcl/manifest.xml
@@ -12,7 +12,7 @@
<depend package="common_rosdeps"/>
<rosdep name="flann" />
<rosdep name="libccd" />
-
+ <rosdep name="octomap" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lfcl"/>
diff --git a/trunk/fcl/src/BV/OBB.cpp b/trunk/fcl/src/BV/OBB.cpp
index ddd9a032..d089c7c6 100644
--- a/trunk/fcl/src/BV/OBB.cpp
+++ b/trunk/fcl/src/BV/OBB.cpp
@@ -121,7 +121,7 @@ bool OBB::obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const V
// A1 x A2 = A0
t = ((T[0] < 0.0) ? -T[0] : T[0]);
- if(t > (a[0] + b.dot(Bf[0])))
+ if(t > (a[0] + Bf.dotX(b)))
return true;
// B1 x B2 = B0
@@ -134,13 +134,13 @@ bool OBB::obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const V
// A2 x A0 = A1
t = ((T[1] < 0.0) ? -T[1] : T[1]);
- if(t > (a[1] + b.dot(Bf[1])))
+ if(t > (a[1] + Bf.dotY(b)))
return true;
// A0 x A1 = A2
t =((T[2] < 0.0) ? -T[2] : T[2]);
- if(t > (a[2] + b.dot(Bf[2])))
+ if(t > (a[2] + Bf.dotZ(b)))
return true;
// B2 x B0 = B1
@@ -158,75 +158,75 @@ bool OBB::obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const V
return true;
// A0 x B0
- s = T[2] * B[1][0] - T[1] * B[2][0];
+ s = T[2] * B(1, 0) - T[1] * B(2, 0);
t = ((s < 0.0) ? -s : s);
- if(t > (a[1] * Bf[2][0] + a[2] * Bf[1][0] +
- b[1] * Bf[0][2] + b[2] * Bf[0][1]))
+ if(t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) +
+ b[1] * Bf(0, 2) + b[2] * Bf(0, 1)))
return true;
// A0 x B1
- s = T[2] * B[1][1] - T[1] * B[2][1];
+ s = T[2] * B(1, 1) - T[1] * B(2, 1);
t = ((s < 0.0) ? -s : s);
- if(t > (a[1] * Bf[2][1] + a[2] * Bf[1][1] +
- b[0] * Bf[0][2] + b[2] * Bf[0][0]))
+ if(t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) +
+ b[0] * Bf(0, 2) + b[2] * Bf(0, 0)))
return true;
// A0 x B2
- s = T[2] * B[1][2] - T[1] * B[2][2];
+ s = T[2] * B(1, 2) - T[1] * B(2, 2);
t = ((s < 0.0) ? -s : s);
- if(t > (a[1] * Bf[2][2] + a[2] * Bf[1][2] +
- b[0] * Bf[0][1] + b[1] * Bf[0][0]))
+ if(t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) +
+ b[0] * Bf(0, 1) + b[1] * Bf(0, 0)))
return true;
// A1 x B0
- s = T[0] * B[2][0] - T[2] * B[0][0];
+ s = T[0] * B(2, 0) - T[2] * B(0, 0);
t = ((s < 0.0) ? -s : s);
- if(t > (a[0] * Bf[2][0] + a[2] * Bf[0][0] +
- b[1] * Bf[1][2] + b[2] * Bf[1][1]))
+ if(t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) +
+ b[1] * Bf(1, 2) + b[2] * Bf(1, 1)))
return true;
// A1 x B1
- s = T[0] * B[2][1] - T[2] * B[0][1];
+ s = T[0] * B(2, 1) - T[2] * B(0, 1);
t = ((s < 0.0) ? -s : s);
- if(t > (a[0] * Bf[2][1] + a[2] * Bf[0][1] +
- b[0] * Bf[1][2] + b[2] * Bf[1][0]))
+ if(t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) +
+ b[0] * Bf(1, 2) + b[2] * Bf(1, 0)))
return true;
// A1 x B2
- s = T[0] * B[2][2] - T[2] * B[0][2];
+ s = T[0] * B(2, 2) - T[2] * B(0, 2);
t = ((s < 0.0) ? -s : s);
- if(t > (a[0] * Bf[2][2] + a[2] * Bf[0][2] +
- b[0] * Bf[1][1] + b[1] * Bf[1][0]))
+ if(t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) +
+ b[0] * Bf(1, 1) + b[1] * Bf(1, 0)))
return true;
// A2 x B0
- s = T[1] * B[0][0] - T[0] * B[1][0];
+ s = T[1] * B(0, 0) - T[0] * B(1, 0);
t = ((s < 0.0) ? -s : s);
- if(t > (a[0] * Bf[1][0] + a[1] * Bf[0][0] +
- b[1] * Bf[2][2] + b[2] * Bf[2][1]))
+ if(t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) +
+ b[1] * Bf(2, 2) + b[2] * Bf(2, 1)))
return true;
// A2 x B1
- s = T[1] * B[0][1] - T[0] * B[1][1];
+ s = T[1] * B(0, 1) - T[0] * B(1, 1);
t = ((s < 0.0) ? -s : s);
- if(t > (a[0] * Bf[1][1] + a[1] * Bf[0][1] +
- b[0] * Bf[2][2] + b[2] * Bf[2][0]))
+ if(t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) +
+ b[0] * Bf(2, 2) + b[2] * Bf(2, 0)))
return true;
// A2 x B2
- s = T[1] * B[0][2] - T[0] * B[1][2];
+ s = T[1] * B(0, 2) - T[0] * B(1, 2);
t = ((s < 0.0) ? -s : s);
- if(t > (a[0] * Bf[1][2] + a[1] * Bf[0][2] +
- b[0] * Bf[2][1] + b[1] * Bf[2][0]))
+ if(t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) +
+ b[0] * Bf(2, 1) + b[1] * Bf(2, 0)))
return true;
return false;
@@ -274,7 +274,7 @@ OBB OBB::merge_largedist(const OBB& b1, const OBB& b2)
vertex_proj[i] = vertex[i] - R0 * vertex[i].dot(R0);
getCovariance(vertex_proj, NULL, NULL, NULL, 16, M);
- matEigen(M, s, E);
+ eigen(M, s, E);
int min, mid, max;
if (s[0] > s[1]) { max = 0; min = 1; }
@@ -365,9 +365,9 @@ FCL_REAL OBB::distance(const OBB& other, Vec3f* P, Vec3f* Q) const
// R is row first
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2)
{
- Matrix3f R0b2(R0[0].dot(b2.axis[0]), R0[0].dot(b2.axis[1]), R0[0].dot(b2.axis[2]),
- R0[1].dot(b2.axis[0]), R0[1].dot(b2.axis[1]), R0[1].dot(b2.axis[2]),
- R0[2].dot(b2.axis[0]), R0[2].dot(b2.axis[1]), R0[2].dot(b2.axis[2]));
+ Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
+ R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
+ R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
diff --git a/trunk/fcl/src/BV/RSS.cpp b/trunk/fcl/src/BV/RSS.cpp
index 31ff3cb0..1cf86b47 100644
--- a/trunk/fcl/src/BV/RSS.cpp
+++ b/trunk/fcl/src/BV/RSS.cpp
@@ -58,9 +58,9 @@ bool RSS::overlap(const RSS& other) const
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
{
- Matrix3f R0b2(R0[0].dot(b2.axis[0]), R0[0].dot(b2.axis[1]), R0[0].dot(b2.axis[2]),
- R0[1].dot(b2.axis[0]), R0[1].dot(b2.axis[1]), R0[1].dot(b2.axis[2]),
- R0[2].dot(b2.axis[0]), R0[2].dot(b2.axis[1]), R0[2].dot(b2.axis[2]));
+ Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
+ R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
+ R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
@@ -299,7 +299,7 @@ RSS RSS::operator + (const RSS& other) const
FCL_REAL s[3] = {0, 0, 0};
getCovariance(v, NULL, NULL, NULL, 16, M);
- matEigen(M, s, E);
+ eigen(M, s, E);
int min, mid, max;
if(s[0] > s[1]) { max = 0; min = 1; }
@@ -339,9 +339,9 @@ FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P, Vec3f* Q)
{
- Matrix3f R0b2(R0[0].dot(b2.axis[0]), R0[0].dot(b2.axis[1]), R0[0].dot(b2.axis[2]),
- R0[1].dot(b2.axis[0]), R0[1].dot(b2.axis[1]), R0[1].dot(b2.axis[2]),
- R0[2].dot(b2.axis[0]), R0[2].dot(b2.axis[1]), R0[2].dot(b2.axis[2]));
+ Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
+ R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
+ R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
@@ -361,10 +361,10 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
{
FCL_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
- A0_dot_B0 = Rab[0][0];
- A0_dot_B1 = Rab[0][1];
- A1_dot_B0 = Rab[1][0];
- A1_dot_B1 = Rab[1][1];
+ A0_dot_B0 = Rab(0, 0);
+ A0_dot_B1 = Rab(0, 1);
+ A1_dot_B0 = Rab(1, 0);
+ A1_dot_B1 = Rab(1, 1);
FCL_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1;
FCL_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1;
@@ -437,17 +437,17 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0],
A1_dot_B1, aA0_dot_B1 - Tba[1],
-Tab[1] - bA1_dot_B0))
- &&
- ((UB1_lx > a[0]) ||
- inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0],
- A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1)))
+ &&
+ ((UB1_lx > a[0]) ||
+ inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0],
+ A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1)))
{
segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0,
Tba[1] - aA0_dot_B1);
- S[0] = Tab[0] + Rab[0][0] * b[0] + Rab[0][1] * u - a[0] ;
- S[1] = Tab[1] + Rab[1][0] * b[0] + Rab[1][1] * u - t;
- S[2] = Tab[2] + Rab[2][0] * b[0] + Rab[2][1] * u;
+ S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0] ;
+ S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
+ S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
if(P && Q)
{
@@ -467,16 +467,16 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(((UA1_ux < 0) ||
inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0,
A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1]))
- &&
- ((LB1_lx > a[0]) ||
- inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0],
- A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1)))
+ &&
+ ((LB1_lx > a[0]) ||
+ inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0],
+ A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1)))
{
segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1);
- S[0] = Tab[0] + Rab[0][1] * u - a[0];
- S[1] = Tab[1] + Rab[1][1] * u - t;
- S[2] = Tab[2] + Rab[2][1] * u;
+ S[0] = Tab[0] + Rab(0, 1) * u - a[0];
+ S[1] = Tab[1] + Rab(1, 1) * u - t;
+ S[2] = Tab[2] + Rab(2, 1) * u;
if(P && Q)
{
@@ -495,16 +495,16 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(((LA1_lx > b[0]) ||
inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0],
A1_dot_B1, -Tba[1], -Tab[1] - bA1_dot_B0))
- &&
- ((UB1_ux < 0) ||
- inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0,
- A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1])))
+ &&
+ ((UB1_ux < 0) ||
+ inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0,
+ A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1])))
{
segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]);
- S[0] = Tab[0] + Rab[0][0] * b[0] + Rab[0][1] * u;
- S[1] = Tab[1] + Rab[1][0] * b[0] + Rab[1][1] * u - t;
- S[2] = Tab[2] + Rab[2][0] * b[0] + Rab[2][1] * u;
+ S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u;
+ S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
+ S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
if(P && Q)
{
@@ -521,8 +521,8 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if((LA1_lx < 0) && (LB1_lx < 0))
{
if (((LA1_ux < 0) ||
- inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1,
- -Tba[1], -Tab[1]))
+ inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1,
+ -Tba[1], -Tab[1]))
&&
((LB1_ux < 0) ||
inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1,
@@ -530,9 +530,9 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
{
segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]);
- S[0] = Tab[0] + Rab[0][1] * u;
- S[1] = Tab[1] + Rab[1][1] * u - t;
- S[2] = Tab[2] + Rab[2][1] * u;
+ S[0] = Tab[0] + Rab(0, 1) * u;
+ S[1] = Tab[1] + Rab(1, 1) * u - t;
+ S[2] = Tab[2] + Rab(2, 1) * u;
if(P && Q)
{
@@ -590,17 +590,17 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(((UA1_ly > b[1]) ||
inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1],
A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1))
- &&
- ((UB0_lx > a[0]) ||
- inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1,
- A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0)))
+ &&
+ ((UB0_lx > a[0]) ||
+ inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1,
+ A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0)))
{
segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1,
Tba[0] - aA0_dot_B0);
- S[0] = Tab[0] + Rab[0][1] * b[1] + Rab[0][0] * u - a[0] ;
- S[1] = Tab[1] + Rab[1][1] * b[1] + Rab[1][0] * u - t;
- S[2] = Tab[2] + Rab[2][1] * b[1] + Rab[2][0] * u;
+ S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0] ;
+ S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
+ S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
if(P && Q)
{
@@ -619,16 +619,16 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(((UA1_uy < 0) ||
inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, A1_dot_B0,
aA0_dot_B0 - Tba[0], -Tab[1]))
- &&
- ((LB0_lx > a[0]) ||
- inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0],
- A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0)))
+ &&
+ ((LB0_lx > a[0]) ||
+ inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0],
+ A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0)))
{
segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0);
- S[0] = Tab[0] + Rab[0][0] * u - a[0];
- S[1] = Tab[1] + Rab[1][0] * u - t;
- S[2] = Tab[2] + Rab[2][0] * u;
+ S[0] = Tab[0] + Rab(0, 0) * u - a[0];
+ S[1] = Tab[1] + Rab(1, 0) * u - t;
+ S[2] = Tab[2] + Rab(2, 0) * u;
if(P && Q)
{
@@ -647,17 +647,17 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(((LA1_ly > b[1]) ||
inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1],
A1_dot_B0, -Tba[0], -Tab[1] - bA1_dot_B1))
- &&
+ &&
- ((UB0_ux < 0) ||
- inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1, A1_dot_B0,
- Tab[1] + bA1_dot_B1, Tba[0])))
+ ((UB0_ux < 0) ||
+ inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1, A1_dot_B0,
+ Tab[1] + bA1_dot_B1, Tba[0])))
{
segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]);
- S[0] = Tab[0] + Rab[0][1] * b[1] + Rab[0][0] * u;
- S[1] = Tab[1] + Rab[1][1] * b[1] + Rab[1][0] * u - t;
- S[2] = Tab[2] + Rab[2][1] * b[1] + Rab[2][0] * u;
+ S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u;
+ S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
+ S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
if(P && Q)
{
@@ -677,16 +677,16 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(((LA1_uy < 0) ||
inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0,
-Tba[0], -Tab[1]))
- &&
- ((LB0_ux < 0) ||
- inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0,
- Tab[1], Tba[0])))
+ &&
+ ((LB0_ux < 0) ||
+ inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0,
+ Tab[1], Tba[0])))
{
segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]);
- S[0] = Tab[0] + Rab[0][0] * u;
- S[1] = Tab[1] + Rab[1][0] * u - t;
- S[2] = Tab[2] + Rab[2][0] * u;
+ S[0] = Tab[0] + Rab(0, 0) * u;
+ S[1] = Tab[1] + Rab(1, 0) * u - t;
+ S[2] = Tab[2] + Rab(2, 0) * u;
if(P&& Q)
{
@@ -744,17 +744,17 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(((UA0_lx > b[0]) ||
inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0],
A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0))
- &&
- ((UB1_ly > a[1]) ||
- inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0,
- A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1)))
+ &&
+ ((UB1_ly > a[1]) ||
+ inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0,
+ A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1)))
{
segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0,
Tba[1] - aA1_dot_B1);
- S[0] = Tab[0] + Rab[0][0] * b[0] + Rab[0][1] * u - t;
- S[1] = Tab[1] + Rab[1][0] * b[0] + Rab[1][1] * u - a[1];
- S[2] = Tab[2] + Rab[2][0] * b[0] + Rab[2][1] * u;
+ S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
+ S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - a[1];
+ S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
if(P && Q)
{
@@ -773,16 +773,16 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(((UA0_ux < 0) ||
inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, A0_dot_B1,
aA1_dot_B1 - Tba[1], -Tab[0]))
- &&
- ((LB1_ly > a[1]) ||
- inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0],
- Tba[1] - aA1_dot_B1)))
+ &&
+ ((LB1_ly > a[1]) ||
+ inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0],
+ Tba[1] - aA1_dot_B1)))
{
segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1);
- S[0] = Tab[0] + Rab[0][1] * u - t;
- S[1] = Tab[1] + Rab[1][1] * u - a[1];
- S[2] = Tab[2] + Rab[2][1] * u;
+ S[0] = Tab[0] + Rab(0, 1) * u - t;
+ S[1] = Tab[1] + Rab(1, 1) * u - a[1];
+ S[2] = Tab[2] + Rab(2, 1) * u;
if(P && Q)
{
@@ -801,16 +801,16 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(((LA0_lx > b[0]) ||
inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1],
-bA0_dot_B0 - Tab[0]))
- &&
- ((UB1_uy < 0) ||
- inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0, A0_dot_B1,
- Tab[0] + bA0_dot_B0, Tba[1])))
+ &&
+ ((UB1_uy < 0) ||
+ inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0, A0_dot_B1,
+ Tab[0] + bA0_dot_B0, Tba[1])))
{
segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]);
- S[0] = Tab[0] + Rab[0][0] * b[0] + Rab[0][1] * u - t;
- S[1] = Tab[1] + Rab[1][0] * b[0] + Rab[1][1] * u;
- S[2] = Tab[2] + Rab[2][0] * b[0] + Rab[2][1] * u;
+ S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
+ S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u;
+ S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
if(P && Q)
{
@@ -829,16 +829,16 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(((LA0_ux < 0) ||
inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, -Tba[1],
-Tab[0]))
- &&
- ((LB1_uy < 0) ||
- inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1,
- Tab[0], Tba[1])))
+ &&
+ ((LB1_uy < 0) ||
+ inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1,
+ Tab[0], Tba[1])))
{
segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]);
- S[0] = Tab[0] + Rab[0][1] * u - t;
- S[1] = Tab[1] + Rab[1][1] * u;
- S[2] = Tab[2] + Rab[2][1] * u;
+ S[0] = Tab[0] + Rab(0, 1) * u - t;
+ S[1] = Tab[1] + Rab(1, 1) * u;
+ S[2] = Tab[2] + Rab(2, 1) * u;
if(P && Q)
{
@@ -889,17 +889,17 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(((UA0_ly > b[1]) ||
inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1],
A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1))
- &&
- ((UB0_ly > a[1]) ||
- inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0,
- Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0)))
+ &&
+ ((UB0_ly > a[1]) ||
+ inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0,
+ Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0)))
{
segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1,
Tba[0] - aA1_dot_B0);
- S[0] = Tab[0] + Rab[0][1] * b[1] + Rab[0][0] * u - t;
- S[1] = Tab[1] + Rab[1][1] * b[1] + Rab[1][0] * u - a[1];
- S[2] = Tab[2] + Rab[2][1] * b[1] + Rab[2][0] * u;
+ S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
+ S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - a[1];
+ S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
if(P && Q)
{
@@ -918,16 +918,16 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(((UA0_uy < 0) ||
inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, A0_dot_B0,
aA1_dot_B0 - Tba[0], -Tab[0]))
- &&
- ((LB0_ly > a[1]) ||
- inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1],
- A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0)))
+ &&
+ ((LB0_ly > a[1]) ||
+ inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1],
+ A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0)))
{
segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0);
- S[0] = Tab[0] + Rab[0][0] * u - t;
- S[1] = Tab[1] + Rab[1][0] * u - a[1];
- S[2] = Tab[2] + Rab[2][0] * u;
+ S[0] = Tab[0] + Rab(0, 0) * u - t;
+ S[1] = Tab[1] + Rab(1, 0) * u - a[1];
+ S[2] = Tab[2] + Rab(2, 0) * u;
if(P && Q)
{
@@ -946,25 +946,25 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(((LA0_ly > b[1]) ||
inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0],
-Tab[0] - bA0_dot_B1))
- &&
+ &&
- ((UB0_uy < 0) ||
- inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1, A0_dot_B0,
- Tab[0] + bA0_dot_B1, Tba[0])))
+ ((UB0_uy < 0) ||
+ inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1, A0_dot_B0,
+ Tab[0] + bA0_dot_B1, Tba[0])))
{
segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]);
- S[0] = Tab[0] + Rab[0][1] * b[1] + Rab[0][0] * u - t;
- S[1] = Tab[1] + Rab[1][1] * b[1] + Rab[1][0] * u;
- S[2] = Tab[2] + Rab[2][1] * b[1] + Rab[2][0] * u;
+ S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
+ S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u;
+ S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
- if(P && Q)
- {
- P->setValue(t, 0, 0);
- *Q = S + (*P);
- }
+ if(P && Q)
+ {
+ P->setValue(t, 0, 0);
+ *Q = S + (*P);
+ }
- return S.length();
+ return S.length();
}
}
@@ -975,16 +975,16 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(((LA0_uy < 0) ||
inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0,
-Tba[0], -Tab[0]))
- &&
- ((LB0_uy < 0) ||
- inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0,
- Tab[0], Tba[0])))
+ &&
+ ((LB0_uy < 0) ||
+ inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0,
+ Tab[0], Tba[0])))
{
segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]);
- S[0] = Tab[0] + Rab[0][0] * u - t;
- S[1] = Tab[1] + Rab[1][0] * u;
- S[2] = Tab[2] + Rab[2][0] * u;
+ S[0] = Tab[0] + Rab(0, 0) * u - t;
+ S[1] = Tab[1] + Rab(1, 0) * u;
+ S[2] = Tab[2] + Rab(2, 0) * u;
if(P && Q)
{
@@ -1003,27 +1003,27 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
if(Tab[2] > 0.0)
{
sep1 = Tab[2];
- if (Rab[2][0] < 0.0) sep1 += b[0] * Rab[2][0];
- if (Rab[2][1] < 0.0) sep1 += b[1] * Rab[2][1];
+ if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0);
+ if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1);
}
else
{
sep1 = -Tab[2];
- if (Rab[2][0] > 0.0) sep1 -= b[0] * Rab[2][0];
- if (Rab[2][1] > 0.0) sep1 -= b[1] * Rab[2][1];
+ if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0);
+ if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1);
}
if(Tba[2] < 0)
{
sep2 = -Tba[2];
- if (Rab[0][2] < 0.0) sep2 += a[0] * Rab[0][2];
- if (Rab[1][2] < 0.0) sep2 += a[1] * Rab[1][2];
+ if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2);
+ if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2);
}
else
{
sep2 = Tba[2];
- if (Rab[0][2] > 0.0) sep2 -= a[0] * Rab[0][2];
- if (Rab[1][2] > 0.0) sep2 -= a[1] * Rab[1][2];
+ if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2);
+ if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2);
}
if(sep1 >= sep2 && sep1 >= 0)
@@ -1046,15 +1046,15 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
Vec3f P_;
if(Tba[2] < 0)
{
- P_[0] = Rab[0][2] * sep2 + Tab[0];
- P_[1] = Rab[1][2] * sep2 + Tab[1];
- P_[2] = Rab[2][2] * sep2 + Tab[2];
+ P_[0] = Rab(0, 2) * sep2 + Tab[0];
+ P_[1] = Rab(1, 2) * sep2 + Tab[1];
+ P_[2] = Rab(2, 2) * sep2 + Tab[2];
}
else
{
- P_[0] = -Rab[0][2] * sep2 + Tab[0];
- P_[1] = -Rab[1][2] * sep2 + Tab[1];
- P_[2] = -Rab[2][2] * sep2 + Tab[2];
+ P_[0] = -Rab(0, 2) * sep2 + Tab[0];
+ P_[1] = -Rab(1, 2) * sep2 + Tab[1];
+ P_[2] = -Rab(2, 2) * sep2 + Tab[2];
}
S = Q_ - P_;
diff --git a/trunk/fcl/src/BVH_utility.cpp b/trunk/fcl/src/BVH_utility.cpp
index d7b3cf19..b5d197aa 100644
--- a/trunk/fcl/src/BVH_utility.cpp
+++ b/trunk/fcl/src/BVH_utility.cpp
@@ -179,7 +179,7 @@ void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Uncertainty*
for(int k = 0; k < dim; ++k)
{
C[j][k] /= weight_sum;
- ucs[i].Sigma[j][k] = C[j][k];
+ ucs[i].Sigma(j, k) = C[j][k];
}
}
@@ -192,91 +192,6 @@ void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Uncertainty*
delete [] query.ptr();
delete [] indices.ptr();
delete [] dists.ptr();
-
- /*
- int nPts = num_vertices;
- ANNpointArray dataPts;
- ANNpoint queryPt;
-
- ANNidxArray nnIdx;
- ANNdistArray dists;
- ANNkd_tree* kdTree;
-
- int knn_k = 10;
- if(knn_k > nPts) knn_k = nPts;
- int dim = 3;
-
- queryPt = annAllocPt(dim);
- dataPts = annAllocPts(nPts, dim);
- nnIdx = new ANNidx[knn_k];
- dists = new ANNdist[knn_k];
-
- for(int i = 0; i < nPts; ++i)
- {
- for(int j = 0; j < dim; ++j)
- dataPts[i][j] = vertices[i][j];
- }
-
- kdTree = new ANNkd_tree(dataPts, nPts, dim);
-
- double eps = 0;
- double scale = 2;
-
- for(int i = 0; i < nPts; ++i)
- {
- float C[3][3];
- for(int j = 0; j < 3; ++j)
- {
- for(int k = 0; k < 3; ++k)
- C[j][k] = 0;
- }
-
- for(int j = 0; j < 3; ++j)
- queryPt[j] = vertices[i][j];
-
- kdTree->annkSearch(queryPt, knn_k, nnIdx, dists, eps);
-
- for(int j = 0; j < knn_k; ++j)
- dists[j] = sqrt(dists[j]);
-
- double r = dists[knn_k - 1];
- double sigma = scale * r;
-
- double weight_sum = 0;
- for(int j = 1; j < knn_k; ++j)
- {
- int id = nnIdx[j];
- Vec3f p = vertices[i] - vertices[id];
- double norm2p = p.sqrLength();
- double weight = exp(-norm2p / (sigma * sigma));
-
- weight_sum += weight;
-
- for(int k = 0; k < 3; ++k)
- {
- for(int l = 0; l < 3; ++l)
- C[k][l] += p[k] * p[l] * weight;
- }
- }
-
- for(int j = 0; j < 3; ++j)
- {
- for(int k = 0; k < 3; ++k)
- {
- C[j][k] /= weight_sum;
- ucs[i].Sigma[j][k] = C[j][k];
- }
- }
-
- ucs[i].preprocess();
- ucs[i].sqrt();
- }
-
- delete [] nnIdx;
- delete [] dists;
- delete kdTree;
- annClose();
- */
}
void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M)
@@ -352,15 +267,15 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, i
int n_points = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
- M[0][0] = S2[0][0] - S1[0]*S1[0] / n_points;
- M[1][1] = S2[1][1] - S1[1]*S1[1] / n_points;
- M[2][2] = S2[2][2] - S1[2]*S1[2] / n_points;
- M[0][1] = S2[0][1] - S1[0]*S1[1] / n_points;
- M[1][2] = S2[1][2] - S1[1]*S1[2] / n_points;
- M[0][2] = S2[0][2] - S1[0]*S1[2] / n_points;
- M[1][0] = M[0][1];
- M[2][0] = M[0][2];
- M[2][1] = M[1][2];
+ M(0, 0) = S2[0][0] - S1[0]*S1[0] / n_points;
+ M(1, 1) = S2[1][1] - S1[1]*S1[1] / n_points;
+ M(2, 2) = S2[2][2] - S1[2]*S1[2] / n_points;
+ M(0, 1) = S2[0][1] - S1[0]*S1[1] / n_points;
+ M(1, 2) = S2[1][2] - S1[1]*S1[2] / n_points;
+ M(0, 2) = S2[0][2] - S1[0]*S1[2] / n_points;
+ M(1, 0) = M(0, 1);
+ M(2, 0) = M(0, 2);
+ M(2, 1) = M(1, 2);
}
@@ -478,9 +393,9 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
FCL_REAL x, dz;
dz = P[minindex][2] - cz;
- minx = P[minindex][0] + sqrt(std::max(radsqr - dz * dz, 0.0));
+ minx = P[minindex][0] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
dz = P[maxindex][2] - cz;
- maxx = P[maxindex][0] - sqrt(std::max(radsqr - dz * dz, 0.0));
+ maxx = P[maxindex][0] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
// grow minx
@@ -490,7 +405,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
if(P[i][0] < minx)
{
dz = P[i][2] - cz;
- x = P[i][0] + sqrt(std::max(radsqr - dz * dz, 0.0));
+ x = P[i][0] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
if(x < minx) minx = x;
}
}
@@ -502,7 +417,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
if(P[i][0] > maxx)
{
dz = P[i][2] - cz;
- x = P[i][0] - sqrt(std::max(radsqr - dz * dz, 0.0));
+ x = P[i][0] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
if(x > maxx) maxx = x;
}
}
@@ -530,9 +445,9 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
FCL_REAL y;
dz = P[minindex][2] - cz;
- miny = P[minindex][1] + sqrt(std::max(radsqr - dz * dz, 0.0));
+ miny = P[minindex][1] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
dz = P[maxindex][2] - cz;
- maxy = P[maxindex][1] - sqrt(std::max(radsqr - dz * dz, 0.0));
+ maxy = P[maxindex][1] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
// grow miny
@@ -541,7 +456,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
if(P[i][1] < miny)
{
dz = P[i][2] - cz;
- y = P[i][1] + sqrt(std::max(radsqr - dz * dz, 0.0));
+ y = P[i][1] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
if(y < miny) miny = y;
}
}
@@ -553,7 +468,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
if(P[i][1] > maxy)
{
dz = P[i][2] - cz;
- y = P[i][1] - sqrt(std::max(radsqr - dz * dz, 0.0));
+ y = P[i][1] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
if(y > maxy) maxy = y;
}
}
@@ -561,7 +476,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
// corners may have some points which are not covered - grow lengths if necessary
// quite conservative (can be improved)
FCL_REAL dx, dy, u, t;
- FCL_REAL a = sqrt((FCL_REAL)0.5);
+ FCL_REAL a = std::sqrt((FCL_REAL)0.5);
for(int i = 0; i < size_P; ++i)
{
if(P[i][0] > maxx)
@@ -574,7 +489,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
t = (a*u - dx)*(a*u - dx) +
(a*u - dy)*(a*u - dy) +
(cz - P[i][2])*(cz - P[i][2]);
- u = u - sqrt(std::max(radsqr - t, 0.0));
+ u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
if(u > 0)
{
maxx += u*a;
@@ -589,7 +504,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
t = (a*u - dx)*(a*u - dx) +
(-a*u - dy)*(-a*u - dy) +
(cz - P[i][2])*(cz - P[i][2]);
- u = u - sqrt(std::max(radsqr - t, 0.0));
+ u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
if(u > 0)
{
maxx += u*a;
@@ -607,7 +522,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
t = (-a*u - dx)*(-a*u - dx) +
(a*u - dy)*(a*u - dy) +
(cz - P[i][2])*(cz - P[i][2]);
- u = u - sqrt(std::max(radsqr - t, 0.0));
+ u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
if(u > 0)
{
minx -= u*a;
@@ -622,7 +537,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
t = (-a*u - dx)*(-a*u - dx) +
(-a*u - dy)*(-a*u - dy) +
(cz - P[i][2])*(cz - P[i][2]);
- u = u - sqrt(std::max(radsqr - t, 0.0));
+ u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
if (u > 0)
{
minx -= u*a;
@@ -787,7 +702,7 @@ void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec
Vec3f e3 = e1.cross(e2);
FCL_REAL e3_len2 = e3.sqrLength();
radius = e1_len2 * e2_len2 * (e1 - e2).sqrLength() / e3_len2;
- radius = sqrt(radius) * 0.5;
+ radius = std::sqrt(radius) * 0.5;
center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c;
}
@@ -826,7 +741,7 @@ static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
}
}
- return sqrt(maxD);
+ return std::sqrt(maxD);
}
static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, const Vec3f& query)
@@ -851,7 +766,7 @@ static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, unsigne
}
}
- return sqrt(maxD);
+ return std::sqrt(maxD);
}
FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query)
diff --git a/trunk/fcl/src/BV_fitter.cpp b/trunk/fcl/src/BV_fitter.cpp
index c3e57ee0..a423b0b6 100644
--- a/trunk/fcl/src/BV_fitter.cpp
+++ b/trunk/fcl/src/BV_fitter.cpp
@@ -140,7 +140,7 @@ void fitn(Vec3f* ps, int n, OBB& bv)
FCL_REAL s[3] = {0, 0, 0}; // three eigen values
getCovariance(ps, NULL, NULL, NULL, n, M);
- matEigen(M, s, E);
+ eigen(M, s, E);
axisFromEigen(E, s, bv.axis);
// set obb centers and extensions
@@ -226,7 +226,7 @@ void fitn(Vec3f* ps, int n, RSS& bv)
FCL_REAL s[3] = {0, 0, 0};
getCovariance(ps, NULL, NULL, NULL, n, M);
- matEigen(M, s, E);
+ eigen(M, s, E);
axisFromEigen(E, s, bv.axis);
// set rss origin, rectangle size and radius
@@ -343,7 +343,7 @@ void fitn(Vec3f* ps, int n, kIOS& bv)
FCL_REAL s[3] = {0, 0, 0}; // three eigen values;
getCovariance(ps, NULL, NULL, NULL, n, M);
- matEigen(M, s, E);
+ eigen(M, s, E);
Vec3f* axis = bv.obb_bv.axis;
axisFromEigen(E, s, axis);
@@ -525,7 +525,7 @@ OBB BVFitter<OBB>::fit(unsigned int* primitive_indices, int num_primitives)
FCL_REAL s[3]; // three eigen values
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
- matEigen(M, s, E);
+ eigen(M, s, E);
axisFromEigen(E, s, bv.axis);
@@ -543,7 +543,7 @@ OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices, int num_primitives
FCL_REAL s[3];
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
- matEigen(M, s, E);
+ eigen(M, s, E);
axisFromEigen(E, s, bv.obb.axis);
bv.rss.axis[0] = bv.obb.axis[0];
@@ -573,7 +573,7 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
Vec3f E[3]; // row first eigen-vectors
FCL_REAL s[3]; // three eigen values
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
- matEigen(M, s, E);
+ eigen(M, s, E);
axisFromEigen(E, s, bv.axis);
// set rss origin, rectangle size and radius
@@ -602,7 +602,7 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
FCL_REAL s[3];
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
- matEigen(M, s, E);
+ eigen(M, s, E);
Vec3f* axis = bv.obb_bv.axis;
axisFromEigen(E, s, axis);
diff --git a/trunk/fcl/src/ccd/interval.cpp b/trunk/fcl/src/ccd/interval.cpp
index 86250917..f86e1350 100644
--- a/trunk/fcl/src/ccd/interval.cpp
+++ b/trunk/fcl/src/ccd/interval.cpp
@@ -40,6 +40,22 @@
namespace fcl
{
+Interval bound(const Interval& i, FCL_REAL v)
+{
+ Interval res = i;
+ if(v < res.i_[0]) res.i_[0] = v;
+ if(v > res.i_[1]) res.i_[1] = v;
+ return res;
+}
+
+Interval bound(const Interval& i, const Interval& other)
+{
+ Interval res = i;
+ 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;
+}
+
Interval Interval::operator * (const Interval& other) const
{
if(other.i_[0] >= 0)
@@ -74,11 +90,111 @@ Interval Interval::operator * (const Interval& other) const
return Interval(v10, v00);
}
+Interval& Interval::operator *= (const Interval& other)
+{
+ if(other.i_[0] >= 0)
+ {
+ if(i_[0] >= 0)
+ {
+ i_[0] *= other.i_[0];
+ i_[1] *= other.i_[1];
+ }
+ else if(i_[1] <= 0)
+ {
+ i_[0] *= other.i_[1];
+ i_[1] *= other.i_[0];
+ }
+ else
+ {
+ i_[0] *= other.i_[1];
+ i_[1] *= other.i_[1];
+ }
+ return *this;
+ }
+
+ if(other.i_[1] <= 0)
+ {
+ if(i_[0] >= 0)
+ {
+ FCL_REAL tmp = i_[0];
+ i_[0] = i_[1] * other.i_[0];
+ i_[1] = tmp * other.i_[1];
+ }
+ else if(i_[1] <= 0)
+ {
+ FCL_REAL tmp = i_[0];
+ i_[0] = i_[1] * other.i_[1];
+ i_[1] = tmp * other.i_[0];
+ }
+ else
+ {
+ FCL_REAL tmp = i_[0];
+ i_[0] = i_[1] * other.i_[0];
+ i_[1] = tmp * other.i_[0];
+ }
+ return *this;
+ }
+
+ if(i_[0] >= 0)
+ {
+ i_[0] = i_[1] * other.i_[0];
+ i_[1] *= other.i_[1];
+ return *this;
+ }
+
+ if(i_[1] <= 0)
+ {
+ i_[1] = i_[0] * other.i_[0];
+ i_[0] *= other.i_[1];
+ return *this;
+ }
+
+ FCL_REAL v00 = i_[0] * other.i_[0];
+ FCL_REAL v11 = i_[1] * other.i_[1];
+ if(v00 <= v11)
+ {
+ FCL_REAL v01 = i_[0] * other.i_[1];
+ FCL_REAL v10 = i_[1] * other.i_[0];
+ if(v01 < v10)
+ {
+ i_[0] = v01;
+ i_[1] = v11;
+ }
+ else
+ {
+ i_[0] = v10;
+ i_[1] = v11;
+ }
+ return *this;
+ }
+
+ FCL_REAL v01 = i_[0] * other.i_[1];
+ FCL_REAL v10 = i_[1] * other.i_[0];
+ if(v01 < v10)
+ {
+ i_[0] = v01;
+ i_[1] = v00;
+ }
+ else
+ {
+ i_[0] = v10;
+ i_[1] = v00;
+ }
+
+ return *this;
+}
+
Interval Interval::operator / (const Interval& other) const
{
return *this * Interval(1.0 / other.i_[1], 1.0 / other.i_[0]);
}
+Interval& Interval::operator /= (const Interval& other)
+{
+ *this *= Interval(1.0 / other.i_[1], 1.0 / other.i_[0]);
+ return *this;
+}
+
void Interval::print() const
{
std::cout << "[" << i_[0] << ", " << i_[1] << "]" << std::endl;
diff --git a/trunk/fcl/src/ccd/interval_matrix.cpp b/trunk/fcl/src/ccd/interval_matrix.cpp
index 452e48a8..442b2858 100644
--- a/trunk/fcl/src/ccd/interval_matrix.cpp
+++ b/trunk/fcl/src/ccd/interval_matrix.cpp
@@ -40,732 +40,189 @@
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() {}
IMatrix3::IMatrix3(FCL_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;
+ v_[0].setValue(v);
+ v_[1].setValue(v);
+ v_[2].setValue(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];
+ v_[0] = m.getRow(0);
+ v_[1] = m.getRow(1);
+ v_[2] = m.getRow(2);
}
IMatrix3::IMatrix3(FCL_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]);
+ v_[0].setValue(m[0]);
+ v_[1].setValue(m[1]);
+ v_[2].setValue(m[2]);
}
IMatrix3::IMatrix3(FCL_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]);
+ v_[0].setValue(m[0]);
+ v_[1].setValue(m[1]);
+ v_[2].setValue(m[2]);
}
IMatrix3::IMatrix3(Interval m[3][3])
{
- 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];
+ v_[0].setValue(m[0]);
+ v_[1].setValue(m[1]);
+ v_[2].setValue(m[2]);
+}
- i_[2][0] = m[2][0];
- i_[2][1] = m[2][1];
- i_[2][2] = m[2][2];
+IMatrix3::IMatrix3(const IVector3& v1, const IVector3& v2, const IVector3& v3)
+{
+ v_[0] = v1;
+ v_[1] = v2;
+ v_[2] = v3;
}
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;
+ v_[0].setValue(1, 0, 0);
+ v_[1].setValue(0, 1, 0);
+ v_[2].setValue(0, 0, 1);
}
IVector3 IMatrix3::getColumn(size_t i) const
{
- return IVector3(i_[0][i], i_[1][i], i_[2][i]);
+ return IVector3(v_[0][i], v_[1][i], v_[2][i]);
}
-IVector3 IMatrix3::getRow(size_t i) const
+const IVector3& IMatrix3::getRow(size_t i) const
{
- return IVector3(i_[i][0], i_[i][1], i_[i][2]);
+ return v_[i];
}
-Vec3f IMatrix3::getRealColumn(size_t i) const
+Vec3f IMatrix3::getColumnLow(size_t i) const
{
- return Vec3f(i_[0][i][0], i_[1][i][0], i_[2][i][0]);
+ return Vec3f(v_[0][i][0], v_[1][i][0], v_[2][i][0]);
}
-Vec3f IMatrix3::getRealRow(size_t i) const
+Vec3f IMatrix3::getRowLow(size_t i) const
{
- return Vec3f(i_[i][0][0], i_[i][1][0], i_[i][2][0]);
+ return Vec3f(v_[i][0][0], v_[i][1][0], v_[i][2][0]);
}
-IMatrix3 IMatrix3::operator * (const Matrix3f& m) const
+Vec3f IMatrix3::getColumnHigh(size_t i) const
{
- FCL_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);
+ return Vec3f(v_[0][i][1], v_[1][i][1], v_[2][i][1]);
}
-IVector3 IMatrix3::operator * (const Vec3f& v) const
+Vec3f IMatrix3::getRowHigh(size_t i) const
{
- FCL_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);
+ return Vec3f(v_[i][0][1], v_[i][1][1], v_[i][2][1]);
}
-
-IMatrix3 IMatrix3::nonIntervalAddMatrix(const IMatrix3& m) const
+Matrix3f IMatrix3::getLow() const
{
- FCL_REAL res[3][3];
+ return Matrix3f(v_[0][0][0], v_[0][1][0], v_[0][2][0],
+ v_[1][0][0], v_[1][1][0], v_[1][2][0],
+ v_[2][0][0], v_[2][1][0], v_[2][2][0]);
+}
- 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];
+Matrix3f IMatrix3::getHigh() const
+{
+ return Matrix3f(v_[0][0][1], v_[0][1][1], v_[0][2][1],
+ v_[1][0][1], v_[1][1][1], v_[1][2][1],
+ v_[2][0][1], v_[2][1][1], v_[2][2][1]);
+}
- 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];
+IMatrix3 IMatrix3::operator * (const Matrix3f& m) const
+{
+ const Vec3f& mc0 = m.getColumn(0);
+ const Vec3f& mc1 = m.getColumn(1);
+ const Vec3f& mc2 = m.getColumn(2);
- 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(IVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
+ IVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
+ IVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
+}
- return IMatrix3(res);
+IVector3 IMatrix3::operator * (const Vec3f& v) const
+{
+ return IVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
}
IVector3 IMatrix3::operator * (const IVector3& v) const
{
- FCL_REAL xl, xu, yl, yu, zl, zu;
- register FCL_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);
+ return IVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
}
IMatrix3 IMatrix3::operator * (const IMatrix3& m) const
{
+ const IVector3& mc0 = m.getColumn(0);
+ const IVector3& mc1 = m.getColumn(1);
+ const IVector3& mc2 = m.getColumn(2);
- register FCL_REAL temp, vmin, vmax;
- FCL_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);
+ return IMatrix3(IVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
+ IVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
+ IVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
}
-IMatrix3 IMatrix3::operator + (const IMatrix3& m) const
+IMatrix3& IMatrix3::operator *= (const Matrix3f& m)
{
- FCL_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];
+ const Vec3f& mc0 = m.getColumn(0);
+ const Vec3f& mc1 = m.getColumn(1);
+ const Vec3f& mc2 = m.getColumn(2);
- return IMatrix3(res);
+ v_[0].setValue(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
+ v_[1].setValue(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
+ v_[2].setValue(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
+ return *this;
}
-IMatrix3& IMatrix3::operator += (const IMatrix3& m)
+
+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];
+ const IVector3& mc0 = m.getColumn(0);
+ const IVector3& mc1 = m.getColumn(1);
+ const IVector3& mc2 = m.getColumn(2);
+ v_[0].setValue(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
+ v_[1].setValue(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
+ v_[2].setValue(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
return *this;
}
-IVector3 IMatrix3::nonIntervalTimesVector(const Vec3f& v) const
+IMatrix3 IMatrix3::operator + (const IMatrix3& m) 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]);
+ return IMatrix3(v_[0] + m.v_[0], v_[1] + m.v_[1], v_[2] + m.v_[2]);
}
-IVector3 IMatrix3::nonIntervalTimesVector(const IVector3& v) const
+IMatrix3& IMatrix3::operator += (const IMatrix3& m)
{
- 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]);
+ v_[0] += m.v_[0];
+ v_[1] += m.v_[1];
+ v_[2] += m.v_[2];
+ return *this;
}
-const Interval& IMatrix3::operator () (size_t i, size_t j) const
+IMatrix3 IMatrix3::operator - (const IMatrix3& m) const
{
- return i_[i][j];
+ return IMatrix3(v_[0] - m.v_[0], v_[1] - m.v_[1], v_[2] - m.v_[2]);
}
-Interval& IMatrix3::operator () (size_t i, size_t j)
+IMatrix3& IMatrix3::operator -= (const IMatrix3& m)
{
- return i_[i][j];
+ v_[0] -= m.v_[0];
+ v_[1] -= m.v_[1];
+ v_[2] -= m.v_[2];
+ return *this;
}
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;
+ std::cout << "[" << v_[0][0][0] << "," << v_[0][0][1] << "]" << " [" << v_[0][1][0] << "," << v_[0][1][1] << "]" << " [" << v_[0][2][0] << "," << v_[0][2][1] << "]" << std::endl;
+ std::cout << "[" << v_[1][0][0] << "," << v_[1][0][1] << "]" << " [" << v_[1][1][0] << "," << v_[1][1][1] << "]" << " [" << v_[1][2][0] << "," << v_[1][2][1] << "]" << std::endl;
+ std::cout << "[" << v_[2][0][0] << "," << v_[2][0][1] << "]" << " [" << v_[2][1][0] << "," << v_[2][1][1] << "]" << " [" << v_[2][2][0] << "," << v_[2][2][1] << "]" << std::endl;
}
}
diff --git a/trunk/fcl/src/ccd/interval_vector.cpp b/trunk/fcl/src/ccd/interval_vector.cpp
index 4c875e28..0147a09b 100644
--- a/trunk/fcl/src/ccd/interval_vector.cpp
+++ b/trunk/fcl/src/ccd/interval_vector.cpp
@@ -41,66 +41,30 @@ namespace fcl
{
-IVector3::IVector3() {}
+IVector3::IVector3() {}
-IVector3::IVector3(FCL_REAL v) { i_[0] = i_[1] = i_[2] = v; }
+IVector3::IVector3(FCL_REAL v) { setValue(v); }
-IVector3::IVector3(FCL_REAL x, FCL_REAL y, FCL_REAL z)
-{
- i_[0].setValue(x);
- i_[1].setValue(y);
- i_[2].setValue(z);
-}
+IVector3::IVector3(FCL_REAL x, FCL_REAL y, FCL_REAL z) { setValue(x, y, z); }
IVector3::IVector3(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu)
{
- i_[0].setValue(xl, xu);
- i_[1].setValue(yl, yu);
- i_[2].setValue(zl, zu);
+ setValue(xl, xu, yl, yu, zl, zu);
}
-IVector3::IVector3(FCL_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(FCL_REAL v[3][2]) { setValue(v); }
-IVector3::IVector3(Interval v[3])
-{
- i_[0] = v[0];
- i_[1] = v[1];
- i_[2] = v[2];
-}
+IVector3::IVector3(Interval v[3]) { setValue(v); }
-IVector3::IVector3(const Interval& v1, const Interval& v2, const Interval& v3)
-{
- i_[0] = v1;
- i_[1] = v2;
- i_[2] = v3;
-}
+IVector3::IVector3(const Interval& v1, const Interval& v2, const Interval& v3) { setValue(v1, v2, v3); }
-IVector3::IVector3(const Vec3f& v)
-{
- i_[0].setValue(v[0]);
- i_[1].setValue(v[1]);
- i_[2].setValue(v[2]);
-}
+IVector3::IVector3(const Vec3f& v) { setValue(v); }
-void IVector3::setZero()
-{
- i_[0].setValue(0);
- i_[1].setValue(0);
- i_[2].setValue(0);
-}
+void IVector3::setZero() { setValue((FCL_REAL)0.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);
+ return IVector3(i_[0] + other.i_[0], i_[1] + other.i_[1], i_[2] + other.i_[2]);
}
IVector3& IVector3::operator += (const IVector3& other)
@@ -113,11 +77,7 @@ IVector3& IVector3::operator += (const IVector3& other)
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);
+ return IVector3(i_[0] - other.i_[0], i_[1] - other.i_[1], i_[2] - other.i_[2]);
}
IVector3& IVector3::operator -= (const IVector3& other)
@@ -130,16 +90,26 @@ IVector3& IVector3::operator -= (const IVector3& other)
Interval IVector3::dot(const IVector3& other) const
{
- return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2];
+ return i_[0] * other.i_[0] + i_[1] * other.i_[1] + i_[2] * other.i_[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);
+ return IVector3(i_[1] * other.i_[2] - i_[2] * other.i_[1],
+ i_[2] * other.i_[0] - i_[0] * other.i_[2],
+ i_[0] * other.i_[1] - i_[1] * other.i_[0]);
+}
+
+Interval IVector3::dot(const Vec3f& other) const
+{
+ return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2];
+}
+
+IVector3 IVector3::cross(const Vec3f& other) const
+{
+ return IVector3(i_[1] * other[2] - i_[2] * other[1],
+ i_[2] * other[0] - i_[0] * other[2],
+ i_[0] * other[1] - i_[1] * other[0]);
}
FCL_REAL IVector3::volumn() const
@@ -182,9 +152,9 @@ void IVector3::bound(const Vec3f& v)
}
-IVector3 IVector3::bounded(const IVector3& v) const
+IVector3 bound(const IVector3& i, const IVector3& v)
{
- IVector3 res = *this;
+ IVector3 res(i);
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];
@@ -196,9 +166,9 @@ IVector3 IVector3::bounded(const IVector3& v) const
return res;
}
-IVector3 IVector3::bounded(const Vec3f& v) const
+IVector3 bound(const IVector3& i, const Vec3f& v)
{
- IVector3 res = *this;
+ IVector3 res(i);
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];
@@ -236,4 +206,6 @@ bool IVector3::contain(const IVector3& v) const
return true;
}
+
+
}
diff --git a/trunk/fcl/src/ccd/taylor_matrix.cpp b/trunk/fcl/src/ccd/taylor_matrix.cpp
index 264d1651..d282b979 100644
--- a/trunk/fcl/src/ccd/taylor_matrix.cpp
+++ b/trunk/fcl/src/ccd/taylor_matrix.cpp
@@ -39,292 +39,180 @@
namespace fcl
{
-TMatrix3::TMatrix3() {}
-TMatrix3::TMatrix3(TaylorModel m[3][3])
+TMatrix3::TMatrix3()
{
- 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];
}
+TMatrix3::TMatrix3(const boost::shared_ptr<TimeInterval>& time_interval)
+{
+ setTimeInterval(time_interval);
+}
-TMatrix3::TMatrix3(const Matrix3f& m)
+TMatrix3::TMatrix3(TaylorModel m[3][3])
{
- setZero();
- i_[0][0].coeffs_[0] = m(0, 0);
- i_[0][1].coeffs_[0] = m(0, 1);
- i_[0][2].coeffs_[0] = m(0, 2);
+ v_[0] = TVector3(m[0]);
+ v_[1] = TVector3(m[1]);
+ v_[2] = TVector3(m[2]);
+}
+
+TMatrix3::TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3)
+{
+ v_[0] = v1;
+ v_[1] = v2;
+ v_[2] = v3;
+}
- i_[1][0].coeffs_[0] = m(1, 0);
- i_[1][1].coeffs_[0] = m(1, 1);
- i_[1][2].coeffs_[0] = m(1, 2);
- i_[2][0].coeffs_[0] = m(2, 0);
- i_[2][1].coeffs_[0] = m(2, 1);
- i_[2][2].coeffs_[0] = m(2, 2);
+TMatrix3::TMatrix3(const Matrix3f& m, const boost::shared_ptr<TimeInterval>& time_interval)
+{
+ v_[0] = TVector3(m.getRow(0), time_interval);
+ v_[1] = TVector3(m.getRow(1), time_interval);
+ v_[2] = TVector3(m.getRow(2), time_interval);
}
void TMatrix3::setIdentity()
{
setZero();
- i_[0][0].coeffs_[0] = 1;
- i_[1][0].coeffs_[0] = 1;
- i_[2][2].coeffs_[0] = 1;
+ v_[0][0].coeffs_[0] = 1;
+ v_[1][1].coeffs_[0] = 1;
+ v_[2][2].coeffs_[0] = 1;
}
void TMatrix3::setZero()
{
- i_[0][0].setZero();
- i_[0][1].setZero();
- i_[0][2].setZero();
- i_[1][0].setZero();
- i_[1][1].setZero();
- i_[1][2].setZero();
- i_[2][0].setZero();
- i_[2][1].setZero();
- i_[2][2].setZero();
+ v_[0].setZero();
+ v_[1].setZero();
+ v_[2].setZero();
}
TVector3 TMatrix3::getColumn(size_t i) const
{
- return TVector3(i_[0][i], i_[1][i], i_[2][i]);
+ return TVector3(v_[0][i], v_[1][i], v_[2][i]);
}
-TVector3 TMatrix3::getRow(size_t i) const
+const TVector3& TMatrix3::getRow(size_t i) const
{
- return TVector3(i_[i][0], i_[i][1], i_[i][2]);
+ return v_[i];
}
-const TaylorModel& TMatrix3::operator () (size_t i, size_t j) const
+const TVector3& TMatrix3::operator [] (size_t i) const
{
- return i_[i][j];
+ return v_[i];
}
-TaylorModel& TMatrix3::operator () (size_t i, size_t j)
+TVector3& TMatrix3::operator [] (size_t i)
{
- return i_[i][j];
+ return v_[i];
}
TMatrix3 TMatrix3::operator * (const Matrix3f& m) const
{
- TaylorModel res[3][3];
- register FCL_REAL temp;
-
- temp = m[0][0];
- res[0][0].coeffs_[0] = i_[0][0].coeffs_[0] * temp; res[0][0].coeffs_[1] = i_[0][0].coeffs_[1] * temp; res[0][0].coeffs_[2] = i_[0][0].coeffs_[2] * temp; res[0][0].coeffs_[3] = i_[0][0].coeffs_[3] * temp; res[1][0].coeffs_[0] = i_[1][0].coeffs_[0] * temp; res[1][0].coeffs_[1] = i_[1][0].coeffs_[1] * temp; res[1][0].coeffs_[2] = i_[1][0].coeffs_[2] * temp; res[1][0].coeffs_[3] = i_[1][0].coeffs_[3] * temp; res[2][0].coeffs_[0] = i_[2][0].coeffs_[0] * temp; res[2][0].coeffs_[1] = i_[2][0].coeffs_[1] * temp; res[2][0].coeffs_[2] = i_[2][0].coeffs_[2] * temp; res[2][0].coeffs_[3] = i_[2][0].coeffs_[3] * temp;
- if(temp >= 0)
- {
- res[0][0].r_[0] = i_[0][0].r_[0] * temp; res[0][0].r_[1] = i_[0][0].r_[1] * temp; res[1][0].r_[0] = i_[1][0].r_[0] * temp; res[1][0].r_[1] = i_[1][0].r_[1] * temp; res[2][0].r_[0] = i_[2][0].r_[0] * temp; res[2][0].r_[1] = i_[2][0].r_[1] * temp;
- }
- else
- {
- res[0][0].r_[0] = i_[0][0].r_[1] * temp; res[0][0].r_[1] = i_[0][0].r_[0] * temp; res[1][0].r_[0] = i_[1][0].r_[1] * temp; res[1][0].r_[1] = i_[1][0].r_[0] * temp; res[2][0].r_[0] = i_[2][0].r_[1] * temp; res[2][0].r_[1] = i_[2][0].r_[0] * temp;
- }
-
- temp = m[0][1];
- res[0][1].coeffs_[0] = i_[0][0].coeffs_[0] * temp; res[0][1].coeffs_[1] = i_[0][0].coeffs_[1] * temp; res[0][1].coeffs_[2] = i_[0][0].coeffs_[2] * temp; res[0][1].coeffs_[3] = i_[0][0].coeffs_[3] * temp; res[1][1].coeffs_[0] = i_[1][0].coeffs_[0] * temp; res[1][1].coeffs_[1] = i_[1][0].coeffs_[1] * temp; res[1][1].coeffs_[2] = i_[1][0].coeffs_[2] * temp; res[1][1].coeffs_[3] = i_[1][0].coeffs_[3] * temp; res[2][1].coeffs_[0] = i_[2][0].coeffs_[0] * temp; res[2][1].coeffs_[1] = i_[2][0].coeffs_[1] * temp; res[2][1].coeffs_[2] = i_[2][0].coeffs_[2] * temp; res[2][1].coeffs_[3] = i_[2][0].coeffs_[3] * temp;
- if(temp >= 0)
- {
- res[0][1].r_[0] = i_[0][0].r_[0] * temp; res[0][1].r_[1] = i_[0][0].r_[1] * temp; res[1][1].r_[0] = i_[1][0].r_[0] * temp; res[1][1].r_[1] = i_[1][0].r_[1] * temp; res[2][1].r_[0] = i_[2][0].r_[0] * temp; res[2][1].r_[1] = i_[2][0].r_[1] * temp;
- }
- else
- {
- res[0][1].r_[0] = i_[0][0].r_[1] * temp; res[0][1].r_[1] = i_[0][0].r_[0] * temp; res[1][1].r_[0] = i_[1][0].r_[1] * temp; res[1][1].r_[1] = i_[1][0].r_[0] * temp; res[2][1].r_[0] = i_[2][0].r_[1] * temp; res[2][1].r_[1] = i_[2][0].r_[0] * temp;
- }
-
- temp = m[0][2];
- res[0][2].coeffs_[0] = i_[0][0].coeffs_[0] * temp; res[0][2].coeffs_[1] = i_[0][0].coeffs_[1] * temp; res[0][2].coeffs_[2] = i_[0][0].coeffs_[2] * temp; res[0][2].coeffs_[3] = i_[0][0].coeffs_[3] * temp; res[1][2].coeffs_[0] = i_[1][0].coeffs_[0] * temp; res[1][2].coeffs_[1] = i_[1][0].coeffs_[1] * temp; res[1][2].coeffs_[2] = i_[1][0].coeffs_[2] * temp; res[1][2].coeffs_[3] = i_[1][0].coeffs_[3] * temp; res[2][2].coeffs_[0] = i_[2][0].coeffs_[0] * temp; res[2][2].coeffs_[1] = i_[2][0].coeffs_[1] * temp; res[2][2].coeffs_[2] = i_[2][0].coeffs_[2] * temp; res[2][2].coeffs_[3] = i_[2][0].coeffs_[3] * temp;
- if(temp >= 0)
- {
- res[0][2].r_[0] = i_[0][0].r_[0] * temp; res[0][2].r_[1] = i_[0][0].r_[1] * temp; res[1][2].r_[0] = i_[1][0].r_[0] * temp; res[1][2].r_[1] = i_[1][0].r_[1] * temp; res[2][2].r_[0] = i_[2][0].r_[0] * temp; res[2][2].r_[1] = i_[2][0].r_[1] * temp;
- }
- else
- {
- res[0][2].r_[0] = i_[0][0].r_[1] * temp; res[0][2].r_[1] = i_[0][0].r_[0] * temp; res[1][2].r_[0] = i_[1][0].r_[1] * temp; res[1][2].r_[1] = i_[1][0].r_[0] * temp; res[2][2].r_[0] = i_[2][0].r_[1] * temp; res[2][2].r_[1] = i_[2][0].r_[0] * temp;
- }
-
- temp = m[1][0];
- res[0][0].coeffs_[0] += i_[0][1].coeffs_[0] * temp; res[0][0].coeffs_[1] += i_[0][1].coeffs_[1] * temp; res[0][0].coeffs_[2] += i_[0][1].coeffs_[2] * temp; res[0][0].coeffs_[3] += i_[0][1].coeffs_[3] * temp; res[1][0].coeffs_[0] += i_[1][1].coeffs_[0] * temp; res[1][0].coeffs_[1] += i_[1][1].coeffs_[1] * temp; res[1][0].coeffs_[2] += i_[1][1].coeffs_[2] * temp; res[1][0].coeffs_[3] += i_[1][1].coeffs_[3] * temp; res[2][0].coeffs_[0] += i_[2][1].coeffs_[0] * temp; res[2][0].coeffs_[1] += i_[2][1].coeffs_[1] * temp; res[2][0].coeffs_[2] += i_[2][1].coeffs_[2] * temp; res[2][0].coeffs_[3] += i_[2][1].coeffs_[3] * temp;
- if(temp >= 0)
- {
- res[0][0].r_[0] += i_[0][1].r_[0] * temp; res[0][0].r_[1] += i_[0][1].r_[1] * temp; res[1][0].r_[0] += i_[1][1].r_[0] * temp; res[1][0].r_[1] += i_[1][1].r_[1] * temp; res[2][0].r_[0] += i_[2][1].r_[0] * temp; res[2][0].r_[1] += i_[2][1].r_[1] * temp;
- }
- else
- {
- res[0][0].r_[0] += i_[0][1].r_[1] * temp; res[0][0].r_[1] += i_[0][1].r_[0] * temp; res[1][0].r_[0] += i_[1][1].r_[1] * temp; res[1][0].r_[1] += i_[1][1].r_[0] * temp; res[2][0].r_[0] += i_[2][1].r_[1] * temp; res[2][0].r_[1] += i_[2][1].r_[0] * temp;
- }
-
- temp = m[1][1];
- res[0][1].coeffs_[0] += i_[0][1].coeffs_[0] * temp; res[0][1].coeffs_[1] += i_[0][1].coeffs_[1] * temp; res[0][1].coeffs_[2] += i_[0][1].coeffs_[2] * temp; res[0][1].coeffs_[3] += i_[0][1].coeffs_[3] * temp; res[1][1].coeffs_[0] += i_[1][1].coeffs_[0] * temp; res[1][1].coeffs_[1] += i_[1][1].coeffs_[1] * temp; res[1][1].coeffs_[2] += i_[1][1].coeffs_[2] * temp; res[1][1].coeffs_[3] += i_[1][1].coeffs_[3] * temp; res[2][1].coeffs_[0] += i_[2][1].coeffs_[0] * temp; res[2][1].coeffs_[1] += i_[2][1].coeffs_[1] * temp; res[2][1].coeffs_[2] += i_[2][1].coeffs_[2] * temp; res[2][1].coeffs_[3] += i_[2][1].coeffs_[3] * temp;
- if(temp >= 0)
- {
- res[0][1].r_[0] += i_[0][1].r_[0] * temp; res[0][1].r_[1] += i_[0][1].r_[1] * temp; res[1][1].r_[0] += i_[1][1].r_[0] * temp; res[1][1].r_[1] += i_[1][1].r_[1] * temp; res[2][1].r_[0] += i_[2][1].r_[0] * temp; res[2][1].r_[1] += i_[2][1].r_[1] * temp;
- }
- else
- {
- res[0][1].r_[0] += i_[0][1].r_[1] * temp; res[0][1].r_[1] += i_[0][1].r_[0] * temp; res[1][1].r_[0] += i_[1][1].r_[1] * temp; res[1][1].r_[1] += i_[1][1].r_[0] * temp; res[2][1].r_[0] += i_[2][1].r_[1] * temp; res[2][1].r_[1] += i_[2][1].r_[0] * temp;
- }
-
- temp = m[1][2];
- res[0][2].coeffs_[0] += i_[0][1].coeffs_[0] * temp; res[0][2].coeffs_[1] += i_[0][1].coeffs_[1] * temp; res[0][2].coeffs_[2] += i_[0][1].coeffs_[2] * temp; res[0][2].coeffs_[3] += i_[0][1].coeffs_[3] * temp; res[1][2].coeffs_[0] += i_[1][1].coeffs_[0] * temp; res[1][2].coeffs_[1] += i_[1][1].coeffs_[1] * temp; res[1][2].coeffs_[2] += i_[1][1].coeffs_[2] * temp; res[1][2].coeffs_[3] += i_[1][1].coeffs_[3] * temp; res[2][2].coeffs_[0] += i_[2][1].coeffs_[0] * temp; res[2][2].coeffs_[1] += i_[2][1].coeffs_[1] * temp; res[2][2].coeffs_[2] += i_[2][1].coeffs_[2] * temp; res[2][2].coeffs_[3] += i_[2][1].coeffs_[3] * temp;
- if(temp >= 0)
- {
- res[0][2].r_[0] += i_[0][1].r_[0] * temp; res[0][2].r_[1] += i_[0][1].r_[1] * temp; res[1][2].r_[0] += i_[1][1].r_[0] * temp; res[1][2].r_[1] += i_[1][1].r_[1] * temp; res[2][2].r_[0] += i_[2][1].r_[0] * temp; res[2][2].r_[1] += i_[2][1].r_[1] * temp;
- }
- else
- {
- res[0][2].r_[0] += i_[0][1].r_[1] * temp; res[0][2].r_[1] += i_[0][1].r_[0] * temp; res[1][2].r_[0] += i_[1][1].r_[1] * temp; res[1][2].r_[1] += i_[1][1].r_[0] * temp; res[2][2].r_[0] += i_[2][1].r_[1] * temp; res[2][2].r_[1] += i_[2][1].r_[0] * temp;
- }
-
- temp = m[2][0];
- res[0][0].coeffs_[0] += i_[0][2].coeffs_[0] * temp; res[0][0].coeffs_[1] += i_[0][2].coeffs_[1] * temp; res[0][0].coeffs_[2] += i_[0][2].coeffs_[2] * temp; res[0][0].coeffs_[3] += i_[0][2].coeffs_[3] * temp; res[1][0].coeffs_[0] += i_[1][2].coeffs_[0] * temp; res[1][0].coeffs_[1] += i_[1][2].coeffs_[1] * temp; res[1][0].coeffs_[2] += i_[1][2].coeffs_[2] * temp; res[1][0].coeffs_[3] += i_[1][2].coeffs_[3] * temp; res[2][0].coeffs_[0] += i_[2][2].coeffs_[0] * temp; res[2][0].coeffs_[1] += i_[2][2].coeffs_[1] * temp; res[2][0].coeffs_[2] += i_[2][2].coeffs_[2] * temp; res[2][0].coeffs_[3] += i_[2][2].coeffs_[3] * temp;
- if(temp >= 0)
- {
- res[0][0].r_[0] += i_[0][2].r_[0] * temp; res[0][0].r_[1] += i_[0][2].r_[1] * temp; res[1][0].r_[0] += i_[1][2].r_[0] * temp; res[1][0].r_[1] += i_[1][2].r_[1] * temp; res[2][0].r_[0] += i_[2][2].r_[0] * temp; res[2][0].r_[1] += i_[2][2].r_[1] * temp;
- }
- else
- {
- res[0][0].r_[0] += i_[0][2].r_[1] * temp; res[0][0].r_[1] += i_[0][2].r_[0] * temp; res[1][0].r_[0] += i_[1][2].r_[1] * temp; res[1][0].r_[1] += i_[1][2].r_[0] * temp; res[2][0].r_[0] += i_[2][2].r_[1] * temp; res[2][0].r_[1] += i_[2][2].r_[0] * temp;
- }
-
- temp = m[2][1];
- res[0][1].coeffs_[0] += i_[0][2].coeffs_[0] * temp; res[0][1].coeffs_[1] += i_[0][2].coeffs_[1] * temp; res[0][1].coeffs_[2] += i_[0][2].coeffs_[2] * temp; res[0][1].coeffs_[3] += i_[0][2].coeffs_[3] * temp; res[1][1].coeffs_[0] += i_[1][2].coeffs_[0] * temp; res[1][1].coeffs_[1] += i_[1][2].coeffs_[1] * temp; res[1][1].coeffs_[2] += i_[1][2].coeffs_[2] * temp; res[1][1].coeffs_[3] += i_[1][2].coeffs_[3] * temp; res[2][1].coeffs_[0] += i_[2][2].coeffs_[0] * temp; res[2][1].coeffs_[1] += i_[2][2].coeffs_[1] * temp; res[2][1].coeffs_[2] += i_[2][2].coeffs_[2] * temp; res[2][1].coeffs_[3] += i_[2][2].coeffs_[3] * temp;
- if(temp >= 0)
- {
- res[0][1].r_[0] += i_[0][2].r_[0] * temp; res[0][1].r_[1] += i_[0][2].r_[1] * temp; res[1][1].r_[0] += i_[1][2].r_[0] * temp; res[1][1].r_[1] += i_[1][2].r_[1] * temp; res[2][1].r_[0] += i_[2][2].r_[0] * temp; res[2][1].r_[1] += i_[2][2].r_[1] * temp;
- }
- else
- {
- res[0][1].r_[0] += i_[0][2].r_[1] * temp; res[0][1].r_[1] += i_[0][2].r_[0] * temp; res[1][1].r_[0] += i_[1][2].r_[1] * temp; res[1][1].r_[1] += i_[1][2].r_[0] * temp; res[2][1].r_[0] += i_[2][2].r_[1] * temp; res[2][1].r_[1] += i_[2][2].r_[0] * temp;
- }
-
- temp = m[2][2];
- res[0][2].coeffs_[0] += i_[0][2].coeffs_[0] * temp; res[0][2].coeffs_[1] += i_[0][2].coeffs_[1] * temp; res[0][2].coeffs_[2] += i_[0][2].coeffs_[2] * temp; res[0][2].coeffs_[3] += i_[0][2].coeffs_[3] * temp; res[1][2].coeffs_[0] += i_[1][2].coeffs_[0] * temp; res[1][2].coeffs_[1] += i_[1][2].coeffs_[1] * temp; res[1][2].coeffs_[2] += i_[1][2].coeffs_[2] * temp; res[1][2].coeffs_[3] += i_[1][2].coeffs_[3] * temp; res[2][2].coeffs_[0] += i_[2][2].coeffs_[0] * temp; res[2][2].coeffs_[1] += i_[2][2].coeffs_[1] * temp; res[2][2].coeffs_[2] += i_[2][2].coeffs_[2] * temp; res[2][2].coeffs_[3] += i_[2][2].coeffs_[3] * temp;
- if(temp >= 0)
- {
- res[0][2].r_[0] += i_[0][2].r_[0] * temp; res[0][2].r_[1] += i_[0][2].r_[1] * temp; res[1][2].r_[0] += i_[1][2].r_[0] * temp; res[1][2].r_[1] += i_[1][2].r_[1] * temp; res[2][2].r_[0] += i_[2][2].r_[0] * temp; res[2][2].r_[1] += i_[2][2].r_[1] * temp;
- }
- else
- {
- res[0][2].r_[0] += i_[0][2].r_[1] * temp; res[0][2].r_[1] += i_[0][2].r_[0] * temp; res[1][2].r_[0] += i_[1][2].r_[1] * temp; res[1][2].r_[1] += i_[1][2].r_[0] * temp; res[2][2].r_[0] += i_[2][2].r_[1] * temp; res[2][2].r_[1] += i_[2][2].r_[0] * temp;
- }
-
- return TMatrix3(res);
+ const Vec3f& mc0 = m.getColumn(0);
+ const Vec3f& mc1 = m.getColumn(1);
+ const Vec3f& mc2 = m.getColumn(2);
+
+ return TMatrix3(TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
+ TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
+ TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
}
TVector3 TMatrix3::operator * (const Vec3f& v) const
{
- TaylorModel res[3];
-
- register FCL_REAL temp;
-
- temp = v[0];
- res[0].coeffs_[0] = i_[0][0].coeffs_[0] * temp; res[0].coeffs_[1] = i_[0][0].coeffs_[1] * temp; res[0].coeffs_[2] = i_[0][0].coeffs_[2] * temp; res[0].coeffs_[3] = i_[0][0].coeffs_[3] * temp; res[1].coeffs_[0] = i_[1][0].coeffs_[0] * temp; res[1].coeffs_[1] = i_[1][0].coeffs_[1] * temp; res[1].coeffs_[2] = i_[1][0].coeffs_[2] * temp; res[1].coeffs_[3] = i_[1][0].coeffs_[3] * temp; res[2].coeffs_[0] = i_[2][0].coeffs_[0] * temp; res[2].coeffs_[1] = i_[2][0].coeffs_[1] * temp; res[2].coeffs_[2] = i_[2][0].coeffs_[2] * temp; res[2].coeffs_[3] = i_[2][0].coeffs_[3] * temp;
- if(temp >= 0)
- {
- res[0].r_[0] = i_[0][0].r_[0] * temp; res[0].r_[1] = i_[0][0].r_[1] * temp; res[1].r_[0] = i_[1][0].r_[0] * temp; res[1].r_[1] = i_[1][0].r_[1] * temp; res[2].r_[0] = i_[2][0].r_[0] * temp; res[2].r_[1] = i_[2][0].r_[1] * temp;
- }
- else
- {
- res[0].r_[0] = i_[0][0].r_[1] * temp; res[0].r_[1] = i_[0][0].r_[0] * temp; res[1].r_[0] = i_[1][0].r_[1] * temp; res[1].r_[1] = i_[1][0].r_[0] * temp; res[2].r_[0] = i_[2][0].r_[1] * temp; res[2].r_[1] = i_[2][0].r_[0] * temp;
- }
-
- temp = v[1];
- res[0].coeffs_[0] += i_[0][1].coeffs_[0] * temp; res[0].coeffs_[1] += i_[0][1].coeffs_[1] * temp; res[0].coeffs_[2] += i_[0][1].coeffs_[2] * temp; res[0].coeffs_[3] += i_[0][1].coeffs_[3] * temp; res[1].coeffs_[0] += i_[1][1].coeffs_[0] * temp; res[1].coeffs_[1] += i_[1][1].coeffs_[1] * temp; res[1].coeffs_[2] += i_[1][1].coeffs_[2] * temp; res[1].coeffs_[3] += i_[1][1].coeffs_[3] * temp; res[2].coeffs_[0] += i_[2][1].coeffs_[0] * temp; res[2].coeffs_[1] += i_[2][1].coeffs_[1] * temp; res[2].coeffs_[2] += i_[2][1].coeffs_[2] * temp; res[2].coeffs_[3] += i_[2][1].coeffs_[3] * temp;
- if(temp >= 0)
- {
- res[0].r_[0] += i_[0][1].r_[0] * temp; res[0].r_[1] += i_[0][1].r_[1] * temp; res[1].r_[0] += i_[1][1].r_[0] * temp; res[1].r_[1] += i_[1][1].r_[1] * temp; res[2].r_[0] += i_[2][1].r_[0] * temp; res[2].r_[1] += i_[2][1].r_[1] * temp;
- }
- else
- {
- res[0].r_[0] += i_[0][1].r_[1] * temp; res[0].r_[1] += i_[0][1].r_[0] * temp; res[1].r_[0] += i_[1][1].r_[1] * temp; res[1].r_[1] += i_[1][1].r_[0] * temp; res[2].r_[0] += i_[2][1].r_[1] * temp; res[2].r_[1] += i_[2][1].r_[0] * temp; \
- }
-
- temp = v[2];
- res[0].coeffs_[0] += i_[0][2].coeffs_[0] * temp; res[0].coeffs_[1] += i_[0][2].coeffs_[1] * temp; res[0].coeffs_[2] += i_[0][2].coeffs_[2] * temp; res[0].coeffs_[3] += i_[0][2].coeffs_[3] * temp; res[1].coeffs_[0] += i_[1][2].coeffs_[0] * temp; res[1].coeffs_[1] += i_[1][2].coeffs_[1] * temp; res[1].coeffs_[2] += i_[1][2].coeffs_[2] * temp; res[1].coeffs_[3] += i_[1][2].coeffs_[3] * temp; res[2].coeffs_[0] += i_[2][2].coeffs_[0] * temp; res[2].coeffs_[1] += i_[2][2].coeffs_[1] * temp; res[2].coeffs_[2] += i_[2][2].coeffs_[2] * temp; res[2].coeffs_[3] += i_[2][2].coeffs_[3] * temp;
- if(temp >= 0)
- {
- res[0].r_[0] += i_[0][2].r_[0] * temp; res[0].r_[1] += i_[0][2].r_[1] * temp; res[1].r_[0] += i_[1][2].r_[0] * temp; res[1].r_[1] += i_[1][2].r_[1] * temp; res[2].r_[0] += i_[2][2].r_[0] * temp; res[2].r_[1] += i_[2][2].r_[1] * temp;
- }
- else
- {
- res[0].r_[0] += i_[0][2].r_[1] * temp; res[0].r_[1] += i_[0][2].r_[0] * temp; res[1].r_[0] += i_[1][2].r_[1] * temp; res[1].r_[1] += i_[1][2].r_[0] * temp; res[2].r_[0] += i_[2][2].r_[1] * temp; res[2].r_[1] += i_[2][2].r_[0] * temp;
- }
-
- return TVector3(res);
+ return TVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
}
TVector3 TMatrix3::operator * (const TVector3& v) const
{
- TaylorModel res[3];
-
- res[0] = i_[0][0] * v[0] + i_[0][1] * v[1] + i_[0][2] * v[2];
- res[1] = i_[1][0] * v[0] + i_[1][1] * v[1] + i_[1][2] * v[2];
- res[2] = i_[2][0] * v[0] + i_[2][1] * v[1] + i_[2][2] * v[2];
-
- return TVector3(res);
+ return TVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
}
TMatrix3 TMatrix3::operator * (const TMatrix3& m) const
{
- TaylorModel res[3][3];
-
- res[0][0] = i_[0][0] * m.i_[0][0] + i_[0][1] * m.i_[1][0] + i_[0][2] * m.i_[2][0];
- res[0][1] = i_[0][0] * m.i_[0][1] + i_[0][1] * m.i_[1][1] + i_[0][2] * m.i_[2][1];
- res[0][2] = i_[0][0] * m.i_[0][2] + i_[0][1] * m.i_[1][2] + i_[0][2] * m.i_[2][2];
+ const TVector3& mc0 = m.getColumn(0);
+ const TVector3& mc1 = m.getColumn(1);
+ const TVector3& mc2 = m.getColumn(2);
- res[1][0] = i_[1][0] * m.i_[0][0] + i_[1][1] * m.i_[1][0] + i_[1][2] * m.i_[2][0];
- res[1][1] = i_[1][0] * m.i_[0][1] + i_[1][1] * m.i_[1][1] + i_[1][2] * m.i_[2][1];
- res[1][2] = i_[1][0] * m.i_[0][2] + i_[1][1] * m.i_[1][2] + i_[1][2] * m.i_[2][2];
+ return TMatrix3(TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
+ TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
+ TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
+}
- res[2][0] = i_[2][0] * m.i_[0][0] + i_[2][1] * m.i_[1][0] + i_[2][2] * m.i_[2][0];
- res[2][1] = i_[2][0] * m.i_[0][1] + i_[2][1] * m.i_[1][1] + i_[2][2] * m.i_[2][1];
- res[2][2] = i_[2][0] * m.i_[0][2] + i_[2][1] * m.i_[1][2] + i_[2][2] * m.i_[2][2];
+TMatrix3 TMatrix3::operator * (const TaylorModel& d) const
+{
+ return TMatrix3(v_[0] * d, v_[1] * d, v_[2] * d);
+}
- return TMatrix3(res);
+TMatrix3 TMatrix3::operator * (FCL_REAL d) const
+{
+ return TMatrix3(v_[0] * d, v_[1] * d, v_[2] * d);
}
-TMatrix3 TMatrix3::operator + (const TMatrix3& m) const
+
+TMatrix3& TMatrix3::operator *= (const Matrix3f& m)
{
- TaylorModel res[3][3];
+ const Vec3f& mc0 = m.getColumn(0);
+ const Vec3f& mc1 = m.getColumn(1);
+ const Vec3f& mc2 = m.getColumn(2);
+
+ v_[0] = TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
+ v_[1] = TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
+ v_[2] = TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
+ return *this;
+}
- res[0][0] = i_[0][0] + m.i_[0][0];
- res[0][1] = i_[0][1] + m.i_[0][1];
- res[0][2] = i_[0][2] + m.i_[0][2];
+TMatrix3& TMatrix3::operator *= (const TMatrix3& m)
+{
+ const TVector3& mc0 = m.getColumn(0);
+ const TVector3& mc1 = m.getColumn(1);
+ const TVector3& mc2 = m.getColumn(2);
+
+ v_[0] = TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
+ v_[1] = TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
+ v_[2] = TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
+ return *this;
+}
- res[1][0] = i_[1][0] + m.i_[1][0];
- res[1][1] = i_[1][1] + m.i_[1][1];
- res[1][2] = i_[1][2] + m.i_[1][2];
+TMatrix3& TMatrix3::operator *= (const TaylorModel& d)
+{
+ v_[0] *= d;
+ v_[1] *= d;
+ v_[2] *= d;
+ return *this;
+}
- res[2][0] = i_[2][0] + m.i_[2][0];
- res[2][1] = i_[2][1] + m.i_[2][1];
- res[2][2] = i_[2][2] + m.i_[2][2];
+TMatrix3& TMatrix3::operator *= (FCL_REAL d)
+{
+ v_[0] *= d;
+ v_[1] *= d;
+ v_[2] *= d;
+ return *this;
+}
- return TMatrix3(res);
+TMatrix3 TMatrix3::operator + (const TMatrix3& m) const
+{
+ return TMatrix3(v_[0] + m.v_[0], v_[1] + m.v_[1], v_[2] + m.v_[2]);
}
TMatrix3& TMatrix3::operator += (const TMatrix3& m)
{
- i_[0][0] += m.i_[0][0];
- i_[0][1] += m.i_[0][1];
- i_[0][2] += m.i_[0][2];
-
- i_[1][0] += m.i_[1][0];
- i_[1][1] += m.i_[1][1];
- i_[1][2] += m.i_[1][2];
+ v_[0] += m.v_[0];
+ v_[1] += m.v_[1];
+ v_[2] += m.v_[2];
+ return *this;
+}
- i_[2][0] += m.i_[2][0];
- i_[2][1] += m.i_[2][1];
- i_[2][2] += m.i_[2][2];
+TMatrix3 TMatrix3::operator - (const TMatrix3& m) const
+{
+ return TMatrix3(v_[0] - m.v_[0], v_[1] - m.v_[1], v_[2] - m.v_[2]);
+}
+TMatrix3& TMatrix3::operator -= (const TMatrix3& m)
+{
+ v_[0] -= m.v_[0];
+ v_[1] -= m.v_[1];
+ v_[2] -= m.v_[2];
return *this;
}
@@ -337,50 +225,43 @@ void TMatrix3::print() const
IMatrix3 TMatrix3::getBound() const
{
- Interval res[3][3];
-
- res[0][0] = i_[0][0].getBound();
- res[0][1] = i_[0][1].getBound();
- res[0][2] = i_[0][2].getBound();
-
- res[1][0] = i_[1][0].getBound();
- res[1][1] = i_[1][1].getBound();
- res[1][2] = i_[1][2].getBound();
-
- res[2][0] = i_[2][0].getBound();
- res[2][1] = i_[2][1].getBound();
- res[2][2] = i_[2][2].getBound();
-
- return IMatrix3(res);
+ return IMatrix3(v_[0].getBound(), v_[1].getBound(), v_[2].getBound());
}
FCL_REAL TMatrix3::diameter() const
{
FCL_REAL d = 0;
-
- FCL_REAL tmp = i_[0][0].r_.diameter();
+ FCL_REAL tmp = v_[0][0].r_.diameter();
if(tmp > d) d = tmp;
- tmp = i_[0][1].r_.diameter();
+ tmp = v_[0][1].r_.diameter();
if(tmp > d) d = tmp;
- tmp = i_[0][2].r_.diameter();
+ tmp = v_[0][2].r_.diameter();
if(tmp > d) d = tmp;
- tmp = i_[1][0].r_.diameter();
+ tmp = v_[1][0].r_.diameter();
if(tmp > d) d = tmp;
- tmp = i_[1][1].r_.diameter();
+ tmp = v_[1][1].r_.diameter();
if(tmp > d) d = tmp;
- tmp = i_[1][2].r_.diameter();
+ tmp = v_[1][2].r_.diameter();
if(tmp > d) d = tmp;
- tmp = i_[2][0].r_.diameter();
+ tmp = v_[2][0].r_.diameter();
if(tmp > d) d = tmp;
- tmp = i_[2][1].r_.diameter();
+ tmp = v_[2][1].r_.diameter();
if(tmp > d) d = tmp;
- tmp = i_[2][2].r_.diameter();
+ tmp = v_[2][2].r_.diameter();
if(tmp > d) d = tmp;
return d;
}
+void TMatrix3::setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval)
+{
+ v_[0].setTimeInterval(time_interval);
+ v_[1].setTimeInterval(time_interval);
+ v_[2].setTimeInterval(time_interval);
+}
+
+
}
diff --git a/trunk/fcl/src/ccd/taylor_model.cpp b/trunk/fcl/src/ccd/taylor_model.cpp
index dd2d25da..769985ab 100644
--- a/trunk/fcl/src/ccd/taylor_model.cpp
+++ b/trunk/fcl/src/ccd/taylor_model.cpp
@@ -38,21 +38,29 @@
#include <cassert>
#include <iostream>
#include <cmath>
+#include <boost/math/constants/constants.hpp>
+
namespace fcl
{
-const FCL_REAL TaylorModel::PI_ = 3.1415626535;
+TaylorModel::TaylorModel()
+{
+ coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
+}
-TaylorModel::TaylorModel() {}
+TaylorModel::TaylorModel(const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
+{
+ coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
+}
-TaylorModel::TaylorModel(FCL_REAL coeff)
+TaylorModel::TaylorModel(FCL_REAL coeff, const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
{
coeffs_[0] = coeff;
coeffs_[1] = coeffs_[2] = coeffs_[3] = r_[0] = r_[1] = 0;
}
-TaylorModel::TaylorModel(FCL_REAL coeffs[3], const Interval& r)
+TaylorModel::TaylorModel(FCL_REAL coeffs[3], const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
{
coeffs_[0] = coeffs[0];
coeffs_[1] = coeffs[1];
@@ -62,7 +70,7 @@ TaylorModel::TaylorModel(FCL_REAL coeffs[3], const Interval& r)
r_ = r;
}
-TaylorModel::TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r)
+TaylorModel::TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
{
coeffs_[0] = c0;
coeffs_[1] = c1;
@@ -74,28 +82,38 @@ TaylorModel::TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, con
void TaylorModel::setTimeInterval(FCL_REAL l, FCL_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]);
+ time_interval_->t_.setValue(l, r);
+ time_interval_->t2_.setValue(l * time_interval_->t_[0], r * time_interval_->t_[1]);
+ time_interval_->t3_.setValue(l * time_interval_->t2_[0], r * time_interval_->t2_[1]);
+ time_interval_->t4_.setValue(l * time_interval_->t3_[0], r * time_interval_->t3_[1]);
+ time_interval_->t5_.setValue(l * time_interval_->t4_[0], r * time_interval_->t4_[1]);
+ time_interval_->t6_.setValue(l * time_interval_->t5_[0], r * time_interval_->t5_[1]);
+}
+
+TaylorModel TaylorModel::operator + (FCL_REAL d) const
+{
+ return TaylorModel(coeffs_[0] + d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_);
+}
+
+TaylorModel& TaylorModel::operator += (FCL_REAL d)
+{
+ coeffs_[0] += d;
}
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_);
+ assert(other.time_interval_ == time_interval_);
+ 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_, time_interval_);
}
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_);
+ assert(other.time_interval__ == time_interval_);
+ 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_, time_interval_);
}
TaylorModel& TaylorModel::operator += (const TaylorModel& other)
{
- assert(other.t_ == t_);
+ assert(other.time_interval_ == time_interval_);
coeffs_[0] += other.coeffs_[0];
coeffs_[1] += other.coeffs_[1];
coeffs_[2] += other.coeffs_[2];
@@ -106,7 +124,7 @@ TaylorModel& TaylorModel::operator += (const TaylorModel& other)
TaylorModel& TaylorModel::operator -= (const TaylorModel& other)
{
- assert(other.t_ == t_);
+ assert(other.time_interval_ == time_interval_);
coeffs_[0] -= other.coeffs_[0];
coeffs_[1] -= other.coeffs_[1];
coeffs_[2] -= other.coeffs_[2];
@@ -132,7 +150,19 @@ TaylorModel& TaylorModel::operator -= (const TaylorModel& other)
TaylorModel TaylorModel::operator * (const TaylorModel& other) const
{
- assert(other.t_ == t_);
+ TaylorModel res(*this);
+ res *= other;
+ return res;
+}
+
+TaylorModel TaylorModel::operator * (FCL_REAL d) const
+{
+ return TaylorModel(coeffs_[0] * d, coeffs_[1] * d, coeffs_[2] * d, coeffs_[3] * d, r_ * d, time_interval_);
+}
+
+TaylorModel& TaylorModel::operator *= (const TaylorModel& other)
+{
+ assert(other.time_interval_ == time_interval_);
register FCL_REAL c0, c1, c2, c3;
register FCL_REAL c0b = other.coeffs_[0], c1b = other.coeffs_[1], c2b = other.coeffs_[2], c3b = other.coeffs_[3];
@@ -145,29 +175,41 @@ TaylorModel TaylorModel::operator * (const TaylorModel& other) const
Interval remainder(r_ * rb);
register FCL_REAL tempVal = coeffs_[1] * c3b + coeffs_[2] * c2b + coeffs_[3] * c1b;
- remainder += t4_ * tempVal;
+ remainder += time_interval_->t4_ * tempVal;
tempVal = coeffs_[2] * c3b + coeffs_[3] * c2b;
- remainder += t5_ * tempVal;
+ remainder += time_interval_->t5_ * tempVal;
tempVal = coeffs_[3] * c3b;
- remainder += t6_ * tempVal;
+ remainder += time_interval_->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_);
+ remainder += ((Interval(coeffs_[0]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3]) * rb +
+ (Interval(c0b) + time_interval_->t_ * c1b + time_interval_->t2_ * c2b + time_interval_->t3_ * c3b) * r_);
- return TaylorModel(c0, c1, c2, c3, remainder);
+ coeffs_[0] = c0;
+ coeffs_[1] = c1;
+ coeffs_[2] = c2;
+ coeffs_[3] = c3;
+
+ r_ = remainder;
+
+ return *this;
}
-TaylorModel TaylorModel::operator * (FCL_REAL d) const
+TaylorModel& TaylorModel::operator *= (FCL_REAL d)
{
- return TaylorModel(coeffs_[0] * d, coeffs_[1] * d, coeffs_[2] * d, coeffs_[3] * d, r_ * d);
+ coeffs_[0] *= d;
+ coeffs_[1] *= d;
+ coeffs_[2] *= d;
+ coeffs_[3] *= d;
+ r_ *= d;
+ return *this;
}
TaylorModel TaylorModel::operator - () const
{
- return TaylorModel(-coeffs_[0], -coeffs_[1], -coeffs_[2], -coeffs_[3], -r_);
+ return TaylorModel(-coeffs_[0], -coeffs_[1], -coeffs_[2], -coeffs_[3], -r_, time_interval_);
}
void TaylorModel::print() const
@@ -191,14 +233,13 @@ Interval TaylorModel::getBound(FCL_REAL t0, FCL_REAL t1) const
Interval TaylorModel::getBound() const
{
- return Interval(coeffs_[0] + r_[0], coeffs_[1] + r_[1]) + t_ * coeffs_[1] + t2_ * coeffs_[2] + t3_ * coeffs_[3];
+ return Interval(coeffs_[0] + r_[0], coeffs_[1] + r_[1]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3];
}
Interval TaylorModel::getTightBound(FCL_REAL t0, FCL_REAL t1) const
{
-
- if(t0 < t_[0]) t0 = t_[0];
- if(t1 > t_[1]) t1 = t_[1];
+ if(t0 < time_interval_->t_[0]) t0 = time_interval_->t_[0];
+ if(t1 > time_interval_->t_[1]) t1 = time_interval_->t_[1];
if(coeffs_[3] == 0)
{
@@ -257,8 +298,8 @@ Interval TaylorModel::getTightBound(FCL_REAL t0, FCL_REAL t1) const
if(delta < 0)
return Interval(LQ, RQ) + r_;
- FCL_REAL r1=(-coeffs_[2]-sqrt(delta))/(3*coeffs_[3]);
- FCL_REAL r2=(-coeffs_[2]+sqrt(delta))/(3*coeffs_[3]);
+ FCL_REAL r1 = (-coeffs_[2]-sqrt(delta))/(3*coeffs_[3]);
+ FCL_REAL r2 = (-coeffs_[2]+sqrt(delta))/(3*coeffs_[3]);
if(r1 <= t1 && r1 >= t0)
{
@@ -280,7 +321,7 @@ Interval TaylorModel::getTightBound(FCL_REAL t0, FCL_REAL t1) const
Interval TaylorModel::getTightBound() const
{
- return getTightBound(t_[0], t_[1]);
+ return getTightBound(time_interval_->t_[0], time_interval_->t_[1]);
}
void TaylorModel::setZero()
@@ -292,7 +333,7 @@ void TaylorModel::setZero()
void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
{
- FCL_REAL a = tm.t_.center();
+ FCL_REAL a = tm.time_interval_->t_.center();
FCL_REAL t = w * a + q0;
FCL_REAL w2 = w * w;
FCL_REAL fa = cos(t);
@@ -310,8 +351,8 @@ void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
if(w == 0) fddddBounds.setValue(0);
else
{
- FCL_REAL cosQL = cos(tm.t_[0] * w + q0);
- FCL_REAL cosQR = cos(tm.t_[1] * w + q0);
+ FCL_REAL cosQL = cos(tm.time_interval_->t_[0] * w + q0);
+ FCL_REAL cosQR = cos(tm.time_interval_->t_[1] * w + q0);
if(cosQL < cosQR) fddddBounds.setValue(cosQL, cosQR);
else fddddBounds.setValue(cosQR, cosQL);
@@ -323,8 +364,8 @@ void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
// 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]
- FCL_REAL k1 = (tm.t_[0] * w + q0) / (2 * TaylorModel::PI_);
- FCL_REAL k2 = (tm.t_[1] * w + q0) / (2 * TaylorModel::PI_);
+ FCL_REAL k1 = (tm.time_interval_->t_[0] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>());
+ FCL_REAL k2 = (tm.time_interval_->t_[1] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>());
if(w > 0)
@@ -346,7 +387,7 @@ void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
FCL_REAL w4 = w2 * w2;
fddddBounds *= w4;
- FCL_REAL midSize = 0.5 * (tm.t_[1] - tm.t_[0]);
+ FCL_REAL midSize = 0.5 * (tm.time_interval_->t_[1] - tm.time_interval_->t_[0]);
FCL_REAL midSize2 = midSize * midSize;
FCL_REAL midSize4 = midSize2 * midSize2;
@@ -361,7 +402,7 @@ void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
{
- FCL_REAL a = tm.t_.center();
+ FCL_REAL a = tm.time_interval_->t_.center();
FCL_REAL t = w * a + q0;
FCL_REAL w2 = w * w;
FCL_REAL fa = sin(t);
@@ -381,8 +422,8 @@ void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
if(w == 0) fddddBounds.setValue(0);
else
{
- FCL_REAL sinQL = sin(w * tm.t_[0] + q0);
- FCL_REAL sinQR = sin(w * tm.t_[1] + q0);
+ FCL_REAL sinQL = sin(w * tm.time_interval_->t_[0] + q0);
+ FCL_REAL sinQR = sin(w * tm.time_interval_->t_[1] + q0);
if(sinQL < sinQR) fddddBounds.setValue(sinQL, sinQR);
else fddddBounds.setValue(sinQR, sinQL);
@@ -394,8 +435,8 @@ void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
// 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]
- FCL_REAL k1 = (tm.t_[0] * w + q0) / (2 * TaylorModel::PI_) - 0.25;
- FCL_REAL k2 = (tm.t_[1] * w + q0) / (2 * TaylorModel::PI_) - 0.25;
+ FCL_REAL k1 = (tm.time_interval_->t_[0] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>()) - 0.25;
+ FCL_REAL k2 = (tm.time_interval_->t_[1] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>()) - 0.25;
if(w > 0)
{
@@ -415,7 +456,7 @@ void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
FCL_REAL w4 = w2 * w2;
fddddBounds *= w4;
- FCL_REAL midSize = 0.5 * (tm.t_[1] - tm.t_[0]);
+ FCL_REAL midSize = 0.5 * (tm.time_interval_->t_[1] - tm.time_interval_->t_[0]);
FCL_REAL midSize2 = midSize * midSize;
FCL_REAL midSize4 = midSize2 * midSize2;
diff --git a/trunk/fcl/src/ccd/taylor_vector.cpp b/trunk/fcl/src/ccd/taylor_vector.cpp
index 95859ce9..25bb84f6 100644
--- a/trunk/fcl/src/ccd/taylor_vector.cpp
+++ b/trunk/fcl/src/ccd/taylor_vector.cpp
@@ -39,7 +39,15 @@
namespace fcl
{
-TVector3::TVector3() {}
+TVector3::TVector3()
+{
+}
+
+TVector3::TVector3(const boost::shared_ptr<TimeInterval>& time_interval)
+{
+ setTimeInterval(time_interval);
+}
+
TVector3::TVector3(TaylorModel v[3])
{
i_[0] = v[0];
@@ -54,48 +62,39 @@ TVector3::TVector3(const TaylorModel& v1, const TaylorModel& v2, const TaylorMod
i_[2] = v3;
}
-TVector3::TVector3(const Vec3f& v)
+TVector3::TVector3(const Vec3f& v, const boost::shared_ptr<TimeInterval>& time_interval)
{
- i_[0] = TaylorModel(v[0]);
- i_[1] = TaylorModel(v[1]);
- i_[2] = TaylorModel(v[2]);
+ i_[0] = TaylorModel(v[0], time_interval);
+ i_[1] = TaylorModel(v[1], time_interval);
+ i_[2] = TaylorModel(v[2], time_interval);
}
void TVector3::setZero()
{
- i_[0] = TaylorModel(0);
- i_[1] = TaylorModel(0);
- i_[2] = TaylorModel(0);
+ i_[0].setZero();
+ i_[1].setZero();
+ i_[2].setZero();
}
TVector3 TVector3::operator + (const TVector3& other) const
{
- TaylorModel res[3];
- res[0] = i_[0] + other.i_[0];
- res[1] = i_[1] + other.i_[1];
- res[2] = i_[2] + other.i_[2];
-
- return TVector3(res);
+ return TVector3(i_[0] + other.i_[0], i_[1] + other.i_[1], i_[2] + other.i_[2]);
}
TVector3 TVector3::operator + (FCL_REAL d) const
{
- TaylorModel res[3];
- res[0] = i_[0];
- res[1] = i_[1];
- res[2] = i_[2];
- res[0].coeffs_[0] += d;
- return TVector3(res);
+ return TVector3(i_[0], i_[1], i_[2] + d);
}
-TVector3 TVector3::operator - (const TVector3& other) const
+TVector3& TVector3::operator += (FCL_REAL d)
{
- TaylorModel res[3];
- res[0] = i_[0] - other.i_[0];
- res[1] = i_[1] - other.i_[1];
- res[2] = i_[2] - other.i_[2];
+ i_[2] += d;
+ return *this;
+}
- return TVector3(res);
+TVector3 TVector3::operator - (const TVector3& other) const
+{
+ return TVector3(i_[0] - other.i_[0], i_[1] - other.i_[1], i_[2] - other.i_[2]);
}
TVector3& TVector3::operator += (const TVector3& other)
@@ -114,11 +113,29 @@ TVector3& TVector3::operator -= (const TVector3& other)
return *this;
}
-TVector3& TVector3::operator = (const Vec3f& other)
+TVector3 TVector3::operator * (const TaylorModel& d) const
{
- i_[0] = TaylorModel(other[0]);
- i_[1] = TaylorModel(other[1]);
- i_[2] = TaylorModel(other[2]);
+ return TVector3(i_[0] * d, i_[1] * d, i_[2] * d);
+}
+
+TVector3& TVector3::operator *= (const TaylorModel& d)
+{
+ i_[0] *= d;
+ i_[1] *= d;
+ i_[2] *= d;
+ return *this;
+}
+
+TVector3 TVector3::operator * (FCL_REAL d) const
+{
+ return TVector3(i_[0] * d, i_[1] * d, i_[2] * d);
+}
+
+TVector3& TVector3::operator *= (FCL_REAL d)
+{
+ i_[0] *= d;
+ i_[1] *= d;
+ i_[2] *= d;
return *this;
}
@@ -139,12 +156,21 @@ TaylorModel TVector3::dot(const TVector3& other) const
TVector3 TVector3::cross(const TVector3& other) const
{
- TaylorModel res[3];
- res[0] = i_[1] * other.i_[2] - i_[2] * other.i_[1];
- res[1] = i_[2] * other.i_[0] - i_[0] * other.i_[2];
- res[2] = i_[0] * other.i_[1] - i_[1] * other.i_[0];
+ return TVector3(i_[1] * other.i_[2] - i_[2] * other.i_[1],
+ i_[2] * other.i_[0] - i_[0] * other.i_[2],
+ i_[0] * other.i_[1] - i_[1] * other.i_[0]);
+}
- return TVector3(res);
+TaylorModel TVector3::dot(const Vec3f& other) const
+{
+ return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2];
+}
+
+TVector3 TVector3::cross(const Vec3f& other) const
+{
+ return TVector3(i_[1] * other[2] - i_[2] * other[1],
+ i_[2] * other[0] - i_[0] * other[2],
+ i_[0] * other[1] - i_[1] * other[0]);
}
FCL_REAL TVector3::volumn() const
@@ -154,12 +180,7 @@ FCL_REAL TVector3::volumn() const
IVector3 TVector3::getBound() const
{
- Interval res[3];
- res[0] = i_[0].getBound();
- res[1] = i_[1].getBound();
- res[2] = i_[2].getBound();
-
- return IVector3(res);
+ return IVector3(i_[0].getBound(), i_[1].getBound(), i_[2].getBound());
}
void TVector3::print() const
@@ -179,6 +200,13 @@ TaylorModel TVector3::squareLength() const
return i_[0] * i_[0] + i_[1] * i_[1] + i_[2] * i_[2];
}
+void TVector3::setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval)
+{
+ i_[0].setTimeInterval(time_interval);
+ i_[1].setTimeInterval(time_interval);
+ i_[2].setTimeInterval(time_interval);
+}
+
void generateTVector3ForLinearFunc(TVector3& v, const Vec3f& position, const Vec3f& velocity)
{
generateTaylorModelForLinearFunc(v.i_[0], position[0], velocity[0]);
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index 09bdd178..74cf3767 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -222,9 +222,9 @@ void computeBV<AABB, Box>(const Box& s, const SimpleTransform& tf, AABB& bv)
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- FCL_REAL x_range = 0.5 * (fabs(R[0][0] * s.side[0]) + fabs(R[0][1] * s.side[1]) + fabs(R[0][2] * s.side[2]));
- FCL_REAL y_range = 0.5 * (fabs(R[1][0] * s.side[0]) + fabs(R[1][1] * s.side[1]) + fabs(R[1][2] * s.side[2]));
- FCL_REAL z_range = 0.5 * (fabs(R[2][0] * s.side[0]) + fabs(R[2][1] * s.side[1]) + fabs(R[2][2] * s.side[2]));
+ FCL_REAL x_range = 0.5 * (fabs(R(0, 0) * s.side[0]) + fabs(R(0, 1) * s.side[1]) + fabs(R(0, 2) * s.side[2]));
+ FCL_REAL y_range = 0.5 * (fabs(R(1, 0) * s.side[0]) + fabs(R(1, 1) * s.side[1]) + fabs(R(1, 2) * s.side[2]));
+ FCL_REAL z_range = 0.5 * (fabs(R(2, 0) * s.side[0]) + fabs(R(2, 1) * s.side[1]) + fabs(R(2, 2) * s.side[2]));
bv.max_ = T + Vec3f(x_range, y_range, z_range);
bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
@@ -245,9 +245,9 @@ void computeBV<AABB, Capsule>(const Capsule& s, const SimpleTransform& tf, AABB&
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- FCL_REAL x_range = 0.5 * fabs(R[0][2] * s.lz) + s.radius;
- FCL_REAL y_range = 0.5 * fabs(R[1][2] * s.lz) + s.radius;
- FCL_REAL z_range = 0.5 * fabs(R[2][2] * s.lz) + s.radius;
+ FCL_REAL x_range = 0.5 * fabs(R(0, 2) * s.lz) + s.radius;
+ FCL_REAL y_range = 0.5 * fabs(R(1, 2) * s.lz) + s.radius;
+ FCL_REAL z_range = 0.5 * fabs(R(2, 2) * s.lz) + s.radius;
bv.max_ = T + Vec3f(x_range, y_range, z_range);
bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
@@ -259,9 +259,9 @@ void computeBV<AABB, Cone>(const Cone& s, const SimpleTransform& tf, AABB& bv)
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- FCL_REAL x_range = fabs(R[0][0] * s.radius) + fabs(R[0][1] * s.radius) + 0.5 * fabs(R[0][2] * s.lz);
- FCL_REAL y_range = fabs(R[1][0] * s.radius) + fabs(R[1][1] * s.radius) + 0.5 * fabs(R[1][2] * s.lz);
- FCL_REAL z_range = fabs(R[2][0] * s.radius) + fabs(R[2][1] * s.radius) + 0.5 * fabs(R[2][2] * s.lz);
+ FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz);
+ FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
+ FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
bv.max_ = T + Vec3f(x_range, y_range, z_range);
bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
@@ -273,9 +273,9 @@ void computeBV<AABB, Cylinder>(const Cylinder& s, const SimpleTransform& tf, AAB
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- FCL_REAL x_range = fabs(R[0][0] * s.radius) + fabs(R[0][1] * s.radius) + 0.5 * fabs(R[0][2] * s.lz);
- FCL_REAL y_range = fabs(R[1][0] * s.radius) + fabs(R[1][1] * s.radius) + 0.5 * fabs(R[1][2] * s.lz);
- FCL_REAL z_range = fabs(R[2][0] * s.radius) + fabs(R[2][1] * s.radius) + 0.5 * fabs(R[2][2] * s.lz);
+ FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz);
+ FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
+ FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
bv.max_ = T + Vec3f(x_range, y_range, z_range);
bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
diff --git a/trunk/fcl/src/intersect.cpp b/trunk/fcl/src/intersect.cpp
index 9185aeba..07c6802a 100644
--- a/trunk/fcl/src/intersect.cpp
+++ b/trunk/fcl/src/intersect.cpp
@@ -1207,7 +1207,9 @@ FCL_REAL Intersect::distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v)
bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t)
{
Vec3f n_ = (v2 - v1).cross(v3 - v1);
- if(n_.normalize())
+ bool can_normalize = false;
+ n_.normalize(&can_normalize);
+ if(can_normalize)
{
*n = n_;
*t = n_.dot(v1);
@@ -1220,7 +1222,9 @@ bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f
bool Intersect::buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t)
{
Vec3f n_ = (v2 - v1).cross(tn);
- if(n_.normalize())
+ bool can_normalize = false;
+ n_.normalize(&can_normalize);
+ if(can_normalize)
{
*n = n_;
*t = n_.dot(v1);
@@ -1473,14 +1477,14 @@ FCL_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
if (i < nPositiveExamples)
{
- double sigma = uc1[i].Sigma.quadraticForm(fgrad);
+ double sigma = quadraticForm(uc1[i].Sigma, fgrad);
FCL_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
}
else
{
- double sigma = uc2[i - nPositiveExamples].Sigma.quadraticForm(fgrad);
+ double sigma = uc2[i - quadraticForm(nPositiveExamples].Sigma, fgrad);
FCL_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
@@ -1661,7 +1665,7 @@ FCL_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
if (i < nPositiveExamples)
{
- double sigma = uc1[i].Sigma.quadraticForm(fgrad);
+ double sigma = quadraticForm(uc1[i].Sigma, fgrad);
FCL_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
@@ -1669,7 +1673,7 @@ FCL_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
else
{
Matrix3f rotatedSigma = R.tensorTransform(uc2[i - nPositiveExamples].Sigma);
- double sigma = rotatedSigma.quadraticForm(fgrad);
+ double sigma = quadraticForm(rotatedSigma, fgrad);
FCL_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
@@ -1753,9 +1757,9 @@ FCL_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
if(b_inside)
{
- FCL_REAL prob1 = gaussianCDF((projected_p.dot(edge_n[0]) - edge_t[0]) / sqrt(newS.quadraticForm(edge_n[0])));
- FCL_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[1]) - edge_t[1]) / sqrt(newS.quadraticForm(edge_n[1])));
- FCL_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[2]) - edge_t[2]) / sqrt(newS.quadraticForm(edge_n[2])));
+ FCL_REAL prob1 = gaussianCDF((projected_p.dot(edge_n[0]) - edge_t[0]) / sqrt(quadraticForm(newS, edge_n[0])));
+ FCL_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[1]) - edge_t[1]) / sqrt(quadraticForm(newS, edge_n[1])));
+ FCL_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[2]) - edge_t[2]) / sqrt(quadraticForm(newS, edge_n[2])));
FCL_REAL prob = 1.0 - prob1 - prob2 - prob3;
if(prob > max_prob) max_prob = prob;
}
@@ -1774,12 +1778,12 @@ FCL_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
if(pos_plane.size() == 1)
{
int pos_id = pos_plane[0];
- FCL_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[pos_id]) - edge_t[pos_id]) / sqrt(newS.quadraticForm(edge_n[pos_id])));
+ FCL_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[pos_id]) - edge_t[pos_id]) / sqrt(quadraticForm(newS, edge_n[pos_id])));
int neg_id1 = neg_plane[0];
int neg_id2 = neg_plane[1];
- FCL_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[neg_id1]) - edge_t[neg_id1]) / sqrt(newS.quadraticForm(edge_n[neg_id2])));
- FCL_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[neg_id2]) - edge_t[neg_id2]) / sqrt(newS.quadraticForm(edge_n[neg_id2])));
+ FCL_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[neg_id1]) - edge_t[neg_id1]) / sqrt(quadraticForm(newS, edge_n[neg_id2])));
+ FCL_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[neg_id2]) - edge_t[neg_id2]) / sqrt(quadraticForm(newS, edge_n[neg_id2])));
FCL_REAL prob = prob1 - prob2 - prob3;
if(prob > max_prob) max_prob = prob;
@@ -1788,13 +1792,13 @@ FCL_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
else if(pos_plane.size() == 2)
{
int neg_id = neg_plane[0];
- FCL_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[neg_id]) - edge_t[neg_id]) / sqrt(newS.quadraticForm(edge_n[neg_id])));
+ FCL_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[neg_id]) - edge_t[neg_id]) / sqrt(quadraticForm(newS, edge_n[neg_id])));
int pos_id1 = pos_plane[0];
int pos_id2 = pos_plane[1];
- FCL_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[pos_id1])) / sqrt(newS.quadraticForm(edge_n[pos_id1])));
- FCL_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[pos_id2])) / sqrt(newS.quadraticForm(edge_n[pos_id2])));
+ FCL_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[pos_id1])) / sqrt(quadraticForm(newS, edge_n[pos_id1])));
+ FCL_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[pos_id2])) / sqrt(quadraticForm(newS, edge_n[pos_id2])));
FCL_REAL prob = prob1 - prob2 - prob3;
if(prob > max_prob) max_prob = prob;
diff --git a/trunk/fcl/src/matrix_3f.cpp b/trunk/fcl/src/matrix_3f.cpp
deleted file mode 100644
index 93cfefb3..00000000
--- a/trunk/fcl/src/matrix_3f.cpp
+++ /dev/null
@@ -1,240 +0,0 @@
-/*
- * 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/matrix_3f.h"
-#include <iostream>
-
-namespace fcl
-{
-
-Matrix3f& Matrix3f::operator *= (const Matrix3f& other)
-{
- setValue(other.transposeDotX(v_[0]), other.transposeDotY(v_[0]), other.transposeDotZ(v_[0]),
- other.transposeDotX(v_[1]), other.transposeDotY(v_[1]), other.transposeDotZ(v_[1]),
- other.transposeDotX(v_[2]), other.transposeDotY(v_[2]), other.transposeDotZ(v_[2]));
-
- return *this;
-}
-
-Matrix3f& Matrix3f::operator += (FCL_REAL c)
-{
- setValue(v_[0][0] + c, v_[0][1] + c, v_[0][2] + c,
- v_[1][0] + c, v_[1][1] + c, v_[1][2] + c,
- v_[2][0] + c, v_[2][1] + c, v_[2][2] + c);
- return *this;
-}
-
-
-FCL_REAL Matrix3f::determinant() const
-{
- return triple(v_[0], v_[1], v_[2]);
-}
-
-Matrix3f Matrix3f::transpose() const
-{
- return Matrix3f(v_[0][0], v_[1][0], v_[2][0],
- v_[0][1], v_[1][1], v_[2][1],
- v_[0][2], v_[1][2], v_[2][2]);
-}
-
-Matrix3f Matrix3f::inverse() const
-{
- FCL_REAL det = determinant();
- FCL_REAL inv_det = 1.0 / det;
-
- return Matrix3f((v_[1][1] * v_[2][2] - v_[1][2] * v_[2][1]) * inv_det,
- (v_[0][2] * v_[2][1] - v_[0][1] * v_[2][2]) * inv_det,
- (v_[0][1] * v_[1][2] - v_[0][2] * v_[1][1]) * inv_det,
- (v_[1][2] * v_[2][0] - v_[1][0] * v_[2][2]) * inv_det,
- (v_[0][0] * v_[2][2] - v_[0][2] * v_[2][0]) * inv_det,
- (v_[0][2] * v_[1][0] - v_[0][0] * v_[1][2]) * inv_det,
- (v_[1][0] * v_[2][1] - v_[1][1] * v_[2][0]) * inv_det,
- (v_[0][1] * v_[2][0] - v_[0][0] * v_[2][1]) * inv_det,
- (v_[0][0] * v_[1][1] - v_[0][1] * v_[1][0]) * inv_det);
-}
-
-
-Matrix3f Matrix3f::transposeTimes(const Matrix3f& m) const
-{
- return Matrix3f(
- v_[0][0] * m[0][0] + v_[1][0] * m[1][0] + v_[2][0] * m[2][0],
- v_[0][0] * m[0][1] + v_[1][0] * m[1][1] + v_[2][0] * m[2][1],
- v_[0][0] * m[0][2] + v_[1][0] * m[1][2] + v_[2][0] * m[2][2],
- v_[0][1] * m[0][0] + v_[1][1] * m[1][0] + v_[2][1] * m[2][0],
- v_[0][1] * m[0][1] + v_[1][1] * m[1][1] + v_[2][1] * m[2][1],
- v_[0][1] * m[0][2] + v_[1][1] * m[1][2] + v_[2][1] * m[2][2],
- v_[0][2] * m[0][0] + v_[1][2] * m[1][0] + v_[2][2] * m[2][0],
- v_[0][2] * m[0][1] + v_[1][2] * m[1][1] + v_[2][2] * m[2][1],
- v_[0][2] * m[0][2] + v_[1][2] * m[1][2] + v_[2][2] * m[2][2]);
-}
-
-Matrix3f Matrix3f::timesTranspose(const Matrix3f& m) const
-{
- return Matrix3f(v_[0].dot(m[0]), v_[0].dot(m[1]), v_[0].dot(m[2]),
- v_[1].dot(m[0]), v_[1].dot(m[1]), v_[1].dot(m[2]),
- v_[2].dot(m[0]), v_[2].dot(m[1]), v_[2].dot(m[2]));
-}
-
-Vec3f Matrix3f::operator * (const Vec3f& v) const
-{
- return Vec3f(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
-}
-
-Vec3f Matrix3f::transposeTimes(const Vec3f& v) const
-{
- return Vec3f(transposeDotX(v), transposeDotY(v), transposeDotZ(v));
-}
-
-Matrix3f Matrix3f::tensorTransform(const Matrix3f& m) const
-{
- Matrix3f res = *this;
- res *= m;
- return res.timesTranspose(*this);
-}
-
-Matrix3f Matrix3f::operator * (const Matrix3f& m) const
-{
- Matrix3f res = *this;
- return res *= m;
-}
-
-void relativeTransform(const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, Matrix3f& R, Vec3f& T)
-{
- R = R1.transposeTimes(R2);
- Vec3f temp = T2 - T1;
- T = R1.transposeTimes(temp);
-}
-
-void matEigen(const Matrix3f& m, FCL_REAL dout[3], Vec3f vout[3])
-{
- Matrix3f R(m);
- int n = 3;
- int j, iq, ip, i;
- FCL_REAL tresh, theta, tau, t, sm, s, h, g, c;
- int nrot;
- FCL_REAL b[3];
- FCL_REAL z[3];
- FCL_REAL v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- FCL_REAL d[3];
-
- for(ip = 0; ip < n; ++ip)
- {
- b[ip] = R[ip][ip];
- d[ip] = R[ip][ip];
- z[ip] = 0.0;
- }
-
- nrot = 0;
-
- for(i = 0; i < 50; ++i)
- {
- sm = 0.0;
- for(ip = 0; ip < n; ++ip)
- for(iq = ip + 1; iq < n; ++iq)
- sm += fabs(R[ip][iq]);
- if(sm == 0.0)
- {
- vout[0].setValue(v[0][0], v[0][1], v[0][2]);
- vout[1].setValue(v[1][0], v[1][1], v[1][2]);
- vout[2].setValue(v[2][0], v[2][1], v[2][2]);
- dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2];
- return;
- }
-
- if(i < 3) tresh = 0.2 * sm / (n * n);
- else tresh = 0.0;
-
- for(ip = 0; ip < n; ++ip)
- {
- for(iq= ip + 1; iq < n; ++iq)
- {
- g = 100.0 * fabs(R[ip][iq]);
- if(i > 3 &&
- fabs(d[ip]) + g == fabs(d[ip]) &&
- fabs(d[iq]) + g == fabs(d[iq]))
- R[ip][iq] = 0.0;
- else if(fabs(R[ip][iq]) > tresh)
- {
- h = d[iq] - d[ip];
- if(fabs(h) + g == fabs(h)) t = (R[ip][iq]) / h;
- else
- {
- theta = 0.5 * h / (R[ip][iq]);
- t = 1.0 /(fabs(theta) + sqrt(1.0 + theta * theta));
- if(theta < 0.0) t = -t;
- }
- c = 1.0 / sqrt(1 + t * t);
- s = t * c;
- tau = s / (1.0 + c);
- h = t * R[ip][iq];
- z[ip] -= h;
- z[iq] += h;
- d[ip] -= h;
- d[iq] += h;
- R[ip][iq] = 0.0;
- for(j = 0; j < ip; ++j) { g = R[j][ip]; h = R[j][iq]; R[j][ip] = g - s * (h + g * tau); R[j][iq] = h + s * (g - h * tau); }
- for(j = ip + 1; j < iq; ++j) { g = R[ip][j]; h = R[j][iq]; R[ip][j] = g - s * (h + g * tau); R[j][iq] = h + s * (g - h * tau); }
- for(j = iq + 1; j < n; ++j) { g = R[ip][j]; h = R[iq][j]; R[ip][j] = g - s * (h + g * tau); R[iq][j] = h + s * (g - h * tau); }
- for(j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); }
- nrot++;
- }
- }
- }
- for(ip = 0; ip < n; ++ip)
- {
- b[ip] += z[ip];
- d[ip] = b[ip];
- z[ip] = 0.0;
- }
- }
-
- std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
-
- return;
-
-}
-
-Matrix3f abs(const Matrix3f& R)
-{
- return Matrix3f(fabs(R.v_[0][0]), fabs(R.v_[0][1]), fabs(R.v_[0][2]),
- fabs(R.v_[1][0]), fabs(R.v_[1][1]), fabs(R.v_[1][2]),
- fabs(R.v_[2][0]), fabs(R.v_[2][1]), fabs(R.v_[2][2]));
-}
-
-
-
-}
diff --git a/trunk/fcl/src/narrowphase/gjk.cpp b/trunk/fcl/src/narrowphase/gjk.cpp
index f5daef36..2813b2f1 100644
--- a/trunk/fcl/src/narrowphase/gjk.cpp
+++ b/trunk/fcl/src/narrowphase/gjk.cpp
@@ -406,7 +406,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
void GJK::getSupport(const Vec3f& d, SimplexV& sv) const
{
- sv.d = d.normalized();
+ sv.d = normalize(d);
sv.w = shape.support(sv.d);
}
diff --git a/trunk/fcl/src/narrowphase/narrowphase.cpp b/trunk/fcl/src/narrowphase/narrowphase.cpp
index 785f584f..17bd02af 100644
--- a/trunk/fcl/src/narrowphase/narrowphase.cpp
+++ b/trunk/fcl/src/narrowphase/narrowphase.cpp
@@ -206,7 +206,7 @@ bool sphereTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
if(distance_sqr > 0)
{
FCL_REAL distance = std::sqrt(distance_sqr);
- if(normal_) *normal_ = contact_to_center.normalized();
+ if(normal_) *normal_ = normalize(contact_to_center);
if(contact_points) *contact_points = contact_point;
if(penetration_depth) *penetration_depth = -(radius - distance);
}
@@ -690,7 +690,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// separating axis = u1, u2, u3
tmp = pp[0];
- s2 = std::abs(tmp) - (Q[0].dot(B) + A[0]);
+ s2 = std::abs(tmp) - (Q.dotX(B) + A[0]);
if(s2 > 0) { *return_code = 0; return 0; }
if(s2 > s)
{
@@ -701,7 +701,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
tmp = pp[1];
- s2 = std::abs(tmp) - (Q[1].dot(B) + A[1]);
+ s2 = std::abs(tmp) - (Q.dotY(B) + A[1]);
if(s2 > 0) { *return_code = 0; return 0; }
if(s2 > s)
{
@@ -712,7 +712,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
tmp = pp[2];
- s2 = std::abs(tmp) - (Q[2].dot(B) + A[2]);
+ s2 = std::abs(tmp) - (Q.dotZ(B) + A[2]);
if(s2 > 0) { *return_code = 0; return 0; }
if(s2 > s)
{
@@ -761,10 +761,10 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
FCL_REAL eps = std::numeric_limits<FCL_REAL>::epsilon();
// separating axis = u1 x (v1,v2,v3)
- tmp = pp[2] * R[1][0] - pp[1] * R[2][0];
- s2 = std::abs(tmp) - (A[1] * Q[2][0] + A[2] * Q[1][0] + B[1] * Q[0][2] + B[2] * Q[0][1]);
+ tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0);
+ s2 = std::abs(tmp) - (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1));
if(s2 > eps) { *return_code = 0; return 0; }
- n = Vec3f(0, -R[2][0], R[1][0]);
+ n = Vec3f(0, -R(2, 0), R(1, 0));
l = n.length();
if(l > eps)
{
@@ -779,10 +779,10 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
}
- tmp = pp[2] * R[1][1] - pp[1] * R[2][1];
- s2 = std::abs(tmp) - (A[1] * Q[2][1] + A[2] * Q[1][1] + B[0] * Q[0][2] + B[2] * Q[0][0]);
+ tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1);
+ s2 = std::abs(tmp) - (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0));
if(s2 > eps) { *return_code = 0; return 0; }
- n = Vec3f(0, -R[2][1], R[1][1]);
+ n = Vec3f(0, -R(2, 1), R(1, 1));
l = n.length();
if(l > eps)
{
@@ -797,10 +797,10 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
}
- tmp = pp[2] * R[1][2] - pp[1] * R[2][2];
- s2 = std::abs(tmp) - (A[1] * Q[2][2] + A[2] * Q[1][2] + B[0] * Q[0][1] + B[1] * Q[0][0]);
+ tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2);
+ s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0));
if(s2 > eps) { *return_code = 0; return 0; }
- n = Vec3f(0, -R[2][2], R[1][2]);
+ n = Vec3f(0, -R(2, 2), R(1, 2));
l = n.length();
if(l > eps)
{
@@ -816,10 +816,10 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
// separating axis = u2 x (v1,v2,v3)
- tmp = pp[0] * R[2][0] - pp[2] * R[0][0];
- s2 = std::abs(tmp) - (A[0] * Q[2][0] + A[2] * Q[0][0] + B[1] * Q[1][2] + B[2] * Q[1][1]);
+ tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0);
+ s2 = std::abs(tmp) - (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1));
if(s2 > eps) { *return_code = 0; return 0; }
- n = Vec3f(R[2][0], 0, -R[0][0]);
+ n = Vec3f(R(2, 0), 0, -R(0, 0));
l = n.length();
if(l > eps)
{
@@ -834,10 +834,10 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
}
- tmp = pp[0] * R[2][1] - pp[2] * R[0][1];
- s2 = std::abs(tmp) - (A[0] * Q[2][1] + A[2] * Q[0][1] + B[0] * Q[1][2] + B[2] * Q[1][0]);
+ tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1);
+ s2 = std::abs(tmp) - (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0));
if(s2 > eps) { *return_code = 0; return 0; }
- n = Vec3f(R[2][1], 0, -R[0][1]);
+ n = Vec3f(R(2, 1), 0, -R(0, 1));
l = n.length();
if(l > eps)
{
@@ -852,10 +852,10 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
}
- tmp = pp[0] * R[2][2] - pp[2] * R[0][2];
- s2 = std::abs(tmp) - (A[0] * Q[2][2] + A[2] * Q[0][2] + B[0] * Q[1][1] + B[1] * Q[1][0]);
+ tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2);
+ s2 = std::abs(tmp) - (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0));
if(s2 > eps) { *return_code = 0; return 0; }
- n = Vec3f(R[2][2], 0, -R[0][2]);
+ n = Vec3f(R(2, 2), 0, -R(0, 2));
l = n.length();
if(l > eps)
{
@@ -871,10 +871,10 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
// separating axis = u3 x (v1,v2,v3)
- tmp = pp[1] * R[0][0] - pp[0] * R[1][0];
- s2 = std::abs(tmp) - (A[0] * Q[1][0] + A[1] * Q[0][0] + B[1] * Q[2][2] + B[2] * Q[2][1]);
+ tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0);
+ s2 = std::abs(tmp) - (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1));
if(s2 > eps) { *return_code = 0; return 0; }
- n = Vec3f(-R[1][0], R[0][0], 0);
+ n = Vec3f(-R(1, 0), R(0, 0), 0);
l = n.length();
if(l > eps)
{
@@ -889,10 +889,10 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
}
- tmp = pp[1] * R[0][1] - pp[0] * R[1][1];
- s2 = std::abs(tmp) - (A[0] * Q[1][1] + A[1] * Q[0][1] + B[0] * Q[2][2] + B[2] * Q[2][0]);
+ tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1);
+ s2 = std::abs(tmp) - (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0));
if(s2 > eps) { *return_code = 0; return 0; }
- n = Vec3f(-R[1][1], R[0][1], 0);
+ n = Vec3f(-R(1, 1), R(0, 1), 0);
l = n.length();
if(l > eps)
{
@@ -907,10 +907,10 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
}
- tmp = pp[1] * R[0][2] - pp[0] * R[1][2];
- s2 = std::abs(tmp) - (A[0] * Q[1][2] + A[1] * Q[0][2] + B[0] * Q[2][1] + B[1] * Q[2][0]);
+ tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2);
+ s2 = std::abs(tmp) - (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0));
if(s2 > eps) { *return_code = 0; return 0; }
- n = Vec3f(-R[1][2], R[0][2], 0);
+ n = Vec3f(-R(1, 2), R(0, 2), 0);
l = n.length();
if(l > eps)
{
@@ -947,7 +947,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
{
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
- Vec3f pa = T1;
+ Vec3f pa(T1);
FCL_REAL sign;
for(int j = 0; j < 3; ++j)
@@ -966,16 +966,15 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
FCL_REAL alpha, beta;
- Vec3f ua, ub;
- ua = R1.getColumn((code-7)/3);
- ub = R2.getColumn((code-7)%3);
+ Vec3f ua(R1.getColumn((code-7)/3));
+ Vec3f ub(R2.getColumn((code-7)%3));
lineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
pa += ua * alpha;
pb += ub * beta;
- Vec3f pointInWorld = (pa + pb) * 0.5;
+ Vec3f pointInWorld((pa + pb) * 0.5);
contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth));
*return_code = code;
diff --git a/trunk/fcl/src/octomap_extension.cpp b/trunk/fcl/src/octomap_extension.cpp
new file mode 100644
index 00000000..69ec965c
--- /dev/null
+++ b/trunk/fcl/src/octomap_extension.cpp
@@ -0,0 +1,351 @@
+/*
+ * 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/octomap_extension.h"
+#include "fcl/geometric_shapes.h"
+
+namespace fcl
+{
+
+class ExtendedBox : public Box
+{
+public:
+ ExtendedBox(FCL_REAL x, FCL_REAL y, FCL_REAL z) : Box(x, y, z)
+ {
+ prob = 0;
+ node = NULL;
+ }
+
+ FCL_REAL prob;
+ octomap::OcTreeNode* node;
+};
+
+bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
+ octomap::OcTree* tree2, octomap::OcTreeNode* root2, const AABB& root2_bv,
+ void* cdata, CollisionCallBack callback)
+{
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(tree2->isNodeOccupied(root2))
+ {
+ if(root1->bv.overlap(root2_bv))
+ {
+ CollisionGeometry* box = new ExtendedBox(root2_bv.max_[0] - root2_bv.min_[0],
+ root2_bv.max_[1] - root2_bv.min_[1],
+ root2_bv.max_[2] - root2_bv.min_[2]);
+ CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), SimpleTransform(root2_bv.center()));
+ return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata);
+ }
+ }
+ }
+
+ if(!tree2->isNodeOccupied(root2) || !root1->bv.overlap(root2_bv)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ if(collisionRecurse(root1->childs[0], tree2, root2, root2_bv, cdata, callback))
+ return true;
+ if(collisionRecurse(root1->childs[1], tree2, root2, root2_bv, cdata, callback))
+ return true;
+ }
+ else
+ {
+ for(int i = 0; i < 8; ++i)
+ {
+ if(root2->childExists(i))
+ {
+ octomap::OcTreeNode* child = root2->getChild(i);
+ AABB child_bv;
+ if(i&1 == 0)
+ {
+ child_bv.min_[0] = root2_bv.min_[0];
+ child_bv.max_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
+ }
+ else
+ {
+ child_bv.min_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
+ child_bv.max_[0] = root2_bv.max_[0];
+ }
+
+ if(i&2 == 0)
+ {
+ child_bv.min_[1] = root2_bv.min_[1];
+ child_bv.max_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
+ }
+ else
+ {
+ child_bv.min_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
+ child_bv.max_[1] = root2_bv.max_[1];
+ }
+
+ if(i&4 == 0)
+ {
+ child_bv.min_[2] = root2_bv.min_[2];
+ child_bv.max_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
+ }
+ else
+ {
+ child_bv.min_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
+ child_bv.max_[2] = root2_bv.max_[2];
+ }
+
+ if(collisionRecurse(root1, tree2, child, child_bv, cdata, callback))
+ return true;
+ }
+ }
+ }
+}
+
+void collide(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCallBack callback)
+{
+ DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
+ octomap::OcTreeNode* root2 = octree->getRoot();
+ const octomap::point3d& bv_min = octree->getBBXMin();
+ const octomap::point3d& bv_max = octree->getBBXMax();
+ collisionRecurse(root1, octree, root2, AABB(Vec3f(bv_min.x(), bv_min.y(), bv_min.z()), Vec3f(bv_max.x(), bv_max.y(), bv_max.z())), cdata, callback);
+}
+
+
+bool collisionCostRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
+ octomap::OcTree* tree2, octomap::OcTreeNode* root2, const AABB& root2_bv,
+ void* cdata, CollisionCostCallBack callback, FCL_REAL& cost)
+{
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(!tree2->isNodeOccupied(root2))
+ {
+ if(root1->bv.overlap(root2_bv))
+ {
+ ExtendedBox* box = new ExtendedBox(root2_bv.max_[0] - root2_bv.min_[0],
+ root2_bv.max_[1] - root2_bv.min_[1],
+ root2_bv.max_[2] - root2_bv.min_[2]);
+ box->prob = root2->getOccupancy();
+ box->node = root2;
+
+ CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), SimpleTransform(root2_bv.center()));
+ return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, cost);
+ }
+ }
+ }
+
+ if(tree2->isNodeOccupied(root2) || !root1->bv.overlap(root2_bv)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ if(collisionCostRecurse(root1->childs[0], tree2, root2, root2_bv, cdata, callback, cost))
+ return true;
+ if(collisionCostRecurse(root1->childs[1], tree2, root2, root2_bv, cdata, callback, cost))
+ return true;
+ }
+ else
+ {
+ for(int i = 0; i < 8; ++i)
+ {
+ if(root2->childExists(i))
+ {
+ octomap::OcTreeNode* child = root2->getChild(i);
+ AABB child_bv;
+ if(i&1 == 0)
+ {
+ child_bv.min_[0] = root2_bv.min_[0];
+ child_bv.max_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
+ }
+ else
+ {
+ child_bv.min_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
+ child_bv.max_[0] = root2_bv.max_[0];
+ }
+
+ if(i&2 == 0)
+ {
+ child_bv.min_[1] = root2_bv.min_[1];
+ child_bv.max_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
+ }
+ else
+ {
+ child_bv.min_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
+ child_bv.max_[1] = root2_bv.max_[1];
+ }
+
+ if(i&4 == 0)
+ {
+ child_bv.min_[2] = root2_bv.min_[2];
+ child_bv.max_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
+ }
+ else
+ {
+ child_bv.min_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
+ child_bv.max_[2] = root2_bv.max_[2];
+ }
+
+ if(collisionCostRecurse(root1, tree2, child, child_bv, cdata, callback, cost))
+ return true;
+ }
+ }
+ }
+}
+
+
+bool collisionCostExtRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
+ octomap::OcTree* tree2, octomap::OcTreeNode* root2, const AABB& root2_bv,
+ void* cdata, CollisionCostCallBackExt callback, FCL_REAL& cost, std::set<octomap::OcTreeNode*>& nodes)
+{
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(!tree2->isNodeOccupied(root2))
+ {
+ if(root1->bv.overlap(root2_bv))
+ {
+ ExtendedBox* box = new ExtendedBox(root2_bv.max_[0] - root2_bv.min_[0],
+ root2_bv.max_[1] - root2_bv.min_[1],
+ root2_bv.max_[2] - root2_bv.min_[2]);
+
+ box->prob = root2->getOccupancy();
+ box->node = root2;
+
+ CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), SimpleTransform(root2_bv.center()));
+ return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, cost, nodes);
+ }
+ }
+ }
+
+ if(tree2->isNodeOccupied(root2) || !root1->bv.overlap(root2_bv)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ if(collisionCostExtRecurse(root1->childs[0], tree2, root2, root2_bv, cdata, callback, cost, nodes))
+ return true;
+ if(collisionCostExtRecurse(root1->childs[1], tree2, root2, root2_bv, cdata, callback, cost, nodes))
+ return true;
+ }
+ else
+ {
+ for(int i = 0; i < 8; ++i)
+ {
+ if(root2->childExists(i))
+ {
+ octomap::OcTreeNode* child = root2->getChild(i);
+ AABB child_bv;
+ if(i&1 == 0)
+ {
+ child_bv.min_[0] = root2_bv.min_[0];
+ child_bv.max_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
+ }
+ else
+ {
+ child_bv.min_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
+ child_bv.max_[0] = root2_bv.max_[0];
+ }
+
+ if(i&2 == 0)
+ {
+ child_bv.min_[1] = root2_bv.min_[1];
+ child_bv.max_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
+ }
+ else
+ {
+ child_bv.min_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
+ child_bv.max_[1] = root2_bv.max_[1];
+ }
+
+ if(i&4 == 0)
+ {
+ child_bv.min_[2] = root2_bv.min_[2];
+ child_bv.max_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
+ }
+ else
+ {
+ child_bv.min_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
+ child_bv.max_[2] = root2_bv.max_[2];
+ }
+
+ if(collisionCostExtRecurse(root1, tree2, child, child_bv, cdata, callback, cost, nodes))
+ return true;
+ }
+ }
+ }
+}
+
+
+
+bool defaultCollisionCostFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost)
+{
+ const AABB& aabb1 = o1->getAABB();
+ const AABB& aabb2 = o2->getAABB();
+ Vec3f delta = min(aabb1.max_, aabb2.max_) - max(aabb1.min_, aabb2.min_);
+ const ExtendedBox* box = static_cast<const ExtendedBox*>(o2->getCollisionGeometry());
+ cost += delta[0] * delta[1] * delta[2] * box->prob;
+ return false;
+}
+
+bool defaultCollisionCostExtFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost, std::set<octomap::OcTreeNode*>& nodes)
+{
+ const AABB& aabb1 = o1->getAABB();
+ const AABB& aabb2 = o2->getAABB();
+ Vec3f delta = min(aabb1.max_, aabb2.max_) - max(aabb1.min_, aabb2.min_);
+ const ExtendedBox* box = static_cast<const ExtendedBox*>(o2->getCollisionGeometry());
+ cost += delta[0] * delta[1] * delta[2] * box->prob;
+ nodes.insert(box->node);
+ return false;
+}
+
+FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostCallBack callback)
+{
+ DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
+ octomap::OcTreeNode* root2 = octree->getRoot();
+ const octomap::point3d& bv_min = octree->getBBXMin();
+ const octomap::point3d& bv_max = octree->getBBXMax();
+ FCL_REAL cost = 0;
+ collisionCostRecurse(root1, octree, root2, AABB(Vec3f(bv_min.x(), bv_min.y(), bv_min.z()), Vec3f(bv_max.x(), bv_max.y(), bv_max.z())), cdata, callback, cost);
+ return cost;
+}
+
+FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostCallBackExt callback, std::set<octomap::OcTreeNode*>& nodes)
+{
+ DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
+ octomap::OcTreeNode* root2 = octree->getRoot();
+ const octomap::point3d& bv_min = octree->getBBXMin();
+ const octomap::point3d& bv_max = octree->getBBXMax();
+ FCL_REAL cost = 0;
+ collisionCostExtRecurse(root1, octree, root2, AABB(Vec3f(bv_min.x(), bv_min.y(), bv_min.z()), Vec3f(bv_max.x(), bv_max.y(), bv_max.z())), cdata, callback, cost, nodes);
+ return cost;
+}
+
+
+
+
+
+}
diff --git a/trunk/fcl/src/transform.cpp b/trunk/fcl/src/transform.cpp
index 68b9ecc7..9fbbd5c3 100644
--- a/trunk/fcl/src/transform.cpp
+++ b/trunk/fcl/src/transform.cpp
@@ -44,7 +44,7 @@ void SimpleQuaternion::fromRotation(const Matrix3f& R)
{
const int next[3] = {1, 2, 0};
- FCL_REAL trace = R[0][0] + R[1][1] + R[2][2];
+ FCL_REAL trace = R(0, 0) + R(1, 1) + R(2, 2);
FCL_REAL root;
if(trace > 0.0)
@@ -53,32 +53,32 @@ void SimpleQuaternion::fromRotation(const Matrix3f& R)
root = sqrt(trace + 1.0); // 2w
data[0] = 0.5 * root;
root = 0.5 / root; // 1/(4w)
- data[1] = (R[2][1] - R[1][2])*root;
- data[2] = (R[0][2] - R[2][0])*root;
- data[3] = (R[1][0] - R[0][1])*root;
+ data[1] = (R(2, 1) - R(1, 2))*root;
+ data[2] = (R(0, 2) - R(2, 0))*root;
+ data[3] = (R(1, 0) - R(0, 1))*root;
}
else
{
// |w| <= 1/2
int i = 0;
- if(R[1][1] > R[0][0])
+ if(R(1, 1) > R(0, 0))
{
i = 1;
}
- if(R[2][2] > R[i][i])
+ if(R(2, 2) > R(i, i))
{
i = 2;
}
int j = next[i];
int k = next[j];
- root = sqrt(R[i][i] - R[j][j] - R[k][k] + 1.0);
+ root = sqrt(R(i, i) - R(j, j) - R(k, k) + 1.0);
FCL_REAL* quat[3] = { &data[1], &data[2], &data[3] };
*quat[i] = 0.5 * root;
root = 0.5 / root;
- data[0] = (R[k][j] - R[j][k]) * root;
- *quat[j] = (R[j][i] + R[i][j]) * root;
- *quat[k] = (R[k][i] + R[i][k]) * root;
+ data[0] = (R(k, j) - R(j, k)) * root;
+ *quat[j] = (R(j, i) + R(i, j)) * root;
+ *quat[k] = (R(k, i) + R(i, k)) * root;
}
}
diff --git a/trunk/fcl/src/traversal_recurse.cpp b/trunk/fcl/src/traversal_recurse.cpp
index aa1343c5..2845baa9 100644
--- a/trunk/fcl/src/traversal_recurse.cpp
+++ b/trunk/fcl/src/traversal_recurse.cpp
@@ -141,11 +141,11 @@ void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const
const OBB& bv1 = node->model2->getBV(c1).bv;
Matrix3f Rc;
temp = R * bv1.axis[0];
- Rc[0][0] = temp[0]; Rc[1][0] = temp[1]; Rc[2][0] = temp[2];
+ Rc(0, 0) = temp[0]; Rc(1, 0) = temp[1]; Rc(2, 0) = temp[2];
temp = R * bv1.axis[1];
- Rc[0][1] = temp[0]; Rc[1][1] = temp[1]; Rc[2][1] = temp[2];
+ Rc(0, 1) = temp[0]; Rc(1, 1) = temp[1]; Rc(2, 1) = temp[2];
temp = R * bv1.axis[2];
- Rc[0][2] = temp[0]; Rc[1][2] = temp[1]; Rc[2][2] = temp[2];
+ Rc(0, 2) = temp[0]; Rc(1, 2) = temp[1]; Rc(2, 2) = temp[2];
Vec3f Tc = R * bv1.To + T;
collisionRecurse(node, b1, c1, Rc, Tc, front_list);
@@ -155,11 +155,11 @@ void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const
const OBB& bv2 = node->model2->getBV(c2).bv;
temp = R * bv2.axis[0];
- Rc[0][0] = temp[0]; Rc[1][0] = temp[1]; Rc[2][0] = temp[2];
+ Rc(0, 0) = temp[0]; Rc(1, 0) = temp[1]; Rc(2, 0) = temp[2];
temp = R * bv2.axis[1];
- Rc[0][1] = temp[0]; Rc[1][1] = temp[1]; Rc[2][1] = temp[2];
+ Rc(0, 1) = temp[0]; Rc(1, 1) = temp[1]; Rc(2, 1) = temp[2];
temp = R * bv2.axis[2];
- Rc[0][2] = temp[0]; Rc[1][2] = temp[1]; Rc[2][2] = temp[2];
+ Rc(0, 2) = temp[0]; Rc(1, 2) = temp[1]; Rc(2, 2) = temp[2];
Tc = R * bv2.To + T;
collisionRecurse(node, b1, c2, Rc, Tc, front_list);
--
GitLab