Skip to content

GitLab

Commit 6eeadecd authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Remove Vec3f::length and Vec3f sqrLength

parent be943c14
Hide whitespace changes
Inline Side-by-side
include/hpp/fcl/BV/AABB.h
View file @ 6eeadecd
......@@ -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
......
include/hpp/fcl/BV/BV.h
View file @ 6eeadecd
......@@ -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;
......
include/hpp/fcl/BV/OBB.h
View file @ 6eeadecd
......@@ -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
......
include/hpp/fcl/BV/kIOS.h
View file @ 6eeadecd
......@@ -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 */
......
include/hpp/fcl/BVH/BVH_model.h
View file @ 6eeadecd
......@@ -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);
......
include/hpp/fcl/ccd/motion.h
View file @ 6eeadecd
......@@ -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
......
include/hpp/fcl/eigen/vec_3fx.h
View file @ 6eeadecd
......@@ -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
{
......
include/hpp/fcl/math/vec_3fx.h
View file @ 6eeadecd
......@@ -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); }
......
include/hpp/fcl/math/vec_nf.h deleted 100644 → 0
View file @ be943c14
#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
include/hpp/fcl/narrowphase/narrowphase.h
View file @ 6eeadecd
......@@ -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;
......
include/hpp/fcl/octree.h
View file @ 6eeadecd
......@@ -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
......
include/hpp/fcl/shape/geometric_shapes.h
View file @ 6eeadecd
......@@ -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);
......
src/BV/OBB.cpp
View file @ 6eeadecd
......@@ -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);
}
......
src/BV/RSS.cpp
View file @ 6eeadecd
......@@ -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
......
src/BV/kIOS.cpp
View file @ 6eeadecd
......@@ -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