From 91e2c7bd2097920db86b6d20c2779eab50a5faeb Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Sun, 9 Mar 2014 17:11:39 +0100
Subject: [PATCH] Add a few methods in Matrix3fX and Vec3fX:
Matrix3fX::isIdentity,
Vec3fX::norm, Vec3fX::squaredNorm,
Vec3fX::setZero,
operator * (double, Vec3fX).x
---
include/fcl/math/matrix_3f.h | 8 ++++++++
include/fcl/math/vec_3f.h | 10 ++++++++++
2 files changed, 18 insertions(+)
diff --git a/include/fcl/math/matrix_3f.h b/include/fcl/math/matrix_3f.h
index 5c959252..b79c7f41 100644
--- a/include/fcl/math/matrix_3f.h
+++ b/include/fcl/math/matrix_3f.h
@@ -171,6 +171,14 @@ public:
data.setIdentity();
}
+ inline bool isIdentity () const
+ {
+ return
+ data (0,0) == 1 && data (0,1) == 0 && data (0,2) == 0 &&
+ data (0,0) == 0 && data (0,1) == 1 && data (0,2) == 0 &&
+ data (0,0) == 0 && data (0,1) == 0 && data (0,2) == 1;
+ }
+
inline void setZero()
{
data.setZero();
diff --git a/include/fcl/math/vec_3f.h b/include/fcl/math/vec_3f.h
index 840ba8de..e1bb0409 100644
--- a/include/fcl/math/vec_3f.h
+++ b/include/fcl/math/vec_3f.h
@@ -125,9 +125,12 @@ public:
}
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); }
+ inline void setZero () {data.setValue (0); }
inline bool equal(const Vec3fX& other, U epsilon = std::numeric_limits<U>::epsilon() * 100) const { return details::equal(data, other.data, epsilon); }
inline Vec3fX<T>& negate() { data.negate(); return *this; }
@@ -241,6 +244,13 @@ static inline std::ostream& operator << (std::ostream& o, const Vec3f& v)
return o;
}
+ template <typename T>
+ inline Vec3fX <T> operator * (const typename Vec3fX <T>::U& t,
+ const Vec3fX <T>& v)
+ {
+ return Vec3fX <T> (v.data * t);
+ }
+
}
--
GitLab