diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h
index 1a2446503d1a71065a5f02e6aff416276e8b5bf3..68d158b424a0dc391fc8263ea34604ee49552cef 100644
--- a/include/hpp/fcl/BV/AABB.h
+++ b/include/hpp/fcl/BV/AABB.h
@@ -190,13 +190,13 @@ public:
/// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
inline FCL_REAL size() const
{
- return (max_ - min_).sqrLength();
+ return (max_ - min_).squaredNorm();
}
/// @brief Radius of the AABB
inline FCL_REAL radius() const
{
- return (max_ - min_).length() / 2;
+ return (max_ - min_).norm() / 2;
}
/// @brief Center of the AABB
diff --git a/include/hpp/fcl/BV/BV.h b/include/hpp/fcl/BV/BV.h
index 957672fdbe91d84cd6f21dac312a586811826e38..76e12fd9adbee163929c77a7cda739fc0174d99b 100644
--- a/include/hpp/fcl/BV/BV.h
+++ b/include/hpp/fcl/BV/BV.h
@@ -75,7 +75,7 @@ public:
static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2)
{
const Vec3f& center = bv1.center();
- FCL_REAL r = (bv1.max_ - bv1.min_).length() * 0.5;
+ FCL_REAL r = (bv1.max_ - bv1.min_).norm() * 0.5;
Vec3f center2 = tf1.transform(center);
Vec3f delta(r, r, r);
bv2.min_ = center2 - delta;
@@ -180,7 +180,7 @@ public:
static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2)
{
const Vec3f& center = bv1.center();
- FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).length() * 0.5;
+ FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
Vec3f delta(r, r, r);
Vec3f center2 = tf1.transform(center);
bv2.min_ = center2 - delta;
diff --git a/include/hpp/fcl/BV/OBB.h b/include/hpp/fcl/BV/OBB.h
index e720480525930e5675d8f9ab9f8a618ae25851f4..9159941cd567ecd993d6335780a7af90f8c67de5 100644
--- a/include/hpp/fcl/BV/OBB.h
+++ b/include/hpp/fcl/BV/OBB.h
@@ -119,7 +119,7 @@ public:
/// @brief Size of the OBB (used in BV_Splitter to order two OBBs)
inline FCL_REAL size() const
{
- return extent.sqrLength();
+ return extent.squaredNorm();
}
/// @brief Center of the OBB
diff --git a/include/hpp/fcl/BV/kIOS.h b/include/hpp/fcl/BV/kIOS.h
index 5bda122cab7f5ecbe82652f60d105dc2729a822d..0475fd81f18386b4d7b05fc9da42834381fe601f 100644
--- a/include/hpp/fcl/BV/kIOS.h
+++ b/include/hpp/fcl/BV/kIOS.h
@@ -58,7 +58,7 @@ class kIOS
static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1)
{
Vec3f d = s1.o - s0.o;
- FCL_REAL dist2 = d.sqrLength();
+ FCL_REAL dist2 = d.squaredNorm();
FCL_REAL diff_r = s1.r - s0.r;
/** The sphere with the larger radius encloses the other */
diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h
index 8db515ee2d5937ad93d414810c1a1fc4ff07afe8..cb85a70aee63466200c38eb55c2e28a8eeefc8d6 100644
--- a/include/hpp/fcl/BVH/BVH_model.h
+++ b/include/hpp/fcl/BVH/BVH_model.h
@@ -236,9 +236,8 @@ public:
const Vec3f& v1 = vertices[tri[0]];
const Vec3f& v2 = vertices[tri[1]];
const Vec3f& v3 = vertices[tri[2]];
- FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
Matrix3f A(v1, v2, v3);
- C += transpose(A) * C_canonical * A * d_six_vol;
+ C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
}
FCL_REAL trace_C = C(0, 0) + C(1, 1) + C(2, 2);
diff --git a/include/hpp/fcl/ccd/motion.h b/include/hpp/fcl/ccd/motion.h
index 7f2af4718d7bb8145a58f1d24dd52c3363a5594a..97bd248d1df6a37816bd1378f98c790588ce13fd 100644
--- a/include/hpp/fcl/ccd/motion.h
+++ b/include/hpp/fcl/ccd/motion.h
@@ -170,7 +170,7 @@ public:
// R(t) = R(t0) + R'(t0) (t-t0) + 1/2 R''(t0)(t-t0)^2 + 1 / 6 R'''(t0) (t-t0)^3 + 1 / 24 R''''(l)(t-t0)^4; t0 = 0.5
/// 1. compute M(1/2)
Vec3f Rt0 = (Rd[0] + Rd[1] * 23 + Rd[2] * 23 + Rd[3]) * (1 / 48.0);
- FCL_REAL Rt0_len = Rt0.length();
+ FCL_REAL Rt0_len = Rt0.norm();
FCL_REAL inv_Rt0_len = 1.0 / Rt0_len;
FCL_REAL inv_Rt0_len_3 = inv_Rt0_len * inv_Rt0_len * inv_Rt0_len;
FCL_REAL inv_Rt0_len_5 = inv_Rt0_len_3 * inv_Rt0_len * inv_Rt0_len;
@@ -196,7 +196,7 @@ public:
/// 3.1. compute M''(1/2)
Vec3f ddRt0 = (Rd[0] - Rd[1] - Rd[2] + Rd[3]) * 0.5;
FCL_REAL Rt0_dot_ddRt0 = Rt0.dot(ddRt0);
- FCL_REAL dRt0_dot_dRt0 = dRt0.sqrLength();
+ FCL_REAL dRt0_dot_dRt0 = dRt0.squaredNorm();
FCL_REAL ddtheta0 = (Rt0_dot_ddRt0 + dRt0_dot_dRt0) * inv_Rt0_len - Rt0_dot_dRt0 * Rt0_dot_dRt0 * inv_Rt0_len_3;
Vec3f ddWt0 = ddRt0 * inv_Rt0_len - (dRt0 * (2 * Rt0_dot_dRt0) + Rt0 * (Rt0_dot_ddRt0 + dRt0_dot_dRt0)) * inv_Rt0_len_3 + (Rt0 * (3 * Rt0_dot_dRt0 * Rt0_dot_dRt0)) * inv_Rt0_len_5;
Matrix3f hatddWt0;
@@ -367,7 +367,7 @@ protected:
{
angular_vel = 0;
axis = tf2.getTranslation() - tf1.getTranslation();
- linear_vel = axis.length();
+ linear_vel = axis.norm();
p = tf1.getTranslation();
}
else
diff --git a/include/hpp/fcl/eigen/vec_3fx.h b/include/hpp/fcl/eigen/vec_3fx.h
index dc47dc43e46a32d14c7d0721662c18f51f419070..2fe02cdc62e8acdb55442e3a4f91c53f34dfd748 100644
--- a/include/hpp/fcl/eigen/vec_3fx.h
+++ b/include/hpp/fcl/eigen/vec_3fx.h
@@ -322,10 +322,6 @@ public:
return *this;
}
- inline T length() const { return this->norm(); }
- // inline T norm() const { return sqrt(details::dot_prod3(data, data)); }
- inline T sqrLength() const { return this->squaredNorm(); }
- // inline T squaredNorm() const { return details::dot_prod3(data, data); }
inline void setValue(T x, T y, T z) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(FclMatrix, 3);
this->m_storage.data()[0] = x;
@@ -548,9 +544,6 @@ public:
return typename UnaryReturnType<EigenOp>::Abs (*this);
}
- inline Scalar length() const { return this->norm(); }
- inline Scalar sqrLength() const { return this->squaredNorm(); }
-
template <typename Derived>
inline bool equal(const Eigen::MatrixBase<Derived>& other, T epsilon = std::numeric_limits<T>::epsilon() * 100) const
{
diff --git a/include/hpp/fcl/math/vec_3fx.h b/include/hpp/fcl/math/vec_3fx.h
index e49cba19532350572777a24d09dd93f79b3ad621..f7dee084f15f333970ad30b15e9c86b07282c04e 100644
--- a/include/hpp/fcl/math/vec_3fx.h
+++ b/include/hpp/fcl/math/vec_3fx.h
@@ -127,9 +127,7 @@ public:
return *this;
}
- inline U length() const { return sqrt(details::dot_prod3(data, data)); }
inline U norm() const { return sqrt(details::dot_prod3(data, data)); }
- inline U sqrLength() const { return details::dot_prod3(data, data); }
inline U squaredNorm() const { return details::dot_prod3(data, data); }
inline void setValue(U x, U y, U z) { data.setValue(x, y, z); }
inline void setValue(U x) { data.setValue(x); }
diff --git a/include/hpp/fcl/math/vec_nf.h b/include/hpp/fcl/math/vec_nf.h
deleted file mode 100644
index 05ff8617c10b7fb6bb6db81ffeabfe70b3e2ace0..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/math/vec_nf.h
+++ /dev/null
@@ -1,231 +0,0 @@
-#ifndef FCL_MATH_VEC_NF_H
-#define FCL_MATH_VEC_NF_H
-
-#include <cmath>
-#include <iostream>
-#include <limits>
-#include <vector>
-#include <boost/array.hpp>
-#include <cstdarg>
-#include <hpp/fcl/data_types.h>
-
-namespace fcl
-{
-
-template<typename T, std::size_t N>
-class Vec_n
-{
-public:
- Vec_n()
- {
- data.resize(N, 0);
- }
-
- template<std::size_t M>
- Vec_n(const Vec_n<T, M>& data_)
- {
- std::size_t n = std::min(M, N);
- for(std::size_t i = 0; i < n; ++i)
- data[i] = data_[i];
- }
-
- Vec_n(const std::vector<T>& data_)
- {
- data.resize(N, 0);
- std::size_t n = std::min(data_.size(), N);
- for(std::size_t i = 0; i < n; ++i)
- data[i] = data_[i];
- }
-
- bool operator == (const Vec_n<T, N>& other) const
- {
- for(std::size_t i = 0; i < N; ++i)
- {
- if(data[i] != other[i]) return false;
- }
- return true;
- }
-
- bool operator != (const Vec_n<T, N>& other) const
- {
- for(std::size_t i = 0; i < N; ++i)
- {
- if(data[i] != other[i]) return true;
- }
-
- return false;
- }
-
-
- std::size_t dim() const
- {
- return N;
- }
-
- void setData(const std::vector<T>& data_)
- {
- std::size_t n = std::min(data.size(), N);
- for(std::size_t i = 0; i < n; ++i)
- data[i] = data_[i];
- }
-
- T operator [] (std::size_t i) const
- {
- return data[i];
- }
-
- T& operator [] (std::size_t i)
- {
- return data[i];
- }
-
- Vec_n<T, N> operator + (const Vec_n<T, N>& other) const
- {
- Vec_n<T, N> result;
- for(std::size_t i = 0; i < N; ++i)
- result[i] = data[i] + other[i];
- return result;
- }
-
- Vec_n<T, N>& operator += (const Vec_n<T, N>& other)
- {
- for(std::size_t i = 0; i < N; ++i)
- data[i] += other[i];
- return *this;
- }
-
- Vec_n<T, N> operator - (const Vec_n<T, N>& other) const
- {
- Vec_n<T, N> result;
- for(std::size_t i = 0; i < N; ++i)
- result[i] = data[i] - other[i];
- return result;
- }
-
- Vec_n<T, N>& operator -= (const Vec_n<T, N>& other)
- {
- for(std::size_t i = 0; i < N; ++i)
- data[i] -= other[i];
- return *this;
- }
-
- Vec_n<T, N> operator * (T t) const
- {
- Vec_n<T, N> result;
- for(std::size_t i = 0; i < N; ++i)
- result[i] = data[i] * t;
- return result;
- }
-
- Vec_n<T, N>& operator *= (T t)
- {
- for(std::size_t i = 0; i < N; ++i)
- data[i] *= t;
- return *this;
- }
-
- Vec_n<T, N> operator / (T t) const
- {
- Vec_n<T, N> result;
- for(std::size_t i = 0; i < N; ++i)
- result[i] = data[i] / 5;
- return result;
- }
-
- Vec_n<T, N>& operator /= (T t)
- {
- for(std::size_t i = 0; i < N; ++i)
- data[i] /= t;
- return *this;
- }
-
- Vec_n<T, N>& setZero()
- {
- for(std::size_t i = 0; i < N; ++i)
- data[i] = 0;
- }
-
- T dot(const Vec_n<T, N>& other) const
- {
- T sum = 0;
- for(std::size_t i = 0; i < N; ++i)
- sum += data[i] * other[i];
- return sum;
- }
-
- std::vector<T> getData() const
- {
- return data;
- }
-
-protected:
- std::vector<T> data;
-};
-
-template<typename T1, std::size_t N1,
- typename T2, std::size_t N2>
-void repack(const Vec_n<T1, N1>& input,
- Vec_n<T2, N2>& output)
-{
- std::size_t n = std::min(N1, N2);
- for(std::size_t i = 0; i < n; ++i)
- output[i] = input[i];
-}
-
-template<typename T, std::size_t N>
-Vec_n<T, N> operator * (T t, const Vec_n<T, N>& v)
-{
- return v * t;
-}
-
-template<typename T, std::size_t N, std::size_t M>
-Vec_n<T, M+N> combine(const Vec_n<T, N>& v1, const Vec_n<T, M>& v2)
-{
- Vec_n<T, M+N> v;
- for(std::size_t i = 0; i < N; ++i)
- v[i] = v1[i];
- for(std::size_t i = 0; i < M; ++i)
- v[i + N] = v2[i];
-
- return v;
-}
-
-template<typename T, std::size_t N>
-std::ostream& operator << (std::ostream& o, const Vec_n<T, N>& v)
-{
- o << "(";
- for(std::size_t i = 0; i < N; ++i)
- {
- if(i == N - 1)
- o << v[i] << ")";
- else
- o << v[i] << " ";
- }
- return o;
-}
-
-// workaround for template alias
-template<std::size_t N>
-class Vecnf : public Vec_n<FCL_REAL, N>
-{
-public:
- Vecnf(const Vec_n<FCL_REAL, N>& other) : Vec_n<FCL_REAL, N>(other)
- {}
-
- Vecnf() : Vec_n<FCL_REAL, N>()
- {}
-
- template<std::size_t M>
- Vecnf(const Vec_n<FCL_REAL, M>& other) : Vec_n<FCL_REAL, N>(other)
- {}
-
- Vecnf(const std::vector<FCL_REAL>& data_) : Vec_n<FCL_REAL, N>(data_)
- {}
-
-
-};
-
-
-}
-
-#endif
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index eb308a1ce9f81d77d4f36d99ec2cc2f6f110ce85..482afc76098e186d5107dff1e948aaf18302ecf5 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -615,7 +615,7 @@ struct GJKSolver_indep
w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
- if(distance) *distance = (w0 - w1).length();
+ if(distance) *distance = (w0 - w1).norm();
if(p1) *p1 = tf1.transform (w0);
if(p2) *p2 = tf1.transform (w1);
@@ -667,7 +667,7 @@ struct GJKSolver_indep
w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
- if(distance) *distance = (w0 - w1).length();
+ if(distance) *distance = (w0 - w1).norm();
if(p1) *p1 = w0;
if(p2) *p2 = shape.toshape0.transform(w1);
return true;
@@ -717,7 +717,7 @@ struct GJKSolver_indep
w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
- if(distance) *distance = (w0 - w1).length();
+ if(distance) *distance = (w0 - w1).norm();
if(p1) *p1 = tf1.transform(w0);
if(p2) *p2 = tf1.transform(w1);
return true;
diff --git a/include/hpp/fcl/octree.h b/include/hpp/fcl/octree.h
index 7f223d1c534a5addb901094a324ded1fc8be6e33..d8d9e8a7eab9ddee549d8ab0eec59ea67656721c 100644
--- a/include/hpp/fcl/octree.h
+++ b/include/hpp/fcl/octree.h
@@ -94,7 +94,7 @@ public:
{
aabb_local = getRootBV();
aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ aabb_radius = (aabb_local.min_ - aabb_center).norm();
}
/// @brief get the bounding volume for the root
diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h
index 6c8717ad522ca2edd5c6b93e10694205ca1cdf00..9c76ec11b5ad22e7450b2de916ed34bfb2a7553e 100644
--- a/include/hpp/fcl/shape/geometric_shapes.h
+++ b/include/hpp/fcl/shape/geometric_shapes.h
@@ -369,9 +369,8 @@ public:
int e_second = index[(j+1)%*points_in_poly];
const Vec3f& v1 = points[e_first];
const Vec3f& v2 = points[e_second];
- FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
Matrix3f A(v1, v2, v3); // this is A' in the original document
- C += transpose(A) * C_canonical * A * d_six_vol; // change accordingly
+ C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
}
points_in_poly += (*points_in_poly + 1);
diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp
index c89883caca70f6dc92aa669a666ba820f479b95a..b88227f7b421eb4e11269d53126d64ae5f30d8a1 100644
--- a/src/BV/OBB.cpp
+++ b/src/BV/OBB.cpp
@@ -599,7 +599,7 @@ OBB OBB::operator + (const OBB& other) const
Vec3f center_diff = To - other.To;
FCL_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
FCL_REAL max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]);
- if(center_diff.length() > 2 * (max_extent + max_extent2))
+ if(center_diff.norm() > 2 * (max_extent + max_extent2))
{
return merge_largedist(*this, other);
}
diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp
index 197fc776419ab5465eed66cd34af080c4d69b783..b78323e98ff2471b97e6f724812ec3d3f459a623 100644
--- a/src/BV/RSS.cpp
+++ b/src/BV/RSS.cpp
@@ -212,7 +212,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -241,7 +241,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -269,7 +269,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -297,7 +297,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -365,7 +365,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -393,7 +393,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -423,7 +423,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
}
- return S.length();
+ return S.norm();
}
}
@@ -451,7 +451,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -519,7 +519,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -547,7 +547,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -575,7 +575,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -603,7 +603,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -664,7 +664,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -692,7 +692,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -721,7 +721,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -749,7 +749,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*Q = S + (*P);
}
- return S.length();
+ return S.norm();
}
}
@@ -889,20 +889,20 @@ bool RSS::contain(const Vec3f& p) const
{
FCL_REAL y = (proj1 > 0) ? l[1] : 0;
Vec3f v(proj0, y, 0);
- return ((proj - v).sqrLength() < r * r);
+ return ((proj - v).squaredNorm() < r * r);
}
else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
{
FCL_REAL x = (proj0 > 0) ? l[0] : 0;
Vec3f v(x, proj1, 0);
- return ((proj - v).sqrLength() < r * r);
+ return ((proj - v).squaredNorm() < r * r);
}
else
{
FCL_REAL x = (proj0 > 0) ? l[0] : 0;
FCL_REAL y = (proj1 > 0) ? l[1] : 0;
Vec3f v(x, y, 0);
- return ((proj - v).sqrLength() < r * r);
+ return ((proj - v).squaredNorm() < r * r);
}
}
@@ -934,7 +934,7 @@ RSS& RSS::operator += (const Vec3f& p)
{
FCL_REAL y = (proj1 > 0) ? l[1] : 0;
Vec3f v(proj0, y, 0);
- FCL_REAL new_r_sqr = (proj - v).sqrLength();
+ FCL_REAL new_r_sqr = (proj - v).squaredNorm();
if(new_r_sqr < r * r)
; // do nothing
else
@@ -964,7 +964,7 @@ RSS& RSS::operator += (const Vec3f& p)
{
FCL_REAL x = (proj0 > 0) ? l[0] : 0;
Vec3f v(x, proj1, 0);
- FCL_REAL new_r_sqr = (proj - v).sqrLength();
+ FCL_REAL new_r_sqr = (proj - v).squaredNorm();
if(new_r_sqr < r * r)
; // do nothing
else
@@ -995,7 +995,7 @@ RSS& RSS::operator += (const Vec3f& p)
FCL_REAL x = (proj0 > 0) ? l[0] : 0;
FCL_REAL y = (proj1 > 0) ? l[1] : 0;
Vec3f v(x, y, 0);
- FCL_REAL new_r_sqr = (proj - v).sqrLength();
+ FCL_REAL new_r_sqr = (proj - v).squaredNorm();
if(new_r_sqr < r * r)
; // do nothing
else
diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp
index 9a1ae33e743cbb443e9598f1d287c4a7446da2b1..acc8150dedc78024144d6659c42833b713a4bb06 100644
--- a/src/BV/kIOS.cpp
+++ b/src/BV/kIOS.cpp
@@ -51,7 +51,7 @@ bool kIOS::overlap(const kIOS& other) const
{
for(unsigned int j = 0; j < other.num_spheres; ++j)
{
- FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).sqrLength();
+ FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
FCL_REAL sum_r = spheres[i].r + other.spheres[j].r;
if(o_dist > sum_r * sum_r)
return false;
@@ -74,7 +74,7 @@ bool kIOS::contain(const Vec3f& p) const
for(unsigned int i = 0; i < num_spheres; ++i)
{
FCL_REAL r = spheres[i].r;
- if((spheres[i].o - p).sqrLength() > r * r)
+ if((spheres[i].o - p).squaredNorm() > r * r)
return false;
}
@@ -86,7 +86,7 @@ kIOS& kIOS::operator += (const Vec3f& p)
for(unsigned int i = 0; i < num_spheres; ++i)
{
FCL_REAL r = spheres[i].r;
- FCL_REAL new_r_sqr = (p - spheres[i].o).sqrLength();
+ FCL_REAL new_r_sqr = (p - spheres[i].o).squaredNorm();
if(new_r_sqr > r * r)
{
spheres[i].r = sqrt(new_r_sqr);
@@ -146,7 +146,7 @@ FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const
{
for(unsigned int j = 0; j < other.num_spheres; ++j)
{
- FCL_REAL d = (spheres[i].o - other.spheres[j].o).length() - (spheres[i].r + other.spheres[j].r);
+ FCL_REAL d = (spheres[i].o - other.spheres[j].o).norm() - (spheres[i].r + other.spheres[j].r);
if(d_max < d)
{
d_max = d;
@@ -163,7 +163,7 @@ FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const
if(id_a != -1 && id_b != -1)
{
Vec3f v = spheres[id_a].o - spheres[id_b].o;
- FCL_REAL len_v = v.length();
+ FCL_REAL len_v = v.norm();
*P = spheres[id_a].o - v * (spheres[id_a].r / len_v);
*Q = spheres[id_b].o + v * (spheres[id_b].r / len_v);
}
diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp
index 1aa211b3b6503832d1ede49a5567e1ccc728f33c..99b4baf69dd67bff949d06755885fb024e03239f 100644
--- a/src/BVH/BVH_model.cpp
+++ b/src/BVH/BVH_model.cpp
@@ -868,7 +868,7 @@ void BVHModel<BV>::computeLocalAABB()
aabb_radius = 0;
for(int i = 0; i < num_vertices; ++i)
{
- FCL_REAL r = (aabb_center - vertices[i]).sqrLength();
+ FCL_REAL r = (aabb_center - vertices[i]).squaredNorm();
if(r > aabb_radius) aabb_radius = r;
}
diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp
index 0ccc22d6240b2b7a762fe6e14fea320841aa843c..36f5157408ee189b2a4d3034c089909b806943ff 100644
--- a/src/BVH/BVH_utility.cpp
+++ b/src/BVH/BVH_utility.cpp
@@ -277,7 +277,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
radsqr = r * r;
cz = (FCL_REAL)0.5 * (maxz + minz);
- // compute an initial length of rectangle along x direction
+ // compute an initial norm of rectangle along x direction
// find minx and maxx as starting points
@@ -332,7 +332,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
}
}
- // compute an initial length of rectangle along y direction
+ // compute an initial norm of rectangle along y direction
// find miny and maxy as starting points
@@ -607,11 +607,11 @@ void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec
{
Vec3f e1 = a - c;
Vec3f e2 = b - c;
- FCL_REAL e1_len2 = e1.sqrLength();
- FCL_REAL e2_len2 = e2.sqrLength();
+ FCL_REAL e1_len2 = e1.squaredNorm();
+ FCL_REAL e2_len2 = e2.squaredNorm();
Vec3f e3 = e1.cross(e2);
- FCL_REAL e3_len2 = e3.sqrLength();
- radius = e1_len2 * e2_len2 * (e1 - e2).sqrLength() / e3_len2;
+ FCL_REAL e3_len2 = e3.squaredNorm();
+ radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2;
radius = std::sqrt(radius) * 0.5;
center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c;
@@ -634,7 +634,7 @@ static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
int point_id = t[j];
const Vec3f& p = ps[point_id];
- FCL_REAL d = (p - query).sqrLength();
+ FCL_REAL d = (p - query).squaredNorm();
if(d > maxD) maxD = d;
}
@@ -645,7 +645,7 @@ static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
int point_id = t[j];
const Vec3f& p = ps2[point_id];
- FCL_REAL d = (p - query).sqrLength();
+ FCL_REAL d = (p - query).squaredNorm();
if(d > maxD) maxD = d;
}
}
@@ -665,13 +665,13 @@ static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, unsigne
int index = indirect_index ? indices[i] : i;
const Vec3f& p = ps[index];
- FCL_REAL d = (p - query).sqrLength();
+ FCL_REAL d = (p - query).squaredNorm();
if(d > maxD) maxD = d;
if(ps2)
{
const Vec3f& v = ps2[index];
- FCL_REAL d = (v - query).sqrLength();
+ FCL_REAL d = (v - query).squaredNorm();
if(d > maxD) maxD = d;
}
}
diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp
index 5240c429ff418438887447b152922f474d5bb3e6..91b4ac6bc0ddead35c28f7d4a5f0850149ec0889 100644
--- a/src/BVH/BV_fitter.cpp
+++ b/src/BVH/BV_fitter.cpp
@@ -82,7 +82,7 @@ void fit2(Vec3f* ps, OBB& bv)
const Vec3f& p1 = ps[0];
const Vec3f& p2 = ps[1];
Vec3f p1p2 = p1 - p2;
- FCL_REAL len_p1p2 = p1p2.length();
+ FCL_REAL len_p1p2 = p1p2.norm();
p1p2.normalize();
bv.axis[0] = p1p2;
@@ -104,9 +104,9 @@ void fit3(Vec3f* ps, OBB& bv)
e[1] = p2 - p3;
e[2] = p3 - p1;
FCL_REAL len[3];
- len[0] = e[0].sqrLength();
- len[1] = e[1].sqrLength();
- len[2] = e[2].sqrLength();
+ len[0] = e[0].squaredNorm();
+ len[1] = e[1].squaredNorm();
+ len[2] = e[2].squaredNorm();
int imax = 0;
if(len[1] > len[0]) imax = 1;
@@ -169,7 +169,7 @@ void fit2(Vec3f* ps, RSS& bv)
const Vec3f& p1 = ps[0];
const Vec3f& p2 = ps[1];
Vec3f p1p2 = p1 - p2;
- FCL_REAL len_p1p2 = p1p2.length();
+ FCL_REAL len_p1p2 = p1p2.norm();
p1p2.normalize();
bv.axis[0] = p1p2;
@@ -191,9 +191,9 @@ void fit3(Vec3f* ps, RSS& bv)
e[1] = p2 - p3;
e[2] = p3 - p1;
FCL_REAL len[3];
- len[0] = e[0].sqrLength();
- len[1] = e[1].sqrLength();
- len[2] = e[2].sqrLength();
+ len[0] = e[0].squaredNorm();
+ len[1] = e[1].squaredNorm();
+ len[2] = e[2].squaredNorm();
int imax = 0;
if(len[1] > len[0]) imax = 1;
@@ -259,7 +259,7 @@ void fit2(Vec3f* ps, kIOS& bv)
const Vec3f& p1 = ps[0];
const Vec3f& p2 = ps[1];
Vec3f p1p2 = p1 - p2;
- FCL_REAL len_p1p2 = p1p2.length();
+ FCL_REAL len_p1p2 = p1p2.norm();
p1p2.normalize();
Vec3f* axis = bv.obb.axis;
@@ -300,9 +300,9 @@ void fit3(Vec3f* ps, kIOS& bv)
e[1] = p2 - p3;
e[2] = p3 - p1;
FCL_REAL len[3];
- len[0] = e[0].sqrLength();
- len[1] = e[1].sqrLength();
- len[2] = e[2].sqrLength();
+ len[0] = e[0].squaredNorm();
+ len[1] = e[1].squaredNorm();
+ len[2] = e[2].squaredNorm();
int imax = 0;
if(len[1] > len[0]) imax = 1;
diff --git a/src/ccd/motion.cpp b/src/ccd/motion.cpp
index a0fb85369a29b00815075db8e1f7c54e30d9947a..6ef952cb8b2977db2bcb0590ad33e4ce0abef99a 100644
--- a/src/ccd/motion.cpp
+++ b/src/ccd/motion.cpp
@@ -63,22 +63,22 @@ FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const SplineMotion& motion) const
if(tmp > cn_max) cn_max = tmp;
// max_i ||c_i||
- FCL_REAL cmax = c1.sqrLength();
- tmp = c2.sqrLength();
+ FCL_REAL cmax = c1.squaredNorm();
+ tmp = c2.squaredNorm();
if(tmp > cmax) cmax = tmp;
- tmp = c3.sqrLength();
+ tmp = c3.squaredNorm();
if(tmp > cmax) cmax = tmp;
- tmp = c4.sqrLength();
+ tmp = c4.squaredNorm();
if(tmp > cmax) cmax = tmp;
cmax = sqrt(cmax);
// max_i ||c_i x n||
- FCL_REAL cxn_max = (c1.cross(n)).sqrLength();
- tmp = (c2.cross(n)).sqrLength();
+ FCL_REAL cxn_max = (c1.cross(n)).squaredNorm();
+ tmp = (c2.cross(n)).squaredNorm();
if(tmp > cxn_max) cxn_max = tmp;
- tmp = (c3.cross(n)).sqrLength();
+ tmp = (c3.cross(n)).squaredNorm();
if(tmp > cxn_max) cxn_max = tmp;
- tmp = (c4.cross(n)).sqrLength();
+ tmp = (c4.cross(n)).squaredNorm();
if(tmp > cxn_max) cxn_max = tmp;
cxn_max = sqrt(cxn_max);
@@ -99,10 +99,10 @@ FCL_REAL TriangleMotionBoundVisitor::visit(const SplineMotion& motion) const
FCL_REAL T_bound = motion.computeTBound(n);
FCL_REAL tf_t = motion.getCurrentTime();
- FCL_REAL R_bound = std::fabs(a.dot(n)) + a.length() + (a.cross(n)).length();
- FCL_REAL R_bound_tmp = std::fabs(b.dot(n)) + b.length() + (b.cross(n)).length();
+ FCL_REAL R_bound = std::fabs(a.dot(n)) + a.norm() + (a.cross(n)).norm();
+ FCL_REAL R_bound_tmp = std::fabs(b.dot(n)) + b.norm() + (b.cross(n)).norm();
if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
- R_bound_tmp = std::fabs(c.dot(n)) + c.length() + (c.cross(n)).length();
+ R_bound_tmp = std::fabs(c.dot(n)) + c.norm() + (c.cross(n)).norm();
if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
FCL_REAL dWdW_max = motion.computeDWMax();
@@ -156,7 +156,7 @@ bool SplineMotion::integrate(double dt) const
Vec3f cur_T = Td[0] * getWeight0(dt) + Td[1] * getWeight1(dt) + Td[2] * getWeight2(dt) + Td[3] * getWeight3(dt);
Vec3f cur_w = Rd[0] * getWeight0(dt) + Rd[1] * getWeight1(dt) + Rd[2] * getWeight2(dt) + Rd[3] * getWeight3(dt);
- FCL_REAL cur_angle = cur_w.length();
+ FCL_REAL cur_angle = cur_w.norm();
cur_w.normalize();
Quaternion3f cur_q;
@@ -325,20 +325,20 @@ FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const ScrewMotion& motion) const
FCL_REAL angular_vel = motion.getAngularVelocity();
const Vec3f& p = motion.getAxisOrigin();
- FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr)).cross(axis)).sqrLength();
+ FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr)).cross(axis)).squaredNorm();
FCL_REAL tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0])).cross(axis)).sqrLength();
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0])).cross(axis)).squaredNorm();
if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1])).cross(axis)).squaredNorm();
if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1])).cross(axis)).squaredNorm();
if(tmp > c_proj_max) c_proj_max = tmp;
c_proj_max = sqrt(c_proj_max);
FCL_REAL v_dot_n = axis.dot(n) * linear_vel;
- FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
- FCL_REAL origin_proj = ((tf.getTranslation() - p).cross(axis)).length();
+ FCL_REAL w_cross_n = (axis.cross(n)).norm() * angular_vel;
+ FCL_REAL origin_proj = ((tf.getTranslation() - p).cross(axis)).norm();
FCL_REAL mu = v_dot_n + w_cross_n * (c_proj_max + bv.r + origin_proj);
@@ -355,17 +355,17 @@ FCL_REAL TriangleMotionBoundVisitor::visit(const ScrewMotion& motion) const
FCL_REAL angular_vel = motion.getAngularVelocity();
const Vec3f& p = motion.getAxisOrigin();
- FCL_REAL proj_max = ((tf.getQuatRotation().transform(a) + tf.getTranslation() - p).cross(axis)).sqrLength();
+ FCL_REAL proj_max = ((tf.getQuatRotation().transform(a) + tf.getTranslation() - p).cross(axis)).squaredNorm();
FCL_REAL tmp;
- tmp = ((tf.getQuatRotation().transform(b) + tf.getTranslation() - p).cross(axis)).sqrLength();
+ tmp = ((tf.getQuatRotation().transform(b) + tf.getTranslation() - p).cross(axis)).squaredNorm();
if(tmp > proj_max) proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(c) + tf.getTranslation() - p).cross(axis)).sqrLength();
+ tmp = ((tf.getQuatRotation().transform(c) + tf.getTranslation() - p).cross(axis)).squaredNorm();
if(tmp > proj_max) proj_max = tmp;
proj_max = std::sqrt(proj_max);
FCL_REAL v_dot_n = axis.dot(n) * linear_vel;
- FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
+ FCL_REAL w_cross_n = (axis.cross(n)).norm() * angular_vel;
FCL_REAL mu = v_dot_n + w_cross_n * proj_max;
return mu;
@@ -390,19 +390,19 @@ FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const InterpMotion& motion) const
FCL_REAL angular_vel = motion.getAngularVelocity();
const Vec3f& linear_vel = motion.getLinearVelocity();
- FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr - reference_p)).cross(angular_axis)).sqrLength();
+ FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr - reference_p)).cross(angular_axis)).squaredNorm();
FCL_REAL tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] - reference_p)).cross(angular_axis)).sqrLength();
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] - reference_p)).cross(angular_axis)).squaredNorm();
if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).squaredNorm();
if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).squaredNorm();
if(tmp > c_proj_max) c_proj_max = tmp;
c_proj_max = std::sqrt(c_proj_max);
FCL_REAL v_dot_n = linear_vel.dot(n);
- FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
+ FCL_REAL w_cross_n = (angular_axis.cross(n)).norm() * angular_vel;
FCL_REAL mu = v_dot_n + w_cross_n * (bv.r + c_proj_max);
return mu;
@@ -422,17 +422,17 @@ FCL_REAL TriangleMotionBoundVisitor::visit(const InterpMotion& motion) const
FCL_REAL angular_vel = motion.getAngularVelocity();
const Vec3f& linear_vel = motion.getLinearVelocity();
- FCL_REAL proj_max = ((tf.getQuatRotation().transform(a - reference_p)).cross(angular_axis)).sqrLength();
+ FCL_REAL proj_max = ((tf.getQuatRotation().transform(a - reference_p)).cross(angular_axis)).squaredNorm();
FCL_REAL tmp;
- tmp = ((tf.getQuatRotation().transform(b - reference_p)).cross(angular_axis)).sqrLength();
+ tmp = ((tf.getQuatRotation().transform(b - reference_p)).cross(angular_axis)).squaredNorm();
if(tmp > proj_max) proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(c - reference_p)).cross(angular_axis)).sqrLength();
+ tmp = ((tf.getQuatRotation().transform(c - reference_p)).cross(angular_axis)).squaredNorm();
if(tmp > proj_max) proj_max = tmp;
proj_max = std::sqrt(proj_max);
FCL_REAL v_dot_n = linear_vel.dot(n);
- FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
+ FCL_REAL w_cross_n = (angular_axis.cross(n)).norm() * angular_vel;
FCL_REAL mu = v_dot_n + w_cross_n * proj_max;
return mu;
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index c5e1e645568efc9ca89bfb39a0b5df5902e27f88..01ed3bc95294944fc6f4cb4f615c0cc55e8e1fb8 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -218,7 +218,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
contact.pos = .5*(p1+p2);
- contact.normal = (p2-p1)/(p2-p1).length ();
+ contact.normal = (p2-p1)/(p2-p1).norm ();
result.addContact (contact);
return 1;
}
diff --git a/src/distance_capsule_capsule.cpp b/src/distance_capsule_capsule.cpp
index 7239066a4f4b2985ef8926c1e09185de5acf320e..efb6389560c13ba84506f51faf58395abb6c2c04 100644
--- a/src/distance_capsule_capsule.cpp
+++ b/src/distance_capsule_capsule.cpp
@@ -47,7 +47,7 @@ namespace fcl {
FCL_REAL a01 = -direction1.dot (direction2);
FCL_REAL b0 = diff.dot (direction1);
FCL_REAL b1 = -diff.dot (direction2);
- FCL_REAL c = diff.sqrLength ();
+ FCL_REAL c = diff.squaredNorm ();
FCL_REAL det = fabs (1.0 - a01*a01);
FCL_REAL s1, s2, sqrDist, extDet0, extDet1, tmpS0, tmpS1;
FCL_REAL epsilon = std::numeric_limits<FCL_REAL>::epsilon () * 100;
@@ -350,7 +350,7 @@ namespace fcl {
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
contact.pos = .5*(p1+p2);
- contact.normal = (p2-p1)/(p2-p1).length ();
+ contact.normal = (p2-p1)/(p2-p1).norm ();
result.addContact (contact);
return 1;
}
diff --git a/src/intersect.cpp b/src/intersect.cpp
index 10a57248179f7a43075094468347e46c69402df2..b4ac609a2a5a91f5f4e26ecd7572e2fc5ef6e861 100644
--- a/src/intersect.cpp
+++ b/src/intersect.cpp
@@ -1036,7 +1036,7 @@ void Intersect::clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polyg
clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp);
if(num_clipped_points_ > 0)
{
- if((tmp - clipped_points[num_clipped_points_ - 1]).sqrLength() > EPSILON)
+ if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > EPSILON)
{
clipped_points[num_clipped_points_] = tmp;
num_clipped_points_++;
@@ -1066,7 +1066,7 @@ void Intersect::clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polyg
clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp);
if(num_clipped_points_ > 0)
{
- if((tmp - clipped_points[num_clipped_points_ - 1]).sqrLength() > EPSILON)
+ if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > EPSILON)
{
clipped_points[num_clipped_points_] = tmp;
num_clipped_points_++;
@@ -1086,7 +1086,7 @@ void Intersect::clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polyg
if(num_clipped_points_ > 2)
{
- if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).sqrLength() < EPSILON)
+ if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).squaredNorm() < EPSILON)
{
num_clipped_points_--;
}
@@ -1316,7 +1316,7 @@ FCL_REAL TriangleDistance::sqrTriDistance
FCL_REAL mindd;
int shown_disjoint = 0;
- mindd = (S[0] - T[0]).sqrLength() + 1; // Set first minimum safely high
+ mindd = (S[0] - T[0]).squaredNorm() + 1; // Set first minimum safely high
for(int i = 0; i < 3; ++i)
{
@@ -1433,7 +1433,7 @@ FCL_REAL TriangleDistance::sqrTriDistance
// the T triangle; the other point is on the face of S
P = T[point] + Sn * (Tp[point] / Snl);
Q = T[point];
- return (P - Q).sqrLength();
+ return (P - Q).squaredNorm();
}
}
}
@@ -1489,7 +1489,7 @@ FCL_REAL TriangleDistance::sqrTriDistance
{
P = S[point];
Q = S[point] + Tn * (Sp[point] / Tnl);
- return (P - Q).sqrLength();
+ return (P - Q).squaredNorm();
}
}
}
@@ -1583,16 +1583,16 @@ Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b, cons
ProjectResult res;
const Vec3f d = b - a;
- const FCL_REAL l = d.sqrLength();
+ const FCL_REAL l = d.squaredNorm();
if(l > 0)
{
const FCL_REAL t = (p - a).dot(d);
res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l));
res.parameterization[0] = 1 - res.parameterization[1];
- if(t >= l) { res.sqr_distance = (p - b).sqrLength(); res.encode = 2; /* 0x10 */ }
- else if(t <= 0) { res.sqr_distance = (p - a).sqrLength(); res.encode = 1; /* 0x01 */ }
- else { res.sqr_distance = (a + d * res.parameterization[1] - p).sqrLength(); res.encode = 3; /* 0x00 */ }
+ if(t >= l) { res.sqr_distance = (p - b).squaredNorm(); res.encode = 2; /* 0x10 */ }
+ else if(t <= 0) { res.sqr_distance = (p - a).squaredNorm(); res.encode = 1; /* 0x01 */ }
+ else { res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); res.encode = 3; /* 0x00 */ }
}
return res;
@@ -1606,7 +1606,7 @@ Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b,
const Vec3f* vt[] = {&a, &b, &c};
const Vec3f dl[] = {a - b, b - c, c - a};
const Vec3f& n = dl[0].cross(dl[1]);
- const FCL_REAL l = n.sqrLength();
+ const FCL_REAL l = n.squaredNorm();
if(l > 0)
{
@@ -1634,10 +1634,10 @@ Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b,
FCL_REAL d = (a - p).dot(n);
FCL_REAL s = sqrt(l);
Vec3f p_to_project = n * (d / l);
- mindist = p_to_project.sqrLength();
+ mindist = p_to_project.squaredNorm();
res.encode = 7; // m = 0x111
- res.parameterization[0] = dl[1].cross(b - p -p_to_project).length() / s;
- res.parameterization[1] = dl[2].cross(c - p -p_to_project).length() / s;
+ res.parameterization[0] = dl[1].cross(b - p -p_to_project).norm() / s;
+ res.parameterization[1] = dl[2].cross(c - p -p_to_project).norm() / s;
res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1];
}
@@ -1705,16 +1705,16 @@ Project::ProjectResult Project::projectLineOrigin(const Vec3f& a, const Vec3f& b
ProjectResult res;
const Vec3f d = b - a;
- const FCL_REAL l = d.sqrLength();
+ const FCL_REAL l = d.squaredNorm();
if(l > 0)
{
const FCL_REAL t = - a.dot(d);
res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l));
res.parameterization[0] = 1 - res.parameterization[1];
- if(t >= l) { res.sqr_distance = b.sqrLength(); res.encode = 2; /* 0x10 */ }
- else if(t <= 0) { res.sqr_distance = a.sqrLength(); res.encode = 1; /* 0x01 */ }
- else { res.sqr_distance = (a + d * res.parameterization[1]).sqrLength(); res.encode = 3; /* 0x00 */ }
+ if(t >= l) { res.sqr_distance = b.squaredNorm(); res.encode = 2; /* 0x10 */ }
+ else if(t <= 0) { res.sqr_distance = a.squaredNorm(); res.encode = 1; /* 0x01 */ }
+ else { res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); res.encode = 3; /* 0x00 */ }
}
return res;
@@ -1728,7 +1728,7 @@ Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, const Vec3
const Vec3f* vt[] = {&a, &b, &c};
const Vec3f dl[] = {a - b, b - c, c - a};
const Vec3f& n = dl[0].cross(dl[1]);
- const FCL_REAL l = n.sqrLength();
+ const FCL_REAL l = n.squaredNorm();
if(l > 0)
{
@@ -1756,10 +1756,10 @@ Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, const Vec3
FCL_REAL d = a.dot(n);
FCL_REAL s = sqrt(l);
Vec3f o_to_project = n * (d / l);
- mindist = o_to_project.sqrLength();
+ mindist = o_to_project.squaredNorm();
res.encode = 7; // m = 0x111
- res.parameterization[0] = dl[1].cross(b - o_to_project).length() / s;
- res.parameterization[1] = dl[2].cross(c - o_to_project).length() / s;
+ res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s;
+ res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s;
res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1];
}
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 8a11f832d07bceea74e93b8cbae47aa32a914fc0..6e01e2021ba875e23a48d857ef1ae7c6add6d57e 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -201,7 +201,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
simplices[0].rank = 0;
ray = guess;
- appendVertex(simplices[0], (ray.sqrLength() > 0) ? -ray : Vec3f(1, 0, 0));
+ appendVertex(simplices[0], (ray.squaredNorm() > 0) ? -ray : Vec3f(1, 0, 0));
simplices[0].p[0] = 1;
ray = simplices[0].c[0]->w;
lastw[0] = lastw[1] = lastw[2] = lastw[3] = ray; // cache previous support points, the new support point will compare with it to avoid too close support points
@@ -213,7 +213,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
Simplex& next_simplex = simplices[next];
// check A: when origin is near the existing simplex, stop
- FCL_REAL rl = ray.length();
+ FCL_REAL rl = ray.norm();
if(rl < tolerance) // mean origin is near the face of original simplex, return touch
{
status = Inside;
@@ -227,7 +227,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
bool found = false;
for(size_t i = 0; i < 4; ++i)
{
- if((w - lastw[i]).sqrLength() < tolerance)
+ if((w - lastw[i]).squaredNorm() < tolerance)
{
found = true; break;
}
@@ -294,7 +294,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
simplex = &simplices[current];
switch(status)
{
- case Valid: distance = ray.length(); break;
+ case Valid: distance = ray.norm(); break;
case Inside: distance = 0; break;
default: break;
}
@@ -352,7 +352,7 @@ bool GJK::encloseOrigin()
Vec3f axis;
axis[i] = 1;
Vec3f p = d.cross(axis);
- if(p.sqrLength() > 0)
+ if(p.squaredNorm() > 0)
{
appendVertex(*simplex, p);
if(encloseOrigin()) return true;
@@ -367,7 +367,7 @@ bool GJK::encloseOrigin()
case 3:
{
Vec3f n = (simplex->c[1]->w - simplex->c[0]->w).cross(simplex->c[2]->w - simplex->c[0]->w);
- if(n.sqrLength() > 0)
+ if(n.squaredNorm() > 0)
{
appendVertex(*simplex, n);
if(encloseOrigin()) return true;
@@ -416,13 +416,13 @@ bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist)
FCL_REAL b_dot_ba = b->w.dot(ba);
if(a_dot_ba > 0)
- dist = a->w.length();
+ dist = a->w.norm();
else if(b_dot_ba < 0)
- dist = b->w.length();
+ dist = b->w.norm();
else
{
FCL_REAL a_dot_b = a->w.dot(b->w);
- dist = std::sqrt(std::max(a->w.sqrLength() * b->w.sqrLength() - a_dot_b * a_dot_b, (FCL_REAL)0));
+ dist = std::sqrt(std::max(a->w.squaredNorm() * b->w.squaredNorm() - a_dot_b * a_dot_b, (FCL_REAL)0));
}
return true;
@@ -443,7 +443,7 @@ EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced)
face->c[1] = b;
face->c[2] = c;
face->n = (b->w - a->w).cross(c->w - a->w);
- FCL_REAL l = face->n.length();
+ FCL_REAL l = face->n.norm();
if(l > tolerance)
{
@@ -586,9 +586,9 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
result.c[0] = outer.c[0];
result.c[1] = outer.c[1];
result.c[2] = outer.c[2];
- result.p[0] = ((outer.c[1]->w - projection).cross(outer.c[2]->w - projection)).length();
- result.p[1] = ((outer.c[2]->w - projection).cross(outer.c[0]->w - projection)).length();
- result.p[2] = ((outer.c[0]->w - projection).cross(outer.c[1]->w - projection)).length();
+ result.p[0] = ((outer.c[1]->w - projection).cross(outer.c[2]->w - projection)).norm();
+ result.p[1] = ((outer.c[2]->w - projection).cross(outer.c[0]->w - projection)).norm();
+ result.p[2] = ((outer.c[0]->w - projection).cross(outer.c[1]->w - projection)).norm();
FCL_REAL sum = result.p[0] + result.p[1] + result.p[2];
result.p[0] /= sum;
@@ -600,7 +600,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
status = FallBack;
normal = -guess;
- FCL_REAL nl = normal.length();
+ FCL_REAL nl = normal.norm();
if(nl > 0) normal /= nl;
else normal = Vec3f(1, 0, 0);
depth = 0;
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index dfa5da3c10ff82a777a81dc5615a7b1d71d03c33..69b27476d210c20d513fe8320e341fafffa5f85e 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -84,7 +84,7 @@ bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1,
lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
Vec3f diff = s_c - segment_point;
- FCL_REAL distance = diff.length() - s1.radius - s2.radius;
+ FCL_REAL distance = diff.norm() - s1.radius - s2.radius;
if (distance > 0)
return false;
@@ -120,7 +120,7 @@ bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1,
lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
Vec3f diff = s_c - segment_point;
- FCL_REAL distance = diff.length() - s1.radius - s2.radius;
+ FCL_REAL distance = diff.norm() - s1.radius - s2.radius;
if(dist) *dist = distance;
@@ -143,7 +143,7 @@ bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
Vec3f diff = tf2.transform(Vec3f()) - tf1.transform(Vec3f());
- FCL_REAL len = diff.length();
+ FCL_REAL len = diff.norm();
if(len > s1.radius + s2.radius)
return false;
@@ -174,7 +174,7 @@ bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
Vec3f o1 = tf1.getTranslation();
Vec3f o2 = tf2.getTranslation();
Vec3f diff = o1 - o2;
- FCL_REAL len = diff.length();
+ FCL_REAL len = diff.norm();
FCL_REAL d (len > s1.radius + s2.radius);
if(dist) *dist = len - (s1.radius + s2.radius);
@@ -297,7 +297,7 @@ bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf,
if(has_contact)
{
Vec3f contact_to_center = contact_point - center;
- FCL_REAL distance_sqr = contact_to_center.sqrLength();
+ FCL_REAL distance_sqr = contact_to_center.squaredNorm();
if(distance_sqr < radius_with_threshold * radius_with_threshold)
{
@@ -334,12 +334,12 @@ bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
Vec3f diff = P1 - center;
Vec3f edge0 = P2 - P1;
Vec3f edge1 = P3 - P1;
- FCL_REAL a00 = edge0.sqrLength();
+ FCL_REAL a00 = edge0.squaredNorm();
FCL_REAL a01 = edge0.dot(edge1);
- FCL_REAL a11 = edge1.sqrLength();
+ FCL_REAL a11 = edge1.squaredNorm();
FCL_REAL b0 = diff.dot(edge0);
FCL_REAL b1 = diff.dot(edge1);
- FCL_REAL c = diff.sqrLength();
+ FCL_REAL c = diff.squaredNorm();
FCL_REAL det = fabs(a00*a11 - a01*a01);
FCL_REAL s = a01*b1 - a11*b0;
FCL_REAL t = a01*b0 - a00*b1;
@@ -911,7 +911,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
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 > 0) { *return_code = 0; return 0; }
n = Vec3f(0, -R(2, 0), R(1, 0));
- l = n.length();
+ l = n.norm();
if(l > eps)
{
s2 /= l;
@@ -929,7 +929,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
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 > 0) { *return_code = 0; return 0; }
n = Vec3f(0, -R(2, 1), R(1, 1));
- l = n.length();
+ l = n.norm();
if(l > eps)
{
s2 /= l;
@@ -947,7 +947,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
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 > 0) { *return_code = 0; return 0; }
n = Vec3f(0, -R(2, 2), R(1, 2));
- l = n.length();
+ l = n.norm();
if(l > eps)
{
s2 /= l;
@@ -966,7 +966,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
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 > 0) { *return_code = 0; return 0; }
n = Vec3f(R(2, 0), 0, -R(0, 0));
- l = n.length();
+ l = n.norm();
if(l > eps)
{
s2 /= l;
@@ -984,7 +984,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
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 > 0) { *return_code = 0; return 0; }
n = Vec3f(R(2, 1), 0, -R(0, 1));
- l = n.length();
+ l = n.norm();
if(l > eps)
{
s2 /= l;
@@ -1002,7 +1002,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
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 > 0) { *return_code = 0; return 0; }
n = Vec3f(R(2, 2), 0, -R(0, 2));
- l = n.length();
+ l = n.norm();
if(l > eps)
{
s2 /= l;
@@ -1021,7 +1021,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
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 > 0) { *return_code = 0; return 0; }
n = Vec3f(-R(1, 0), R(0, 0), 0);
- l = n.length();
+ l = n.norm();
if(l > eps)
{
s2 /= l;
@@ -1039,7 +1039,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
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 > 0) { *return_code = 0; return 0; }
n = Vec3f(-R(1, 1), R(0, 1), 0);
- l = n.length();
+ l = n.norm();
if(l > eps)
{
s2 /= l;
@@ -1057,7 +1057,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
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 > 0) { *return_code = 0; return 0; }
n = Vec3f(-R(1, 2), R(0, 2), 0);
- l = n.length();
+ l = n.norm();
if(l > eps)
{
s2 /= l;
@@ -1582,7 +1582,7 @@ bool cylinderHalfspaceIntersect(const Cylinder& s1, const Transform3f& tf1,
C = Vec3f(0, 0, 0);
else
{
- FCL_REAL s = C.length();
+ FCL_REAL s = C.norm();
s = s1.radius / s;
C *= s;
}
@@ -1635,7 +1635,7 @@ bool coneHalfspaceIntersect(const Cone& s1, const Transform3f& tf1,
C = Vec3f(0, 0, 0);
else
{
- FCL_REAL s = C.length();
+ FCL_REAL s = C.norm();
s = s1.radius / s;
C *= s;
}
@@ -1747,7 +1747,7 @@ bool planeHalfspaceIntersect(const Plane& s1, const Transform3f& tf1,
ret = 0;
Vec3f dir = (new_s1.n).cross(new_s2.n);
- FCL_REAL dir_norm = dir.sqrLength();
+ FCL_REAL dir_norm = dir.squaredNorm();
if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
{
if((new_s1.n).dot(new_s2.n) > 0)
@@ -1809,7 +1809,7 @@ bool halfspaceIntersect(const Halfspace& s1, const Transform3f& tf1,
ret = 0;
Vec3f dir = (new_s1.n).cross(new_s2.n);
- FCL_REAL dir_norm = dir.sqrLength();
+ FCL_REAL dir_norm = dir.squaredNorm();
if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
{
if((new_s1.n).dot(new_s2.n) > 0)
@@ -2129,7 +2129,7 @@ bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1,
C = Vec3f(0, 0, 0);
else
{
- FCL_REAL s = C.length();
+ FCL_REAL s = C.norm();
s = s1.radius / s;
C *= s;
}
@@ -2209,7 +2209,7 @@ bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1,
C = Vec3f(0, 0, 0);
else
{
- FCL_REAL s = C.length();
+ FCL_REAL s = C.norm();
s = s1.radius / s;
C *= s;
}
diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp
index fc3f056f88bcdae806366c737de5f796e8aeb2f6..5af26610fc7af09365fa2a5059827cc330e54216 100644
--- a/src/shape/geometric_shapes.cpp
+++ b/src/shape/geometric_shapes.cpp
@@ -100,7 +100,7 @@ void Convex::fillEdges()
void Halfspace::unitNormalTest()
{
- FCL_REAL l = n.length();
+ FCL_REAL l = n.norm();
if(l > 0)
{
FCL_REAL inv_l = 1.0 / l;
@@ -116,7 +116,7 @@ void Halfspace::unitNormalTest()
void Plane::unitNormalTest()
{
- FCL_REAL l = n.length();
+ FCL_REAL l = n.norm();
if(l > 0)
{
FCL_REAL inv_l = 1.0 / l;
@@ -135,7 +135,7 @@ void Box::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ aabb_radius = (aabb_local.min_ - aabb_center).norm();
}
void Sphere::computeLocalAABB()
@@ -149,49 +149,49 @@ void Capsule::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ aabb_radius = (aabb_local.min_ - aabb_center).norm();
}
void Cone::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ aabb_radius = (aabb_local.min_ - aabb_center).norm();
}
void Cylinder::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ aabb_radius = (aabb_local.min_ - aabb_center).norm();
}
void Convex::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ aabb_radius = (aabb_local.min_ - aabb_center).norm();
}
void Halfspace::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ aabb_radius = (aabb_local.min_ - aabb_center).norm();
}
void Plane::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ aabb_radius = (aabb_local.min_ - aabb_center).norm();
}
void TriangleP::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
- aabb_radius = (aabb_local.min_ - aabb_center).length();
+ aabb_radius = (aabb_local.min_ - aabb_center).norm();
}
diff --git a/test/test_fcl_eigen.cpp b/test/test_fcl_eigen.cpp
index 7db1958a888e2e2db5cc4ba5266d0e3739a8d641..70223d033db4c2c77e17745a99507bb26f28f85f 100644
--- a/test/test_fcl_eigen.cpp
+++ b/test/test_fcl_eigen.cpp
@@ -79,9 +79,9 @@ BOOST_AUTO_TEST_CASE(fcl_eigen_vec3fx)
// std::cout << d - 3.4 << std::endl;
// std::cout << d * 2 << std::endl;
// std::cout << d / 3 << std::endl;
- // std::cout << (d - 3.4).abs().sqrLength() << std::endl;
+ // std::cout << (d - 3.4).abs().squaredNorm() << std::endl;
- // std::cout << (((d - 3.4).abs() + 1) + 3).sqrLength() << std::endl;
+ // std::cout << (((d - 3.4).abs() + 1) + 3).squaredNorm() << std::endl;
PRINT_VECTOR(a)
PRINT_VECTOR(b)
PRINT_VECTOR(min(a,b))
@@ -107,11 +107,11 @@ BOOST_AUTO_TEST_CASE(fcl_eigen_matrix3fx)
a *= a;
PRINT_MATRIX(a);
- Matrix3fX b = a; b.inverse ();
+ Matrix3fX b = inverse(a);
a += a + a * b;
PRINT_MATRIX(a);
- b = a; b.inverse ();
+ b = inverse(a);
a.transpose ();
PRINT_MATRIX(a);
PRINT_MATRIX(a.transposeTimes (b));
diff --git a/test/test_fcl_math.cpp b/test/test_fcl_math.cpp
index 18ee66bb85251e3c17665ae62172d21294053f79..fb1dbd4ae2b67091deabadf3d6fcc4044e12e2e6 100644
--- a/test/test_fcl_math.cpp
+++ b/test/test_fcl_math.cpp
@@ -100,9 +100,9 @@ BOOST_AUTO_TEST_CASE(vec_test_basic_vec32)
BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
v1 = Vec3f32(3.0f, 4.0f, 5.0f);
- BOOST_CHECK(std::abs(v1.sqrLength() - 50.0) < 1e-5);
- BOOST_CHECK(std::abs(v1.length() - sqrt(50.0)) < 1e-5);
- BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
+ BOOST_CHECK(std::abs(v1.squaredNorm() - 50.0) < 1e-5);
+ BOOST_CHECK(std::abs(v1.norm() - sqrt(50.0)) < 1e-5);
+ BOOST_CHECK(normalize(v1).equal(v1 / v1.norm()));
}
BOOST_AUTO_TEST_CASE(vec_test_basic_vec64)
@@ -155,9 +155,9 @@ BOOST_AUTO_TEST_CASE(vec_test_basic_vec64)
BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
v1 = Vec3f64(3.0, 4.0, 5.0);
- BOOST_CHECK(std::abs(v1.sqrLength() - 50.0) < 1e-5);
- BOOST_CHECK(std::abs(v1.length() - sqrt(50.0)) < 1e-5);
- BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
+ BOOST_CHECK(std::abs(v1.squaredNorm() - 50.0) < 1e-5);
+ BOOST_CHECK(std::abs(v1.norm() - sqrt(50.0)) < 1e-5);
+ BOOST_CHECK(normalize(v1).equal(v1 / v1.norm()));
v1 = Vec3f64(1.0, 2.0, 3.0);
@@ -218,9 +218,9 @@ BOOST_AUTO_TEST_CASE(vec_test_eigen_vec32)
BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
v1 = Vec3f32(3.0f, 4.0f, 5.0f);
- BOOST_CHECK(std::abs(v1.sqrLength() - 50) < 1e-5);
- BOOST_CHECK(std::abs(v1.length() - sqrt(50)) < 1e-5);
- BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
+ BOOST_CHECK(std::abs(v1.squaredNorm() - 50) < 1e-5);
+ BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5);
+ BOOST_CHECK(normalize(v1).equal(v1 / v1.norm()));
}
BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64)
@@ -273,9 +273,9 @@ BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64)
BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
v1 = Vec3f64(3.0, 4.0, 5.0);
- BOOST_CHECK(std::abs(v1.sqrLength() - 50) < 1e-5);
- BOOST_CHECK(std::abs(v1.length() - sqrt(50)) < 1e-5);
- BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
+ BOOST_CHECK(std::abs(v1.squaredNorm() - 50) < 1e-5);
+ BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5);
+ BOOST_CHECK(normalize(v1).equal(v1 / v1.norm()));
v1 = Vec3f64(1.0, 2.0, 3.0);
@@ -309,8 +309,8 @@ BOOST_AUTO_TEST_CASE(eigen_mat32_consistent)
for(size_t j = 0; j < 3; ++j)
BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
- m3 = m1; m3.transpose();
- m4 = m2; m4.transpose();
+ m3 = transpose(m1);
+ m4 = transpose(m2);
for(size_t i = 0; i < 3; ++i)
for(size_t j = 0; j < 3; ++j)
@@ -323,8 +323,8 @@ BOOST_AUTO_TEST_CASE(eigen_mat32_consistent)
for(size_t j = 0; j < 3; ++j)
BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
- m3 = m1; m3.inverse();
- m4 = m2; m4.inverse();
+ m3 = inverse(m1);
+ m4 = inverse(m2);
for(size_t i = 0; i < 3; ++i)
for(size_t j = 0; j < 3; ++j)
@@ -458,8 +458,8 @@ BOOST_AUTO_TEST_CASE(vec_test_eigen_vec32_consistent)
BOOST_CHECK((v1 + delta1).equal(v1));
BOOST_CHECK((v3 + delta2).equal(v3));
- BOOST_CHECK(std::abs(v1.length() - v3.length()) < 1e-5);
- BOOST_CHECK(std::abs(v1.sqrLength() - v3.sqrLength()) < 1e-5);
+ BOOST_CHECK(std::abs(v1.norm() - v3.norm()) < 1e-5);
+ BOOST_CHECK(std::abs(v1.squaredNorm() - v3.squaredNorm()) < 1e-5);
v12 = v1; v12.negate();
v34 = v3; v34.negate();
@@ -606,8 +606,8 @@ BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64_consistent)
BOOST_CHECK((v1 + delta1).equal(v1));
BOOST_CHECK((v3 + delta2).equal(v3));
- BOOST_CHECK(std::abs(v1.length() - v3.length()) < 1e-5);
- BOOST_CHECK(std::abs(v1.sqrLength() - v3.sqrLength()) < 1e-5);
+ BOOST_CHECK(std::abs(v1.norm() - v3.norm()) < 1e-5);
+ BOOST_CHECK(std::abs(v1.squaredNorm() - v3.squaredNorm()) < 1e-5);
v12 = v1; v12.negate();
v34 = v3; v34.negate();
@@ -683,9 +683,9 @@ BOOST_AUTO_TEST_CASE(vec_test_sse_vec32)
BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
v1 = Vec3f32(3.0f, 4.0f, 5.0f);
- BOOST_CHECK(std::abs(v1.sqrLength() - 50) < 1e-5);
- BOOST_CHECK(std::abs(v1.length() - sqrt(50)) < 1e-5);
- BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
+ BOOST_CHECK(std::abs(v1.squaredNorm() - 50) < 1e-5);
+ BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5);
+ BOOST_CHECK(normalize(v1).equal(v1 / v1.norm()));
}
BOOST_AUTO_TEST_CASE(vec_test_sse_vec64)
@@ -738,9 +738,9 @@ BOOST_AUTO_TEST_CASE(vec_test_sse_vec64)
BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
v1 = Vec3f64(3.0, 4.0, 5.0);
- BOOST_CHECK(std::abs(v1.sqrLength() - 50) < 1e-5);
- BOOST_CHECK(std::abs(v1.length() - sqrt(50)) < 1e-5);
- BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
+ BOOST_CHECK(std::abs(v1.squaredNorm() - 50) < 1e-5);
+ BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5);
+ BOOST_CHECK(normalize(v1).equal(v1 / v1.norm()));
v1 = Vec3f64(1.0, 2.0, 3.0);
@@ -774,8 +774,8 @@ BOOST_AUTO_TEST_CASE(sse_mat32_consistent)
for(size_t j = 0; j < 3; ++j)
BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
- m3 = m1; m3.transpose();
- m4 = m2; m4.transpose();
+ m3 = transpose(m1);
+ m4 = transpose(m2);
for(size_t i = 0; i < 3; ++i)
for(size_t j = 0; j < 3; ++j)
@@ -788,8 +788,8 @@ BOOST_AUTO_TEST_CASE(sse_mat32_consistent)
for(size_t j = 0; j < 3; ++j)
BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
- m3 = m1; m3.inverse();
- m4 = m2; m4.inverse();
+ m3 = inverse(m1);
+ m4 = inverse(m2);
for(size_t i = 0; i < 3; ++i)
for(size_t j = 0; j < 3; ++j)
@@ -922,8 +922,8 @@ BOOST_AUTO_TEST_CASE(vec_test_sse_vec32_consistent)
BOOST_CHECK((v1 + delta1).equal(v1));
BOOST_CHECK((v3 + delta2).equal(v3));
- BOOST_CHECK(std::abs(v1.length() - v3.length()) < 1e-5);
- BOOST_CHECK(std::abs(v1.sqrLength() - v3.sqrLength()) < 1e-5);
+ BOOST_CHECK(std::abs(v1.norm() - v3.norm()) < 1e-5);
+ BOOST_CHECK(std::abs(v1.squaredNorm() - v3.squaredNorm()) < 1e-5);
v12 = v1; v12.negate();
v34 = v3; v34.negate();
@@ -1070,8 +1070,8 @@ BOOST_AUTO_TEST_CASE(vec_test_sse_vec64_consistent)
BOOST_CHECK((v1 + delta1).equal(v1));
BOOST_CHECK((v3 + delta2).equal(v3));
- BOOST_CHECK(std::abs(v1.length() - v3.length()) < 1e-5);
- BOOST_CHECK(std::abs(v1.sqrLength() - v3.sqrLength()) < 1e-5);
+ BOOST_CHECK(std::abs(v1.norm() - v3.norm()) < 1e-5);
+ BOOST_CHECK(std::abs(v1.squaredNorm() - v3.squaredNorm()) < 1e-5);
v12 = v1; v12.negate();
v34 = v3; v34.negate();