[ignition-math2] 01/02: Missing files from bad merge of upstream branch
Jose Luis Rivero
jrivero-guest at moszumanska.debian.org
Thu Sep 17 22:55:18 UTC 2015
This is an automated email from the git hooks/post-receive script.
jrivero-guest pushed a commit to branch master
in repository ignition-math2.
commit 2970830b4a6aaf4711e2e13d5bb13decf39dcaf9
Author: Jose Luis Rivero <jrivero at osrfoundation.org>
Date: Fri Sep 18 00:53:37 2015 +0200
Missing files from bad merge of upstream branch
---
CMakeLists.txt | 4 +-
cmake/ignition-math-config.cmake.in | 6 +
include/ignition/math/Angle.hh | 4 +
include/ignition/math/CMakeLists.txt | 2 +
include/ignition/math/Frustum.hh | 176 +++++++++++++
include/ignition/math/FrustumPrivate.hh | 78 ++++++
include/ignition/math/Helpers.hh | 3 +
include/ignition/math/Line3.hh | 204 +++++++++++++++
include/ignition/math/Plane.hh | 82 +++++-
include/ignition/math/SignalStats.hh | 32 +++
include/ignition/math/Vector3.hh | 17 +-
src/Angle.cc | 6 +-
src/CMakeLists.txt | 3 +
src/Frustum.cc | 241 ++++++++++++++++++
src/Frustum_TEST.cc | 428 ++++++++++++++++++++++++++++++++
src/Kmeans.cc | 10 +-
src/Line3_TEST.cc | 166 +++++++++++++
src/Plane_TEST.cc | 125 +++++++---
src/SignalStats.cc | 59 ++++-
src/SignalStats_TEST.cc | 201 +++++++++++++--
20 files changed, 1783 insertions(+), 64 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4566993..54a5983 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,8 +7,8 @@ string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
set (PROJECT_MAJOR_VERSION 2)
-set (PROJECT_MINOR_VERSION 1)
-set (PROJECT_PATCH_VERSION 1)
+set (PROJECT_MINOR_VERSION 2)
+set (PROJECT_PATCH_VERSION 2)
set (PROJECT_VERSION ${PROJECT_MAJOR_VERSION}.${PROJECT_MINOR_VERSION})
set (PROJECT_VERSION_FULL
diff --git a/cmake/ignition-math-config.cmake.in b/cmake/ignition-math-config.cmake.in
index 0320a5e..1385ccb 100644
--- a/cmake/ignition-math-config.cmake.in
+++ b/cmake/ignition-math-config.cmake.in
@@ -12,6 +12,12 @@ if ("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang")
set(@PKG_NAME at _CXX_FLAGS "${@PKG_NAME at _CXX_FLAGS} -stdlib=libc++")
endif ()
+# On windows we produce .dll libraries with no prefix
+if (WIN32)
+ set(CMAKE_FIND_LIBRARY_PREFIXES "")
+ set(CMAKE_FIND_LIBRARY_SUFFIXES ".lib" ".dll")
+endif()
+
foreach(lib @PKG_LIBRARIES@)
set(onelib "${lib}-NOTFOUND")
find_library(onelib ${lib}
diff --git a/include/ignition/math/Angle.hh b/include/ignition/math/Angle.hh
index 75908e2..da1dcb1 100644
--- a/include/ignition/math/Angle.hh
+++ b/include/ignition/math/Angle.hh
@@ -88,6 +88,10 @@ namespace ignition
/// \brief Normalize the angle in the range -Pi to Pi
public: void Normalize();
+ /// \brief Return the angle's radian value
+ /// \return double containing the angle's radian value
+ public: double operator()() const;
+
/// \brief Dereference operator
/// \return Double containing the angle's radian value
public: inline double operator*() const
diff --git a/include/ignition/math/CMakeLists.txt b/include/ignition/math/CMakeLists.txt
index 9ca56cb..4dcab9a 100644
--- a/include/ignition/math/CMakeLists.txt
+++ b/include/ignition/math/CMakeLists.txt
@@ -5,10 +5,12 @@ set (headers
Angle.hh
Box.hh
Filter.hh
+ Frustum.hh
Helpers.hh
IndexException.hh
Kmeans.hh
Line2.hh
+ Line3.hh
Matrix3.hh
Matrix4.hh
Plane.hh
diff --git a/include/ignition/math/Frustum.hh b/include/ignition/math/Frustum.hh
new file mode 100644
index 0000000..307bfa5
--- /dev/null
+++ b/include/ignition/math/Frustum.hh
@@ -0,0 +1,176 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef _IGNITION_FRUSTUM_HH_
+#define _IGNITION_FRUSTUM_HH_
+
+#include <ignition/math/Plane.hh>
+#include <ignition/math/Angle.hh>
+#include <ignition/math/Pose3.hh>
+
+namespace ignition
+{
+ namespace math
+ {
+ // Forward declaration of private data
+ class FrustumPrivate;
+
+ /// \brief Mathematical representation of a frustum and related functions.
+ /// This is also known as a view frustum.
+ class IGNITION_VISIBLE Frustum
+ {
+ /// \brief Planes that define the boundaries of the frustum.
+ public: enum FrustumPlane
+ {
+ /// \brief Near plane
+ FRUSTUM_PLANE_NEAR = 0,
+
+ /// \brief Far plane
+ FRUSTUM_PLANE_FAR = 1,
+
+ /// \brief Left plane
+ FRUSTUM_PLANE_LEFT = 2,
+
+ /// \brief Right plane
+ FRUSTUM_PLANE_RIGHT = 3,
+
+ /// \brief Top plane
+ FRUSTUM_PLANE_TOP = 4,
+
+ /// \brief Bottom plane
+ FRUSTUM_PLANE_BOTTOM = 5
+ };
+
+ /// \brief Default constructor. With the following default values:
+ ///
+ /// * near: 0.0
+ /// * far: 1.0
+ /// * fov: 0.78539 radians (45 degrees)
+ /// * aspect ratio: 1.0
+ /// * pose: Pose3d::Zero
+ public: Frustum();
+
+ /// \brief Constructor
+ /// \param[in] _near Near plane distance. This is the distance from
+ /// the frustum's vertex to the closest plane
+ /// \param[in] _far Far plane distance. This is the distance from the
+ /// frustum's vertex to the farthest plane.
+ /// \param[in] _fov Field of view. The field of view is the
+ /// angle between the frustum's vertex and the edges of the near or far
+ /// plane. This value represents the horizontal angle.
+ /// \param[in] _aspectRatio The aspect ratio, which is the width divided
+ /// by height of the near or far planes.
+ /// \param[in] _pose Pose of the frustum, which is the vertex (top of
+ /// the pyramid).
+ public: Frustum(const double _near,
+ const double _far,
+ const math::Angle &_fov,
+ const double _aspectRatio,
+ const math::Pose3d &_pose = math::Pose3d::Zero);
+
+ /// \brief Copy Constructor
+ /// \param[in] _p Frustum to copy.
+ public: Frustum(const Frustum &_p);
+
+ /// \brief Destructor
+ public: virtual ~Frustum();
+
+ /// \brief Get the near distance. This is the distance from the
+ /// frustum's vertex to the closest plane.
+ /// \return Near distance.
+ /// \sa SetNear
+ public: double Near() const;
+
+ /// \brief Set the near distance. This is the distance from the
+ /// frustum's vertex to the closest plane.
+ /// \param[in] _near Near distance.
+ /// \sa Near
+ public: void SetNear(const double _near);
+
+ /// \brief Get the far distance. This is the distance from the
+ /// frustum's vertex to the farthest plane.
+ /// \return Far distance.
+ /// \sa SetFar
+ public: double Far() const;
+
+ /// \brief Set the far distance. This is the distance from the
+ /// frustum's vertex to the farthest plane.
+ /// \param[in] _far Far distance.
+ /// \sa Far
+ public: void SetFar(const double _far);
+
+ /// \brief Get the horizontal field of view. The field of view is the
+ /// angle between the frustum's vertex and the edges of the near or far
+ /// plane. This value represents the horizontal angle.
+ /// \return The field of view.
+ /// \sa SetFOV
+ public: math::Angle FOV() const;
+
+ /// \brief Set the horizontal field of view. The field of view is the
+ /// angle between the frustum's vertex and the edges of the near or far
+ /// plane. This value represents the horizontal angle.
+ /// \param[in] _fov The field of view.
+ /// \sa FOV
+ public: void SetFOV(const math::Angle &_fov);
+
+ /// \brief Get the aspect ratio, which is the width divided by height
+ /// of the near or far planes.
+ /// \return The frustum's aspect ratio.
+ /// \sa SetAspectRatio
+ public: double AspectRatio() const;
+
+ /// \brief Set the aspect ratio, which is the width divided by height
+ /// of the near or far planes.
+ /// \param[in] _aspectRatio The frustum's aspect ratio.
+ /// \sa AspectRatio
+ public: void SetAspectRatio(const double _aspectRatio);
+
+ /// \brief Get a plane of the frustum.
+ /// \param[in] _plane The plane to return.
+ /// \return Plane of the frustum.
+ public: Planed Plane(const FrustumPlane _plane) const;
+
+ /// \brief Check if a box lies inside the pyramid frustum.
+ /// \param[in] _b Box to check.
+ /// \return True if the box is inside the pyramid frustum.
+ public: bool Contains(const Box &_b) const;
+
+ /// \brief Check if a point lies inside the pyramid frustum.
+ /// \param[in] _p Point to check.
+ /// \return True if the point is inside the pyramid frustum.
+ public: bool Contains(const Vector3d &_p) const;
+
+ /// \brief Get the pose of the frustum
+ /// \return Pose of the frustum
+ /// \sa SetPose
+ public: Pose3d Pose() const;
+
+ /// \brief Set the pose of the frustum
+ /// \param[in] _pose Pose of the frustum, top vertex.
+ /// \sa Pose
+ public: void SetPose(const Pose3d &_pose);
+
+ /// \brief Compute the planes of the frustum. This is called whenever
+ /// a property of the frustum is changed.
+ private: void ComputePlanes();
+
+ /// \internal
+ /// \brief Private data pointer
+ private: FrustumPrivate *dataPtr;
+ };
+ }
+}
+#endif
diff --git a/include/ignition/math/FrustumPrivate.hh b/include/ignition/math/FrustumPrivate.hh
new file mode 100644
index 0000000..59a1b77
--- /dev/null
+++ b/include/ignition/math/FrustumPrivate.hh
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef _IGNITION_FRUSTUM_PRIVATE_HH_
+#define _IGNITION_FRUSTUM_PRIVATE_HH_
+
+#include <array>
+
+#include <ignition/math/Pose3.hh>
+#include <ignition/math/Angle.hh>
+#include <ignition/math/Plane.hh>
+
+namespace ignition
+{
+ namespace math
+ {
+ /// \internal
+ /// \brief Private data for the Frustum class
+ class FrustumPrivate
+ {
+ /// \brief Constructor
+ /// \param[in] _near Near distance. This is the distance from
+ /// the frustum's vertex to the closest plane
+ /// \param[in] _far Far distance. This is the distance from the
+ /// frustum's vertex to the farthest plane.
+ /// \param[in] _fov Field of view. The field of view is the
+ /// angle between the frustum's vertex and the edges of the near or far
+ /// plane. This value represents the horizontal angle.
+ /// \param[in] _aspectRatio The aspect ratio, which is the width divided
+ /// by height of the near or far planes.
+ /// \param[in] _pose Pose of the frustum, which is the vertex (top of
+ /// the pyramid).
+ public: FrustumPrivate(const double _near,
+ const double _far,
+ const math::Angle &_fov,
+ const double _aspectRatio,
+ const Pose3d &_pose)
+ : near(_near), far(_far), fov(_fov),
+ aspectRatio(_aspectRatio), pose(_pose)
+ {
+ }
+
+ /// \brief Near distance
+ public: double near;
+
+ /// \brief Far distance
+ public: double far;
+
+ /// \brief Field of view
+ public: math::Angle fov;
+
+ /// \brief Aspect ratio of the near and far planes. This is the
+ // width divided by the height.
+ public: double aspectRatio;
+
+ /// \brief Pose of the frustum
+ public: math::Pose3d pose;
+
+ /// \brief Each plane of the frustum.
+ /// \sa Frustum::FrustumPlane
+ public: std::array<Planed, 6> planes;
+ };
+ }
+}
+#endif
diff --git a/include/ignition/math/Helpers.hh b/include/ignition/math/Helpers.hh
index 97f1369..482c975 100644
--- a/include/ignition/math/Helpers.hh
+++ b/include/ignition/math/Helpers.hh
@@ -34,6 +34,9 @@
/// \brief Double min value
#define IGN_DBL_MIN std::numeric_limits<double>::min()
+/// \brief Double positive infinite value
+#define IGN_DBL_INF std::numeric_limits<double>::infinity()
+
/// \brief Float maximum value
#define IGN_FLT_MAX std::numeric_limits<float>::max()
diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh
new file mode 100644
index 0000000..488855f
--- /dev/null
+++ b/include/ignition/math/Line3.hh
@@ -0,0 +1,204 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef _IGNITION_LINE3_HH_
+#define _IGNITION_LINE3_HH_
+
+#include <algorithm>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/IndexException.hh>
+
+namespace ignition
+{
+ namespace math
+ {
+ /// \class Line3 Line3.hh ignition/math/Line3.hh
+ /// \brief A three dimensional line segment. The line is defined by a
+ /// start and end point.
+ template<typename T>
+ class Line3
+ {
+ /// \brief Line Constructor
+ public: Line3() = default;
+
+ /// \brief Copy constructor
+ /// \param[in] _line a line object
+ public: Line3(const Line3<T> &_line)
+ {
+ this->pts[0] = _line[0];
+ this->pts[1] = _line[1];
+ }
+
+ /// \brief Constructor.
+ /// \param[in] _ptA Start point of the line segment
+ /// \param[in] _ptB End point of the line segment
+ public: Line3(const math::Vector3<T> &_ptA, const math::Vector3<T> &_ptB)
+ {
+ this->Set(_ptA, _ptB);
+ }
+
+ /// \brief 2D Constructor where Z coordinates are 0
+ /// \param[in] _x1 X coordinate of the start point.
+ /// \param[in] _y1 Y coordinate of the start point.
+ /// \param[in] _x2 X coordinate of the end point.
+ /// \param[in] _y2 Y coordinate of the end point.
+ public: Line3(const double _x1, const double _y1,
+ const double _x2, const double _y2)
+ {
+ this->Set(_x1, _y1, _x2, _y2);
+ }
+
+ /// \brief Constructor.
+ /// \param[in] _x1 X coordinate of the start point.
+ /// \param[in] _y1 Y coordinate of the start point.
+ /// \param[in] _z1 Z coordinate of the start point.
+ /// \param[in] _x2 X coordinate of the end point.
+ /// \param[in] _y2 Y coordinate of the end point.
+ /// \param[in] _z2 Z coordinate of the end point.
+ public: Line3(const double _x1, const double _y1,
+ const double _z1, const double _x2,
+ const double _y2, const double _z2)
+ {
+ this->Set(_x1, _y1, _z1, _x2, _y2, _z2);
+ }
+
+ /// \brief Set the start and end point of the line segment
+ /// \param[in] _ptA Start point of the line segment
+ /// \param[in] _ptB End point of the line segment
+ public: void Set(const math::Vector3<T> &_ptA,
+ const math::Vector3<T> &_ptB)
+ {
+ this->pts[0] = _ptA;
+ this->pts[1] = _ptB;
+ }
+
+ /// \brief Set the start point of the line segment
+ /// \param[in] _ptA Start point of the line segment
+ public: void SetA(const math::Vector3<T> &_ptA)
+ {
+ this->pts[0] = _ptA;
+ }
+
+ /// \brief Set the end point of the line segment
+ /// \param[in] _ptB End point of the line segment
+ public: void SetB(const math::Vector3<T> &_ptB)
+ {
+ this->pts[1] = _ptB;
+ }
+
+ /// \brief Set the start and end point of the line segment, assuming that
+ /// both points have the same height.
+ /// \param[in] _x1 X coordinate of the start point.
+ /// \param[in] _y1 Y coordinate of the start point.
+ /// \param[in] _x2 X coordinate of the end point.
+ /// \param[in] _y2 Y coordinate of the end point.
+ /// \param[in] _z Z coordinate of both points,
+ /// by default _z is set to 0.
+ public: void Set(const double _x1, const double _y1,
+ const double _x2, const double _y2,
+ const double _z = 0)
+ {
+ this->pts[0].Set(_x1, _y1, _z);
+ this->pts[1].Set(_x2, _y2, _z);
+ }
+
+ /// \brief Set the start and end point of the line segment
+ /// \param[in] _x1 X coordinate of the start point.
+ /// \param[in] _y1 Y coordinate of the start point.
+ /// \param[in] _z1 Z coordinate of the start point.
+ /// \param[in] _x2 X coordinate of the end point.
+ /// \param[in] _y2 Y coordinate of the end point.
+ /// \param[in] _z2 Z coordinate of the end point.
+ public: void Set(const double _x1, const double _y1,
+ const double _z1, const double _x2,
+ const double _y2, const double _z2)
+ {
+ this->pts[0].Set(_x1, _y1, _z1);
+ this->pts[1].Set(_x2, _y2, _z2);
+ }
+
+ /// \brief Get the direction of the line
+ /// \return The direction vector
+ public: math::Vector3<T> Direction() const
+ {
+ return (this->pts[1] - this->pts[0]).Normalize();
+ }
+
+ /// \brief Get the length of the line
+ /// \return The length of the line.
+ public: T Length() const
+ {
+ return this->pts[0].Distance(this->pts[1]);
+ }
+
+ /// \brief Equality operator.
+ /// \param[in] _line Line to compare for equality.
+ /// \return True if the given line is equal to this line
+ public: bool operator==(const Line3<T> &_line) const
+ {
+ return this->pts[0] == _line[0] && this->pts[1] == _line[1];
+ }
+
+ /// \brief Inequality operator.
+ /// \param[in] _line Line to compare for inequality.
+ /// \return True if the given line is not to this line
+ public: bool operator!=(const Line3<T> &_line) const
+ {
+ return !(*this == _line);
+ }
+
+ /// \brief Get the start or end point.
+ /// \param[in] _index 0 = start point, 1 = end point.
+ /// \throws IndexException if _index is > 1.
+ public: math::Vector3<T> operator[](const size_t _index) const
+ {
+ if (_index > 1)
+ throw IndexException();
+ return this->pts[_index];
+ }
+
+ /// \brief Stream extraction operator
+ /// \param[in] _out output stream
+ /// \param[in] _line Line3 to output
+ /// \return The stream
+ public: friend std::ostream &operator<<(
+ std::ostream &_out, const Line3<T> &_line)
+ {
+ _out << _line[0] << " " << _line[1];
+ return _out;
+ }
+
+ /// \brief Assignment operator
+ /// \param[in] _line a new value
+ /// \return this
+ public: Line3 &operator=(const Line3<T> &_line)
+ {
+ this->pts[0] = _line[0];
+ this->pts[1] = _line[1];
+
+ return *this;
+ }
+
+ /// \brief Vector for storing the start and end points of the line
+ private: math::Vector3<T> pts[2];
+ };
+
+ typedef Line3<int> Line3i;
+ typedef Line3<double> Line3d;
+ typedef Line3<float> Line3f;
+ }
+}
+#endif
diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh
index ac978e6..b993ced 100644
--- a/include/ignition/math/Plane.hh
+++ b/include/ignition/math/Plane.hh
@@ -18,6 +18,7 @@
#ifndef _IGNITION_PLANE_HH_
#define _IGNITION_PLANE_HH_
+#include <ignition/math/Box.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Vector2.hh>
@@ -30,13 +31,33 @@ namespace ignition
template<typename T>
class Plane
{
+ /// \brief Enum used to indicate a side of the plane, no side, or both
+ /// sides for entities on the plane.
+ /// \sa Side
+ public: enum PlaneSide
+ {
+ /// \brief Negative side of the plane. This is the side that is
+ /// opposite the normal.
+ NEGATIVE_SIDE = 0,
+
+ /// \brief Positive side of the plane. This is the side that has the
+ /// normal vector.
+ POSITIVE_SIDE = 1,
+
+ /// \brief On the plane.
+ NO_SIDE = 2,
+
+ /// \brief On both sides of the plane.
+ BOTH_SIDE = 3
+ };
+
/// \brief Constructor
public: Plane()
{
this->d = 0.0;
}
- /// \brief Constructor from a normal and a distanec
+ /// \brief Constructor from a normal and a distance
/// \param[in] _normal The plane normal
/// \param[in] _offset Offset along the normal
public: Plane(const Vector3<T> &_normal, T _offset = 0.0)
@@ -60,6 +81,15 @@ namespace ignition
/// \brief Set the plane
/// \param[in] _normal The plane normal
+ /// \param[in] _offset Offset along the normal
+ public: void Set(const Vector3<T> &_normal, T _offset)
+ {
+ this->normal = _normal;
+ this->d = _offset;
+ }
+
+ /// \brief Set the plane
+ /// \param[in] _normal The plane normal
/// \param[in] _size Size of the plane
/// \param[in] _offset Offset along the normal
public: void Set(const Vector3<T> &_normal, const Vector2<T> &_size,
@@ -70,6 +100,56 @@ namespace ignition
this->d = _offset;
}
+ /// \brief The distance to the plane from the given point. The
+ /// distance can be negative, which indicates the point is on the
+ /// negative side of the plane.
+ /// \param[in] _point 3D point to calculate distance from.
+ /// \return Distance from the point to the plane.
+ /// \sa Side
+ public: T Distance(const Vector3<T> &_point) const
+ {
+ return this->normal.Dot(_point) - this->d;
+ }
+
+ /// \brief The side of the plane a point is on.
+ /// \param[in] _point The 3D point to check.
+ /// \return Plane::NEGATIVE_SIDE if the distance from the point to the
+ /// plane is negative, Plane::POSITIVE_SIDE if the distance from the
+ /// point to the plane is positive, or Plane::NO_SIDE if the
+ /// point is on the plane.
+ public: PlaneSide Side(const Vector3<T> &_point) const
+ {
+ T dist = this->Distance(_point);
+
+ if (dist < 0.0)
+ return NEGATIVE_SIDE;
+
+ if (dist > 0.0)
+ return POSITIVE_SIDE;
+
+ return NO_SIDE;
+ }
+
+ /// \brief The side of the plane a box is on.
+ /// \param[in] _box The 3D box to check.
+ /// \return Plane::NEGATIVE_SIDE if the distance from the box to the
+ /// plane is negative, Plane::POSITIVE_SIDE if the distance from the
+ /// box to the plane is positive, or Plane::BOTH_SIDE if the
+ /// box is on the plane.
+ public: PlaneSide Side(const math::Box &_box) const
+ {
+ double dist = this->Distance(_box.Center());
+ double maxAbsDist = this->normal.AbsDot(_box.Size()/2.0);
+
+ if (dist < -maxAbsDist)
+ return NEGATIVE_SIDE;
+
+ if (dist > maxAbsDist)
+ return POSITIVE_SIDE;
+
+ return BOTH_SIDE;
+ }
+
/// \brief Get distance to the plane give an origin and direction
/// \param[in] _origin the origin
/// \param[in] _dir a direction
diff --git a/include/ignition/math/SignalStats.hh b/include/ignition/math/SignalStats.hh
index 17ab194..e66b38c 100644
--- a/include/ignition/math/SignalStats.hh
+++ b/include/ignition/math/SignalStats.hh
@@ -62,6 +62,22 @@ namespace ignition
};
/// \}
+ /// \class SignalMaximum SignalStats.hh ignition/math/SignalStats.hh
+ /// \brief Computing the maximum value of a discretely sampled signal.
+ class IGNITION_VISIBLE SignalMaximum : public SignalStatistic
+ {
+ // Documentation inherited.
+ public: virtual double Value() const;
+
+ /// \brief Get a short version of the name of this statistical measure.
+ /// \return "max"
+ public: virtual std::string ShortName() const;
+
+ // Documentation inherited.
+ public: virtual void InsertData(const double _data);
+ };
+ /// \}
+
/// \class SignalMean SignalStats.hh ignition/math/SignalStats.hh
/// \brief Computing the mean value of a discretely sampled signal.
class IGNITION_VISIBLE SignalMean : public SignalStatistic
@@ -78,6 +94,22 @@ namespace ignition
};
/// \}
+ /// \class SignalMinimum SignalStats.hh ignition/math/SignalStats.hh
+ /// \brief Computing the minimum value of a discretely sampled signal.
+ class IGNITION_VISIBLE SignalMinimum : public SignalStatistic
+ {
+ // Documentation inherited.
+ public: virtual double Value() const;
+
+ /// \brief Get a short version of the name of this statistical measure.
+ /// \return "min"
+ public: virtual std::string ShortName() const;
+
+ // Documentation inherited.
+ public: virtual void InsertData(const double _data);
+ };
+ /// \}
+
/// \class SignalRootMeanSquare SignalStats.hh ignition/math/SignalStats.hh
/// \brief Computing the square root of the mean squared value
/// of a discretely sampled signal.
diff --git a/include/ignition/math/Vector3.hh b/include/ignition/math/Vector3.hh
index 768bc87..80523f8 100644
--- a/include/ignition/math/Vector3.hh
+++ b/include/ignition/math/Vector3.hh
@@ -195,6 +195,21 @@ namespace ignition
this->data[2] * _v[2];
}
+ /// \brief Return the absolute dot product of this vector and
+ /// another vector. This is similar to the Dot function, except the
+ /// absolute value of each component of the vector is used.
+ ///
+ /// result = abs(x1 * x2) + abs(y1 * y2) + abs(z1 *z2)
+ ///
+ /// \param[in] _v the vector
+ /// \return The absolute dot product
+ public: T AbsDot(const Vector3<T> &_v) const
+ {
+ return std::abs(this->data[0] * _v[0]) +
+ std::abs(this->data[1] * _v[1]) +
+ std::abs(this->data[2] * _v[2]);
+ }
+
/// \brief Get the absolute value of the vector
/// \return a vector with positive elements
public: Vector3 Abs() const
@@ -232,7 +247,7 @@ namespace ignition
Vector3<T> a = _v2 - _v1;
Vector3<T> b = _v3 - _v1;
Vector3<T> n = a.Cross(b);
- return n;
+ return n.Normalize();
}
/// \brief Get distance to a line
diff --git a/src/Angle.cc b/src/Angle.cc
index c9698fe..5b0db03 100644
--- a/src/Angle.cc
+++ b/src/Angle.cc
@@ -166,4 +166,8 @@ bool Angle::operator>=(const Angle &angle) const
return this->value > angle.value || math::equal(this->value, angle.value);
}
-
+//////////////////////////////////////////////////
+double Angle::operator()() const
+{
+ return this->value;
+}
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 30da36e..a40c795 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -5,6 +5,7 @@ set (sources
Angle.cc
Box.cc
BoxPrivate.cc
+ Frustum.cc
Helpers.cc
Kmeans.cc
Rand.cc
@@ -19,9 +20,11 @@ set (gtest_sources
Angle_TEST.cc
Box_TEST.cc
Filter_TEST.cc
+ Frustum_TEST.cc
Helpers_TEST.cc
Kmeans_TEST.cc
Line2_TEST.cc
+ Line3_TEST.cc
Matrix3_TEST.cc
Matrix4_TEST.cc
Plane_TEST.cc
diff --git a/src/Frustum.cc b/src/Frustum.cc
new file mode 100644
index 0000000..d286e7f
--- /dev/null
+++ b/src/Frustum.cc
@@ -0,0 +1,241 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#include <cmath>
+
+#include "ignition/math/Matrix4.hh"
+#include "ignition/math/FrustumPrivate.hh"
+#include "ignition/math/Frustum.hh"
+
+using namespace ignition;
+using namespace math;
+
+/////////////////////////////////////////////////
+Frustum::Frustum()
+ : dataPtr(new FrustumPrivate(0, 1, IGN_DTOR(45), 1, Pose3d::Zero))
+{
+}
+
+/////////////////////////////////////////////////
+Frustum::Frustum(const double _near,
+ const double _far,
+ const Angle &_fov,
+ const double _aspectRatio,
+ const Pose3d &_pose)
+ : dataPtr(new FrustumPrivate(_near, _far, _fov, _aspectRatio, _pose))
+{
+ // Compute plane based on near distance, far distance, field of view,
+ // aspect ratio, and pose
+ this->ComputePlanes();
+}
+
+/////////////////////////////////////////////////
+Frustum::~Frustum()
+{
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+/////////////////////////////////////////////////
+Frustum::Frustum(const Frustum &_p)
+ : dataPtr(new FrustumPrivate(_p.Near(), _p.Far(), _p.FOV(),
+ _p.AspectRatio(), _p.Pose()))
+{
+ for (int i = 0; i < 6; ++i)
+ this->dataPtr->planes[i] = _p.dataPtr->planes[i];
+}
+
+/////////////////////////////////////////////////
+Planed Frustum::Plane(const FrustumPlane _plane) const
+{
+ return this->dataPtr->planes[_plane];
+}
+
+/////////////////////////////////////////////////
+bool Frustum::Contains(const Box &_b) const
+{
+ // If the box is on the negative side of a plane, then the box is not
+ // visible.
+ for (auto const &plane : this->dataPtr->planes)
+ {
+ if (plane.Side(_b) == Planed::NEGATIVE_SIDE)
+ return false;
+ }
+
+ return true;
+}
+
+/////////////////////////////////////////////////
+bool Frustum::Contains(const Vector3d &_p) const
+{
+ // If the point is on the negative side of a plane, then the point is not
+ // visible.
+ for (auto const &plane : this->dataPtr->planes)
+ {
+ if (plane.Side(_p) == Planed::NEGATIVE_SIDE)
+ return false;
+ }
+
+ return true;
+}
+
+/////////////////////////////////////////////////
+double Frustum::Near() const
+{
+ return this->dataPtr->near;
+}
+
+/////////////////////////////////////////////////
+void Frustum::SetNear(const double _near)
+{
+ this->dataPtr->near = _near;
+ this->ComputePlanes();
+}
+
+/////////////////////////////////////////////////
+double Frustum::Far() const
+{
+ return this->dataPtr->far;
+}
+
+/////////////////////////////////////////////////
+void Frustum::SetFar(const double _far)
+{
+ this->dataPtr->far = _far;
+ this->ComputePlanes();
+}
+
+/////////////////////////////////////////////////
+Angle Frustum::FOV() const
+{
+ return this->dataPtr->fov;
+}
+
+/////////////////////////////////////////////////
+void Frustum::SetFOV(const Angle &_angle)
+{
+ this->dataPtr->fov = _angle;
+ this->ComputePlanes();
+}
+
+/////////////////////////////////////////////////
+Pose3d Frustum::Pose() const
+{
+ return this->dataPtr->pose;
+}
+
+/////////////////////////////////////////////////
+void Frustum::SetPose(const Pose3d &_pose)
+{
+ this->dataPtr->pose = _pose;
+ this->ComputePlanes();
+}
+
+/////////////////////////////////////////////////
+double Frustum::AspectRatio() const
+{
+ return this->dataPtr->aspectRatio;
+}
+
+/////////////////////////////////////////////////
+void Frustum::SetAspectRatio(const double _aspectRatio)
+{
+ this->dataPtr->aspectRatio = _aspectRatio;
+ this->ComputePlanes();
+}
+
+/////////////////////////////////////////////////
+void Frustum::ComputePlanes()
+{
+ // Tangent of half the field of view.
+ double tanFOV2 = std::tan(this->dataPtr->fov() * 0.5);
+
+ // Width of near plane
+ double nearWidth = 2.0 * tanFOV2 * this->dataPtr->near;
+
+ // Height of near plane
+ double nearHeight = nearWidth / this->dataPtr->aspectRatio;
+
+ // Width of far plane
+ double farWidth = 2.0 * tanFOV2 * this->dataPtr->far;
+
+ // Height of far plane
+ double farHeight = farWidth / this->dataPtr->aspectRatio;
+
+ // Up, right, and forward unit vectors.
+ Vector3d forward = this->dataPtr->pose.Rot().RotateVector(Vector3d::UnitX);
+ Vector3d up = this->dataPtr->pose.Rot().RotateVector(Vector3d::UnitZ);
+ Vector3d right = this->dataPtr->pose.Rot().RotateVector(-Vector3d::UnitY);
+
+ // Near plane center
+ Vector3d nearCenter = this->dataPtr->pose.Pos() + forward *
+ this->dataPtr->near;
+
+ // Far plane center
+ Vector3d farCenter = this->dataPtr->pose.Pos() + forward *
+ this->dataPtr->far;
+
+ // These four variables are here for convenience.
+ Vector3d upNearHeight2 = up * (nearHeight * 0.5);
+ Vector3d rightNearWidth2 = right * (nearWidth * 0.5);
+ Vector3d upFarHeight2 = up * (farHeight * 0.5);
+ Vector3d rightFarWidth2 = right * (farWidth * 0.5);
+
+ // Compute the vertices of the near plane
+ Vector3d nearTopLeft = nearCenter + upNearHeight2 - rightNearWidth2;
+ Vector3d nearTopRight = nearCenter + upNearHeight2 + rightNearWidth2;
+ Vector3d nearBottomLeft = nearCenter - upNearHeight2 - rightNearWidth2;
+ Vector3d nearBottomRight = nearCenter - upNearHeight2 + rightNearWidth2;
+
+ // Compute the vertices of the far plane
+ Vector3d farTopLeft = farCenter + upFarHeight2 - rightFarWidth2;
+ Vector3d farTopRight = farCenter + upFarHeight2 + rightFarWidth2;
+ Vector3d farBottomLeft = farCenter - upFarHeight2 - rightFarWidth2;
+ Vector3d farBottomRight = farCenter - upFarHeight2 + rightFarWidth2;
+
+ Vector3d leftCenter =
+ (farTopLeft + nearTopLeft + farBottomLeft + nearBottomLeft) / 4.0;
+
+ Vector3d rightCenter =
+ (farTopRight + nearTopRight + farBottomRight + nearBottomRight) / 4.0;
+
+ Vector3d topCenter =
+ (farTopRight + nearTopRight + farTopLeft + nearTopLeft) / 4.0;
+
+ Vector3d bottomCenter =
+ (farBottomRight + nearBottomRight + farBottomLeft + nearBottomLeft) / 4.0;
+
+ // Compute plane offsets
+ // Set the planes, where the first value is the plane normal and the
+ // second the plane offset
+ Vector3d norm = Vector3d::Normal(nearTopLeft, nearTopRight, nearBottomLeft);
+ this->dataPtr->planes[FRUSTUM_PLANE_NEAR].Set(norm, nearCenter.Dot(norm));
+
+ norm = Vector3d::Normal(farTopRight, farTopLeft, farBottomLeft);
+ this->dataPtr->planes[FRUSTUM_PLANE_FAR].Set(norm, farCenter.Dot(norm));
+
+ norm = Vector3d::Normal(farTopLeft, nearTopLeft, nearBottomLeft);
+ this->dataPtr->planes[FRUSTUM_PLANE_LEFT].Set(norm, leftCenter.Dot(norm));
+
+ norm = Vector3d::Normal(nearTopRight, farTopRight, farBottomRight);
+ this->dataPtr->planes[FRUSTUM_PLANE_RIGHT].Set(norm, rightCenter.Dot(norm));
+
+ norm = Vector3d::Normal(nearTopLeft, farTopLeft, nearTopRight);
+ this->dataPtr->planes[FRUSTUM_PLANE_TOP].Set(norm, topCenter.Dot(norm));
+
+ norm = Vector3d::Normal(nearBottomLeft, nearBottomRight, farBottomRight);
+ this->dataPtr->planes[FRUSTUM_PLANE_BOTTOM].Set(norm, bottomCenter.Dot(norm));
+}
diff --git a/src/Frustum_TEST.cc b/src/Frustum_TEST.cc
new file mode 100644
index 0000000..5ac11f9
--- /dev/null
+++ b/src/Frustum_TEST.cc
@@ -0,0 +1,428 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <gtest/gtest.h>
+
+#include "ignition/math/Helpers.hh"
+#include "ignition/math/Frustum.hh"
+
+using namespace ignition;
+using namespace math;
+
+/////////////////////////////////////////////////
+TEST(FrustumTest, Constructor)
+{
+ Frustum frustum;
+
+ EXPECT_EQ(frustum.Near(), 0);
+ EXPECT_EQ(frustum.Far(), 1);
+ EXPECT_EQ(frustum.FOV(), IGN_DTOR(45));
+ EXPECT_EQ(frustum.AspectRatio(), 1);
+ EXPECT_EQ(frustum.Pose(), Pose3d::Zero);
+}
+
+/////////////////////////////////////////////////
+TEST(FrustumTest, CopyConstructor)
+{
+ // Frustum pointing down the +x axis
+ Frustum frustum(
+ // Near distance
+ 1,
+ // Far distance
+ 10,
+ // Field of view
+ Angle(IGN_DTOR(45)),
+ // Aspect ratio
+ 320.0/240.0,
+ // Pose
+ Pose3d(0, 0, 0, 0, 0, 0));
+
+ Frustum frustum2(frustum);
+
+ EXPECT_EQ(frustum.FOV(), frustum2.FOV());
+ EXPECT_EQ(frustum.Near(), frustum2.Near());
+ EXPECT_EQ(frustum.Far(), frustum2.Far());
+ EXPECT_EQ(frustum.AspectRatio(), frustum2.AspectRatio());
+ EXPECT_EQ(frustum.AspectRatio(), frustum2.AspectRatio());
+
+ EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_NEAR).Normal(),
+ frustum2.Plane(Frustum::FRUSTUM_PLANE_NEAR).Normal());
+
+ EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_FAR).Normal(),
+ frustum2.Plane(Frustum::FRUSTUM_PLANE_FAR).Normal());
+
+ EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_LEFT).Normal(),
+ frustum2.Plane(Frustum::FRUSTUM_PLANE_LEFT).Normal());
+
+ EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_RIGHT).Normal(),
+ frustum2.Plane(Frustum::FRUSTUM_PLANE_RIGHT).Normal());
+
+ EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_TOP).Normal(),
+ frustum2.Plane(Frustum::FRUSTUM_PLANE_TOP).Normal());
+
+ EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_BOTTOM).Normal(),
+ frustum2.Plane(Frustum::FRUSTUM_PLANE_BOTTOM).Normal());
+}
+
+/////////////////////////////////////////////////
+TEST(FrustumTest, PyramidXAxisPos)
+{
+ // Frustum pointing down the +x axis
+ Frustum frustum(
+ // Near distance
+ 1,
+ // Far distance
+ 10,
+ // Field of view
+ Angle(IGN_DTOR(45)),
+ // Aspect ratio
+ 320.0/240.0,
+ // Pose
+ Pose3d(0, 0, 0, 0, 0, 0));
+
+ EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 0)));
+ EXPECT_TRUE(frustum.Contains(Vector3d(1, 0, 0)));
+
+ EXPECT_TRUE(frustum.Contains(Vector3d(2, 0, 0)));
+ EXPECT_TRUE(frustum.Contains(Vector3d(10, 0, 0)));
+ EXPECT_FALSE(frustum.Contains(Vector3d(10.1, 0, 0)));
+
+ EXPECT_TRUE(frustum.Contains(Box(Vector3d(1, 0, 0), Vector3d(5, 5, 5))));
+ EXPECT_FALSE(frustum.Contains(Box(Vector3d(-1, 0, 0), Vector3d(.1, .2, .3))));
+}
+
+/////////////////////////////////////////////////
+TEST(FrustumTest, PyramidXAxisNeg)
+{
+ // Frustum pointing down the -x axis
+ Frustum frustum(
+ // Near distance
+ 1,
+ // Far distance
+ 10,
+ // Field of view
+ Angle(IGN_DTOR(45)),
+ // Aspect ratio
+ 320.0/240.0,
+ // Pose
+ Pose3d(0, 0, 0, 0, 0, M_PI));
+
+ EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 0)));
+ EXPECT_FALSE(frustum.Contains(Vector3d(-0.5, 0, 0)));
+ EXPECT_FALSE(frustum.Contains(Vector3d(-10.1, 0, 0)));
+
+ EXPECT_TRUE(frustum.Contains(Vector3d(-1, 0, 0)));
+ EXPECT_TRUE(frustum.Contains(Vector3d(-2, 0, 0)));
+ EXPECT_TRUE(frustum.Contains(Vector3d(-10, 0, 0)));
+
+ EXPECT_FALSE(frustum.Contains(Box(Vector3d(1, 0, 0), Vector3d(5, 5, 5))));
+ EXPECT_TRUE(frustum.Contains(Box(Vector3d(-1, 0, 0), Vector3d(.1, .2, .3))));
+}
+
+/////////////////////////////////////////////////
+TEST(FrustumTest, PyramidYAxis)
+{
+ // Frustum pointing down the +y axis
+ Frustum frustum(
+ // Near distance
+ .1,
+ // Far distance
+ 5,
+ // Field of view
+ Angle(IGN_DTOR(45)),
+ // Aspect ratio
+ 320.0/320.0,
+ // Pose
+ Pose3d(0, 0, 0, 0, 0, M_PI*0.5));
+
+ EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 0)));
+ EXPECT_FALSE(frustum.Contains(Vector3d(1, 0, 0)));
+ EXPECT_FALSE(frustum.Contains(Vector3d(.05, 0, 0)));
+
+ EXPECT_TRUE(frustum.Contains(Vector3d(0, .1, 0)));
+ EXPECT_TRUE(frustum.Contains(Vector3d(0, 1, 0)));
+ EXPECT_TRUE(frustum.Contains(Vector3d(0, 5, 0)));
+
+ EXPECT_TRUE(frustum.Contains(Box(Vector3d(0, 1, 0), Vector3d(5, 5, 5))));
+ EXPECT_FALSE(frustum.Contains(Box(Vector3d(0, -1, 0), Vector3d(.1, 0, .3))));
+}
+
+/////////////////////////////////////////////////
+TEST(FrustumTest, PyramidZAxis)
+{
+ // Frustum pointing down the -z axis
+ Frustum frustum(
+ // Near distance
+ 1,
+ // Far distance
+ 10,
+ // Field of view
+ Angle(IGN_DTOR(45)),
+ // Aspect ratio
+ 320.0/320.0,
+ // Pose
+ Pose3d(0, 0, 0, 0, M_PI*0.5, 0));
+
+ EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 0)));
+ EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, -0.9)));
+ EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, -10.5)));
+ EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 0.9)));
+ EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 10.5)));
+
+ EXPECT_TRUE(frustum.Contains(Vector3d(0, 0, -1.1)));
+ EXPECT_TRUE(frustum.Contains(Vector3d(0.5, 0.5, -5.5)));
+ EXPECT_TRUE(frustum.Contains(Vector3d(0, 0, -10)));
+
+ EXPECT_FALSE(frustum.Contains(Box(Vector3d(0, 0, 0), Vector3d(5, 5, 5))));
+ EXPECT_TRUE(frustum.Contains(Box(Vector3d(0, 0, -1), Vector3d(.1, 0, .3))));
+}
+
+/////////////////////////////////////////////////
+TEST(FrustumTest, NearFar)
+{
+ Frustum frustum(
+ // Near distance
+ 1,
+ // Far distance
+ 10,
+ // Field of view
+ Angle(IGN_DTOR(45)),
+ // Aspect ratio
+ 320.0/320.0,
+ // Pose
+ Pose3d(0, 0, 0, 0, M_PI*0.5, 0));
+
+ EXPECT_DOUBLE_EQ(frustum.Near(), 1.0);
+ EXPECT_DOUBLE_EQ(frustum.Far(), 10.0);
+
+ frustum.SetNear(-1.0);
+ frustum.SetFar(-10.0);
+
+ EXPECT_DOUBLE_EQ(frustum.Near(), -1.0);
+ EXPECT_DOUBLE_EQ(frustum.Far(), -10.0);
+}
+
+/////////////////////////////////////////////////
+TEST(FrustumTest, FOV)
+{
+ Frustum frustum(
+ // Near distance
+ 1,
+ // Far distance
+ 10,
+ // Field of view
+ Angle(IGN_DTOR(45)),
+ // Aspect ratio
+ 320.0/320.0,
+ // Pose
+ Pose3d(0, 0, 0, 0, M_PI*0.5, 0));
+
+ EXPECT_EQ(frustum.FOV(), math::Angle(IGN_DTOR(45)));
+
+ frustum.SetFOV(1.5707);
+
+ EXPECT_EQ(frustum.FOV(), math::Angle(1.5707));
+}
+
+/////////////////////////////////////////////////
+TEST(FrustumTest, AspectRatio)
+{
+ Frustum frustum(
+ // Near distance
+ 1,
+ // Far distance
+ 10,
+ // Field of view
+ Angle(IGN_DTOR(45)),
+ // Aspect ratio
+ 320.0/320.0,
+ // Pose
+ Pose3d(0, 0, 0, 0, M_PI*0.5, 0));
+
+ EXPECT_DOUBLE_EQ(frustum.AspectRatio(), 320.0/320.0);
+
+ frustum.SetAspectRatio(1.3434);
+
+ EXPECT_DOUBLE_EQ(frustum.AspectRatio(), 1.3434);
+}
+
+/////////////////////////////////////////////////
+TEST(FrustumTest, Pose)
+{
+ Frustum frustum(
+ // Near distance
+ 1,
+ // Far distance
+ 10,
+ // Field of view
+ Angle(IGN_DTOR(45)),
+ // Aspect ratio
+ 320.0/320.0,
+ // Pose
+ Pose3d(0, 0, 0, 0, M_PI*0.5, 0));
+
+ EXPECT_EQ(frustum.Pose(), Pose3d(0, 0, 0, 0, M_PI*0.5, 0));
+
+ frustum.SetPose(Pose3d(1, 2, 3, M_PI, 0, 0));
+
+ EXPECT_EQ(frustum.Pose(), Pose3d(1, 2, 3, M_PI, 0, 0));
+}
+
+/////////////////////////////////////////////////
+TEST(FrustumTest, PoseContains)
+{
+ Frustum frustum(
+ // Near distance
+ 1,
+ // Far distance
+ 10,
+ // Field of view
+ Angle(IGN_DTOR(60)),
+ // Aspect ratio
+ 1920.0/1080.0,
+ // Pose
+ Pose3d(0, -5, 0, 0, 0, M_PI*0.5));
+
+ // Test the near clip boundary
+ EXPECT_FALSE(frustum.Contains(Vector3d(0, -4.01, 0)));
+ EXPECT_TRUE(frustum.Contains(Vector3d(0, -4.0, 0)));
+
+ // Test a point between the near and far clip planes
+ EXPECT_TRUE(frustum.Contains(Vector3d(0, 1, 0)));
+
+ // Test the far clip boundary
+ EXPECT_TRUE(frustum.Contains(Vector3d(0, 5, 0)));
+ EXPECT_FALSE(frustum.Contains(Vector3d(0, 5.001, 0)));
+
+ // Use an offset for the test points. This makes the test more stable, and
+ // is also used to generate point outside the frustum.
+ double offset = 0.00001;
+
+ // Compute near clip points
+ Vector3d nearTopLeft(
+ -tan(IGN_DTOR(30)) + offset,
+ frustum.Pose().Pos().Y() + frustum.Near() + offset,
+ tan(IGN_DTOR(30)) / frustum.AspectRatio() - offset);
+
+ Vector3d nearTopLeftBad(
+ -tan(IGN_DTOR(30)) - offset,
+ frustum.Pose().Pos().Y() + frustum.Near() - offset,
+ tan(IGN_DTOR(30)) / frustum.AspectRatio() + offset);
+
+ Vector3d nearTopRight(
+ tan(IGN_DTOR(30)) - offset,
+ frustum.Pose().Pos().Y() + frustum.Near() + offset,
+ tan(IGN_DTOR(30)) / frustum.AspectRatio() - offset);
+
+ Vector3d nearTopRightBad(
+ tan(IGN_DTOR(30)) + offset,
+ frustum.Pose().Pos().Y() + frustum.Near() - offset,
+ tan(IGN_DTOR(30)) / frustum.AspectRatio() + offset);
+
+ Vector3d nearBottomLeft(
+ -tan(IGN_DTOR(30)) + offset,
+ frustum.Pose().Pos().Y() + frustum.Near() + offset,
+ -tan(IGN_DTOR(30)) / frustum.AspectRatio() + offset);
+
+ Vector3d nearBottomLeftBad(
+ -tan(IGN_DTOR(30)) - offset,
+ frustum.Pose().Pos().Y() + frustum.Near() - offset,
+ -tan(IGN_DTOR(30)) / frustum.AspectRatio() - offset);
+
+ Vector3d nearBottomRight(
+ tan(IGN_DTOR(30)) - offset,
+ frustum.Pose().Pos().Y() + frustum.Near() + offset,
+ -tan(IGN_DTOR(30)) / frustum.AspectRatio() + offset);
+
+ Vector3d nearBottomRightBad(
+ tan(IGN_DTOR(30)) + offset,
+ frustum.Pose().Pos().Y() + frustum.Near() - offset,
+ -tan(IGN_DTOR(30)) / frustum.AspectRatio() - offset);
+
+ // Test near clip corners
+ EXPECT_TRUE(frustum.Contains(nearTopLeft));
+ EXPECT_FALSE(frustum.Contains(nearTopLeftBad));
+
+ EXPECT_TRUE(frustum.Contains(nearTopRight));
+ EXPECT_FALSE(frustum.Contains(nearTopRightBad));
+
+ EXPECT_TRUE(frustum.Contains(nearBottomLeft));
+ EXPECT_FALSE(frustum.Contains(nearBottomLeftBad));
+
+ EXPECT_TRUE(frustum.Contains(nearBottomRight));
+ EXPECT_FALSE(frustum.Contains(nearBottomRightBad));
+
+ // Compute far clip points
+ Vector3d farTopLeft(
+ -tan(IGN_DTOR(30)) * frustum.Far() + offset,
+ frustum.Pose().Pos().Y() + frustum.Far() - offset,
+ (tan(IGN_DTOR(30)) * frustum.Far()) / frustum.AspectRatio() - offset);
+
+ Vector3d farTopLeftBad(
+ -tan(IGN_DTOR(30))*frustum.Far() - offset,
+ frustum.Pose().Pos().Y() + frustum.Far() + offset,
+ (tan(IGN_DTOR(30) * frustum.Far())) / frustum.AspectRatio() + offset);
+
+ Vector3d farTopRight(
+ tan(IGN_DTOR(30))*frustum.Far() - offset,
+ frustum.Pose().Pos().Y() + frustum.Far() - offset,
+ (tan(IGN_DTOR(30)) * frustum.Far()) / frustum.AspectRatio() - offset);
+
+ Vector3d farTopRightBad(
+ tan(IGN_DTOR(30))*frustum.Far() + offset,
+ frustum.Pose().Pos().Y() + frustum.Far() + offset,
+ (tan(IGN_DTOR(30)) * frustum.Far()) / frustum.AspectRatio() + offset);
+
+ Vector3d farBottomLeft(
+ -tan(IGN_DTOR(30))*frustum.Far() + offset,
+ frustum.Pose().Pos().Y() + frustum.Far() - offset,
+ (-tan(IGN_DTOR(30)) * frustum.Far()) / frustum.AspectRatio() + offset);
+
+ Vector3d farBottomLeftBad(
+ -tan(IGN_DTOR(30))*frustum.Far() - offset,
+ frustum.Pose().Pos().Y() + frustum.Far() + offset,
+ (-tan(IGN_DTOR(30)) * frustum.Far()) / frustum.AspectRatio() - offset);
+
+ Vector3d farBottomRight(
+ tan(IGN_DTOR(30))*frustum.Far() - offset,
+ frustum.Pose().Pos().Y() + frustum.Far() - offset,
+ (-tan(IGN_DTOR(30)) * frustum.Far()) / frustum.AspectRatio() + offset);
+
+ Vector3d farBottomRightBad(
+ tan(IGN_DTOR(30))*frustum.Far() + offset,
+ frustum.Pose().Pos().Y() + frustum.Far() + offset,
+ (-tan(IGN_DTOR(30)) * frustum.Far()) / frustum.AspectRatio() - offset);
+
+ // Test far clip corners
+ EXPECT_TRUE(frustum.Contains(farTopLeft));
+ EXPECT_FALSE(frustum.Contains(farTopLeftBad));
+
+ EXPECT_TRUE(frustum.Contains(farTopRight));
+ EXPECT_FALSE(frustum.Contains(farTopRightBad));
+
+ EXPECT_TRUE(frustum.Contains(farBottomLeft));
+ EXPECT_FALSE(frustum.Contains(farBottomLeftBad));
+
+ EXPECT_TRUE(frustum.Contains(farBottomRight));
+ EXPECT_FALSE(frustum.Contains(farBottomRightBad));
+
+ // Adjust to 45 degrees rotation
+ frustum.SetPose(Pose3d(1, 1, 0, 0, 0, -M_PI*0.25));
+ EXPECT_TRUE(frustum.Contains(Vector3d(2, -1, 0)));
+ EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 0)));
+ EXPECT_FALSE(frustum.Contains(Vector3d(1, 1, 0)));
+}
diff --git a/src/Kmeans.cc b/src/Kmeans.cc
index e7558fc..d4983c2 100644
--- a/src/Kmeans.cc
+++ b/src/Kmeans.cc
@@ -114,20 +114,20 @@ bool Kmeans::Cluster(int _k,
}
// Initialize labels.
- for (auto i = 0; i < this->dataPtr->obs.size(); ++i)
+ for (auto i = 0u; i < this->dataPtr->obs.size(); ++i)
this->dataPtr->labels[i] = 0;
do
{
// Reset sums and counters.
- for (auto i = 0; i < this->dataPtr->centroids.size(); ++i)
+ for (auto i = 0u; i < this->dataPtr->centroids.size(); ++i)
{
this->dataPtr->sums[i] = Vector3d::Zero;
this->dataPtr->counters[i] = 0;
}
changed = 0;
- for (auto i = 0; i < this->dataPtr->obs.size(); ++i)
+ for (auto i = 0u; i < this->dataPtr->obs.size(); ++i)
{
// Update the labels containing the closest centroid for each point.
auto label = this->ClosestCentroid(this->dataPtr->obs[i]);
@@ -141,7 +141,7 @@ bool Kmeans::Cluster(int _k,
}
// Update the centroids.
- for (auto i = 0; i < this->dataPtr->centroids.size(); ++i)
+ for (auto i = 0u; i < this->dataPtr->centroids.size(); ++i)
{
this->dataPtr->centroids[i] =
this->dataPtr->sums[i] / this->dataPtr->counters[i];
@@ -159,7 +159,7 @@ unsigned int Kmeans::ClosestCentroid(const Vector3d &_p) const
{
double min = HUGE_VAL;
unsigned int minIdx = 0;
- for (auto i = 0; i < this->dataPtr->centroids.size(); ++i)
+ for (auto i = 0u; i < this->dataPtr->centroids.size(); ++i)
{
double d = _p.Distance(this->dataPtr->centroids[i]);
if (d < min)
diff --git a/src/Line3_TEST.cc b/src/Line3_TEST.cc
new file mode 100644
index 0000000..35347be
--- /dev/null
+++ b/src/Line3_TEST.cc
@@ -0,0 +1,166 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <gtest/gtest.h>
+
+#include "ignition/math/Line3.hh"
+#include "ignition/math/Helpers.hh"
+
+using namespace ignition;
+
+/////////////////////////////////////////////////
+TEST(Line3Test, Constructor)
+{
+ math::Line3d lineA(0, 0, 10, 10);
+ EXPECT_DOUBLE_EQ(lineA[0].X(), 0.0);
+ EXPECT_DOUBLE_EQ(lineA[0].Y(), 0.0);
+ EXPECT_DOUBLE_EQ(lineA[0].Z(), 0.0);
+ EXPECT_DOUBLE_EQ(lineA[1].X(), 10.0);
+ EXPECT_DOUBLE_EQ(lineA[1].Y(), 10.0);
+ EXPECT_DOUBLE_EQ(lineA[1].Z(), 0.0);
+
+ math::Line3d lineB(math::Vector3d(1, 2, 3), math::Vector3d(4, 5, 6));
+ EXPECT_DOUBLE_EQ(lineB[0].X(), 1.0);
+ EXPECT_DOUBLE_EQ(lineB[0].Y(), 2.0);
+ EXPECT_DOUBLE_EQ(lineB[0].Z(), 3.0);
+ EXPECT_DOUBLE_EQ(lineB[1].X(), 4.0);
+ EXPECT_DOUBLE_EQ(lineB[1].Y(), 5.0);
+ EXPECT_DOUBLE_EQ(lineB[1].Z(), 6.0);
+
+ math::Line3d lineC(0, 0, 5, 10, 10, 6);
+ EXPECT_DOUBLE_EQ(lineC[0].X(), 0.0);
+ EXPECT_DOUBLE_EQ(lineC[0].Y(), 0.0);
+ EXPECT_DOUBLE_EQ(lineC[0].Z(), 5.0);
+ EXPECT_DOUBLE_EQ(lineC[1].X(), 10.0);
+ EXPECT_DOUBLE_EQ(lineC[1].Y(), 10.0);
+ EXPECT_DOUBLE_EQ(lineC[1].Z(), 6.0);
+
+ EXPECT_THROW(lineB[2].X(), math::IndexException);
+ EXPECT_NO_THROW(lineA[0].X());
+}
+
+TEST(Line3Test, Set)
+{
+ math::Line3d lineA;
+ lineA.Set(1, 1, 2, 2);
+ EXPECT_DOUBLE_EQ(lineA[0].X(), 1.0);
+ EXPECT_DOUBLE_EQ(lineA[0].Y(), 1.0);
+ EXPECT_DOUBLE_EQ(lineA[0].Z(), 0.0);
+ EXPECT_DOUBLE_EQ(lineA[1].X(), 2.0);
+ EXPECT_DOUBLE_EQ(lineA[1].Y(), 2.0);
+ EXPECT_DOUBLE_EQ(lineA[1].Z(), 0.0);
+
+ lineA.Set(10, 11, 12, 13, 14, 15);
+ EXPECT_DOUBLE_EQ(lineA[0].X(), 10.0);
+ EXPECT_DOUBLE_EQ(lineA[0].Y(), 11.0);
+ EXPECT_DOUBLE_EQ(lineA[0].Z(), 12.0);
+ EXPECT_DOUBLE_EQ(lineA[1].X(), 13.0);
+ EXPECT_DOUBLE_EQ(lineA[1].Y(), 14.0);
+ EXPECT_DOUBLE_EQ(lineA[1].Z(), 15.0);
+
+ lineA.SetA(math::Vector3<double>(0, -1, -2));
+ EXPECT_DOUBLE_EQ(lineA[0].X(), 0.0);
+ EXPECT_DOUBLE_EQ(lineA[0].Y(), -1.0);
+ EXPECT_DOUBLE_EQ(lineA[0].Z(), -2.0);
+ EXPECT_DOUBLE_EQ(lineA[1].X(), 13.0);
+ EXPECT_DOUBLE_EQ(lineA[1].Y(), 14.0);
+ EXPECT_DOUBLE_EQ(lineA[1].Z(), 15.0);
+
+ lineA.SetB(math::Vector3<double>(5, 6, 7));
+ EXPECT_DOUBLE_EQ(lineA[0].X(), 0.0);
+ EXPECT_DOUBLE_EQ(lineA[0].Y(), -1.0);
+ EXPECT_DOUBLE_EQ(lineA[0].Z(), -2.0);
+ EXPECT_DOUBLE_EQ(lineA[1].X(), 5.0);
+ EXPECT_DOUBLE_EQ(lineA[1].Y(), 6.0);
+ EXPECT_DOUBLE_EQ(lineA[1].Z(), 7.0);
+}
+
+/////////////////////////////////////////////////
+TEST(Line3Test, Length)
+{
+ math::Line3d lineA(0, 0, 0, 10, 10, 10);
+ EXPECT_NEAR(lineA.Length(), sqrt(300), 1e-10);
+}
+
+/////////////////////////////////////////////////
+TEST(Line3Test, Equality)
+{
+ math::Line3d lineA(1, 1, 1, 2, 1, 2);
+ math::Line3d lineB(1, 2, 3, 2, 2, 4);
+
+ EXPECT_TRUE(lineA != lineB);
+ EXPECT_TRUE(lineA == lineA);
+
+ lineB.Set(1, 1, 1, 2, 1.1, 2);
+ EXPECT_FALSE(lineA == lineB);
+
+ lineB.Set(1, 1, 1, 2.1, 1, 2);
+ EXPECT_FALSE(lineA == lineB);
+
+ lineB.Set(1, 1, 1.1, 2, 1, 2);
+ EXPECT_FALSE(lineA == lineB);
+
+ lineB.Set(1.1, 1, 1, 2, 1, 2);
+ EXPECT_FALSE(lineA == lineB);
+}
+
+/////////////////////////////////////////////////
+TEST(Line3Test, OperatorStreamOut)
+{
+ math::Line3d line(0, 1, 4, 2, 3, 7);
+ std::ostringstream stream;
+ stream << line;
+ EXPECT_EQ(stream.str(), "0 1 4 2 3 7");
+}
+
+/////////////////////////////////////////////////
+TEST(Line3Test, CopyConstructor)
+{
+ math::Line3d lineA(0, 1, 4, 2, 3, 7);
+ math::Line3d lineB(lineA);
+
+ EXPECT_EQ(lineA, lineB);
+}
+
+/////////////////////////////////////////////////
+TEST(Line3Test, OperatorAssign)
+{
+ math::Line3d lineA(0, 1, 4, 2, 3, 7);
+ math::Line3d lineB = lineA;
+
+ EXPECT_EQ(lineA, lineB);
+}
+
+/////////////////////////////////////////////////
+TEST(Line3Test, Direction)
+{
+ math::Line3d lineA(1, 1, 1, 0, 0, 0);
+ math::Line3d lineB(2, 2, 2, 0, 0, 0);
+ math::Line3d lineC(0, 0, 0, 1, 1, 1);
+ EXPECT_TRUE(lineA.Direction() == (lineA[1] - lineA[0]).Normalize());
+ EXPECT_TRUE(lineA.Direction() == lineB.Direction());
+ EXPECT_FALSE(lineA.Direction() == lineC.Direction());
+
+ lineA.Set(1, 1, 2, 1, 1, 10);
+ EXPECT_TRUE(lineA.Direction() == math::Vector3d::UnitZ);
+
+ lineA.Set(1, 5, 1, 1, 1, 1);
+ EXPECT_TRUE(lineA.Direction() == -math::Vector3d::UnitY);
+
+ lineA.Set(1, 1, 1, 7, 1, 1);
+ EXPECT_TRUE(lineA.Direction() == math::Vector3d::UnitX);
+}
diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc
index 029d012..27430c2 100644
--- a/src/Plane_TEST.cc
+++ b/src/Plane_TEST.cc
@@ -21,59 +21,120 @@
#include "ignition/math/Plane.hh"
using namespace ignition;
+using namespace math;
/////////////////////////////////////////////////
TEST(PlaneTest, PlaneConstructor)
{
- math::Planed plane(math::Vector3d(1, 0, 0), 0.1);
- EXPECT_EQ(plane.Normal(), math::Vector3d(1, 0, 0));
+ Planed plane(Vector3d(1, 0, 0), 0.1);
+ EXPECT_EQ(plane.Normal(), Vector3d(1, 0, 0));
EXPECT_NEAR(plane.Offset(), 0.1, 1e-6);
}
/////////////////////////////////////////////////
TEST(PlaneTest, Distance)
{
- math::Planed plane(math::Vector3d(0, 0, 1), 0.1);
- EXPECT_NEAR(plane.Distance(math::Vector3d(0, 0, 0),
- math::Vector3d(0, 0, 1)), 0.1, 1e-6);
+ Planed plane(Vector3d(0, 0, 1), 0.1);
+ EXPECT_NEAR(plane.Distance(Vector3d(0, 0, 0),
+ Vector3d(0, 0, 1)), 0.1, 1e-6);
- EXPECT_NEAR(plane.Distance(math::Vector3d(0, 0, 0.1),
- math::Vector3d(0, 0, 1)), 0, 1e-6);
+ EXPECT_NEAR(plane.Distance(Vector3d(0, 0, 0.1),
+ Vector3d(0, 0, 1)), 0, 1e-6);
- EXPECT_NEAR(plane.Distance(math::Vector3d(0, 0, 0.2),
- math::Vector3d(0, 0, 1)), -0.1, 1e-6);
+ EXPECT_NEAR(plane.Distance(Vector3d(0, 0, 0.2),
+ Vector3d(0, 0, 1)), -0.1, 1e-6);
- EXPECT_NEAR(plane.Distance(math::Vector3d(0, 0, 0.1),
- math::Vector3d(1, 0, 0)), 0, 1e-6);
+ EXPECT_NEAR(plane.Distance(Vector3d(0, 0, 0.1),
+ Vector3d(1, 0, 0)), 0, 1e-6);
}
/////////////////////////////////////////////////
TEST(PlaneTest, Plane)
{
{
- math::Planed plane;
- EXPECT_TRUE(math::equal(plane.Offset(), 0.0));
- EXPECT_TRUE(plane.Normal() == math::Vector3d());
- EXPECT_TRUE(plane.Size() == math::Vector2d(0, 0));
+ Planed plane;
+ EXPECT_TRUE(equal(plane.Offset(), 0.0));
+ EXPECT_TRUE(plane.Normal() == Vector3d());
+ EXPECT_TRUE(plane.Size() == Vector2d(0, 0));
}
{
- math::Planed plane(math::Vector3d(0, 0, 1), math::Vector2d(2, 3), 2.0);
- EXPECT_TRUE(math::equal(plane.Offset(), 2.0));
- EXPECT_TRUE(plane.Normal() == math::Vector3d(0, 0, 1));
- EXPECT_TRUE(plane.Size() == math::Vector2d(2, 3));
-
- EXPECT_DOUBLE_EQ(-1, plane.Distance(math::Vector3d(0, 0, 1),
- math::Vector3d(0, 0, -1)));
-
- plane.Set(math::Vector3d(1, 0, 0), math::Vector2d(1, 1), 1.0);
- EXPECT_TRUE(math::equal(plane.Offset(), 1.0));
- EXPECT_TRUE(plane.Normal() == math::Vector3d(1, 0, 0));
- EXPECT_TRUE(plane.Size() == math::Vector2d(1, 1));
-
- plane = math::Planed(math::Vector3d(0, 1, 0), math::Vector2d(4, 4), 5.0);
- EXPECT_TRUE(math::equal(plane.Offset(), 5.0));
- EXPECT_TRUE(plane.Normal() == math::Vector3d(0, 1, 0));
- EXPECT_TRUE(plane.Size() == math::Vector2d(4, 4));
+ Planed plane(Vector3d(0, 0, 1), Vector2d(2, 3), 2.0);
+ EXPECT_TRUE(equal(plane.Offset(), 2.0));
+ EXPECT_TRUE(plane.Normal() == Vector3d(0, 0, 1));
+ EXPECT_TRUE(plane.Size() == Vector2d(2, 3));
+
+ EXPECT_DOUBLE_EQ(-1, plane.Distance(Vector3d(0, 0, 1),
+ Vector3d(0, 0, -1)));
+
+ plane.Set(Vector3d(1, 0, 0), Vector2d(1, 1), 1.0);
+ EXPECT_TRUE(equal(plane.Offset(), 1.0));
+ EXPECT_TRUE(plane.Normal() == Vector3d(1, 0, 0));
+ EXPECT_TRUE(plane.Size() == Vector2d(1, 1));
+
+ plane = Planed(Vector3d(0, 1, 0), Vector2d(4, 4), 5.0);
+ EXPECT_TRUE(equal(plane.Offset(), 5.0));
+ EXPECT_TRUE(plane.Normal() == Vector3d(0, 1, 0));
+ EXPECT_TRUE(plane.Size() == Vector2d(4, 4));
+ }
+}
+
+/////////////////////////////////////////////////
+TEST(PlaneTest, SidePoint)
+{
+ Planed plane(Vector3d(0, 0, 1), 1);
+
+ // On the negative side of the plane (below the plane)
+ Vector3d point(0, 0, 0);
+ EXPECT_EQ(plane.Side(point), Planed::NEGATIVE_SIDE);
+
+ // Still on the negative side of the plane (below the plane)
+ point.Set(1, 1, 0);
+ EXPECT_EQ(plane.Side(point), Planed::NEGATIVE_SIDE);
+
+ // Above the plane (positive side)
+ point.Set(1, 1, 2);
+ EXPECT_EQ(plane.Side(point), Planed::POSITIVE_SIDE);
+
+ // On the plane
+ point.Set(0, 0, 1);
+ EXPECT_EQ(plane.Side(point), Planed::NO_SIDE);
+
+ // Change the plane, but the point is still on the negative side
+ plane.Set(Vector3d(1, 0, 0), 4);
+ EXPECT_EQ(plane.Side(point), Planed::NEGATIVE_SIDE);
+
+ // Point is now on the positive side
+ point.Set(4.1, 0, 1);
+ EXPECT_EQ(plane.Side(point), Planed::POSITIVE_SIDE);
+}
+
+/////////////////////////////////////////////////
+TEST(PlaneTest, SideBox)
+{
+ Planed plane(Vector3d(0, 0, 1), 1);
+
+ // On the negative side of the plane (below the plane)
+ {
+ Box box(Vector3d(-.5, -.5, -.5), Vector3d(.5, .5, .5));
+ EXPECT_EQ(plane.Side(box), Planed::NEGATIVE_SIDE);
+ }
+
+ // Still on the negative side of the plane (below the plane)
+ {
+ Box box(Vector3d(-10, -10, -10), Vector3d(.9, .9, .9));
+ EXPECT_EQ(plane.Side(box), Planed::NEGATIVE_SIDE);
+ }
+
+ // Above the plane (positive side)
+ {
+ Box box(Vector3d(2, 2, 2), Vector3d(3, 3, 3));
+ EXPECT_EQ(plane.Side(box), Planed::POSITIVE_SIDE);
+ }
+
+ // On both sides the plane
+ {
+ Box box(Vector3d(0, 0, 0), Vector3d(3, 3, 3));
+ EXPECT_EQ(plane.Side(box), Planed::BOTH_SIDE);
}
}
diff --git a/src/SignalStats.cc b/src/SignalStats.cc
index 931a050..af55e1f 100644
--- a/src/SignalStats.cc
+++ b/src/SignalStats.cc
@@ -52,6 +52,28 @@ void SignalStatistic::Reset()
}
//////////////////////////////////////////////////
+double SignalMaximum::Value() const
+{
+ return this->dataPtr->data;
+}
+
+//////////////////////////////////////////////////
+std::string SignalMaximum::ShortName() const
+{
+ return "max";
+}
+
+//////////////////////////////////////////////////
+void SignalMaximum::InsertData(const double _data)
+{
+ if (this->dataPtr->count == 0 || _data > this->dataPtr->data)
+ {
+ this->dataPtr->data = _data;
+ }
+ this->dataPtr->count++;
+}
+
+//////////////////////////////////////////////////
double SignalMean::Value() const
{
if (this->dataPtr->count == 0)
@@ -75,6 +97,28 @@ void SignalMean::InsertData(const double _data)
}
//////////////////////////////////////////////////
+double SignalMinimum::Value() const
+{
+ return this->dataPtr->data;
+}
+
+//////////////////////////////////////////////////
+std::string SignalMinimum::ShortName() const
+{
+ return "min";
+}
+
+//////////////////////////////////////////////////
+void SignalMinimum::InsertData(const double _data)
+{
+ if (this->dataPtr->count == 0 || _data < this->dataPtr->data)
+ {
+ this->dataPtr->data = _data;
+ }
+ this->dataPtr->count++;
+}
+
+//////////////////////////////////////////////////
double SignalRootMeanSquare::Value() const
{
if (this->dataPtr->count == 0)
@@ -213,25 +257,29 @@ bool SignalStats::InsertStatistic(const std::string &_name)
}
SignalStatisticPtr stat;
- if (_name == "maxAbs")
+ if (_name == "max")
+ {
+ stat.reset(new SignalMaximum());
+ }
+ else if (_name == "maxAbs")
{
stat.reset(new SignalMaxAbsoluteValue());
- this->dataPtr->stats.push_back(stat);
}
else if (_name == "mean")
{
stat.reset(new SignalMean());
- this->dataPtr->stats.push_back(stat);
+ }
+ else if (_name == "min")
+ {
+ stat.reset(new SignalMinimum());
}
else if (_name == "rms")
{
stat.reset(new SignalRootMeanSquare());
- this->dataPtr->stats.push_back(stat);
}
else if (_name == "var")
{
stat.reset(new SignalVariance());
- this->dataPtr->stats.push_back(stat);
}
else
{
@@ -242,6 +290,7 @@ bool SignalStats::InsertStatistic(const std::string &_name)
<< std::endl;
return false;
}
+ this->dataPtr->stats.push_back(stat);
return true;
}
diff --git a/src/SignalStats_TEST.cc b/src/SignalStats_TEST.cc
index 578502a..20ce48e 100644
--- a/src/SignalStats_TEST.cc
+++ b/src/SignalStats_TEST.cc
@@ -22,6 +22,79 @@
using namespace ignition;
+//////////////////////////////////////////////////
+TEST(SignalStatsTest, SignalMaximumConstructor)
+{
+ // Constructor
+ math::SignalMaximum max;
+ EXPECT_DOUBLE_EQ(max.Value(), 0.0);
+ EXPECT_EQ(max.Count(), 0u);
+ EXPECT_EQ(max.ShortName(), std::string("max"));
+
+ // Reset
+ max.Reset();
+ EXPECT_DOUBLE_EQ(max.Value(), 0.0);
+ EXPECT_EQ(max.Count(), 0u);
+}
+
+//////////////////////////////////////////////////
+TEST(SignalStatsTest, SignalMaximumConstantValues)
+{
+ // Constant values, max should match
+ math::SignalMaximum max;
+ EXPECT_DOUBLE_EQ(max.Value(), 0.0);
+ EXPECT_EQ(max.Count(), 0u);
+
+ const double value = 3.14159;
+
+ // Loop two times to verify Reset
+ for (int j = 0; j < 2; ++j)
+ {
+ for (unsigned int i = 1; i <= 10; ++i)
+ {
+ max.InsertData(value);
+ EXPECT_DOUBLE_EQ(max.Value(), value);
+ EXPECT_EQ(max.Count(), i);
+ }
+
+ // Reset
+ max.Reset();
+ EXPECT_DOUBLE_EQ(max.Value(), 0.0);
+ EXPECT_EQ(max.Count(), 0u);
+ }
+}
+
+//////////////////////////////////////////////////
+TEST(SignalStatsTest, SignalMaximumAlternatingValues)
+{
+ // Values with alternating sign, increasing magnitude
+ // Should always match positive value
+ math::SignalMaximum max;
+ EXPECT_DOUBLE_EQ(max.Value(), 0.0);
+ EXPECT_EQ(max.Count(), 0u);
+
+ const double value = 3.14159;
+
+ // Loop two times to verify Reset
+ for (int j = 0; j < 2; ++j)
+ {
+ for (unsigned int i = 1; i <= 10; ++i)
+ {
+ max.InsertData(value * i);
+ EXPECT_DOUBLE_EQ(max.Value(), value * i);
+ max.InsertData(-value * i);
+ EXPECT_DOUBLE_EQ(max.Value(), value * i);
+ EXPECT_EQ(max.Count(), i*2);
+ }
+
+ // Reset
+ max.Reset();
+ EXPECT_DOUBLE_EQ(max.Value(), 0.0);
+ EXPECT_EQ(max.Count(), 0u);
+ }
+}
+
+//////////////////////////////////////////////////
TEST(SignalStatsTest, SignalMean)
{
{
@@ -51,7 +124,7 @@ TEST(SignalStatsTest, SignalMean)
for (unsigned int i = 1; i <= 10; ++i)
{
mean.InsertData(value);
- EXPECT_NEAR(mean.Value(), value, 1e-10);
+ EXPECT_DOUBLE_EQ(mean.Value(), value);
EXPECT_EQ(mean.Count(), i);
}
@@ -78,7 +151,7 @@ TEST(SignalStatsTest, SignalMean)
{
mean.InsertData(value * i);
mean.InsertData(-value * i);
- EXPECT_NEAR(mean.Value(), 0.0, 1e-10);
+ EXPECT_DOUBLE_EQ(mean.Value(), 0.0);
EXPECT_EQ(mean.Count(), i*2);
}
@@ -90,6 +163,78 @@ TEST(SignalStatsTest, SignalMean)
}
}
+//////////////////////////////////////////////////
+TEST(SignalStatsTest, SignalMinimumConstructor)
+{
+ // Constructor
+ math::SignalMinimum min;
+ EXPECT_DOUBLE_EQ(min.Value(), 0.0);
+ EXPECT_EQ(min.Count(), 0u);
+ EXPECT_EQ(min.ShortName(), std::string("min"));
+
+ // Reset
+ min.Reset();
+ EXPECT_DOUBLE_EQ(min.Value(), 0.0);
+ EXPECT_EQ(min.Count(), 0u);
+}
+
+//////////////////////////////////////////////////
+TEST(SignalStatsTest, SignalMinimumConstantValues)
+{
+ // Constant values, min should match
+ math::SignalMinimum min;
+ EXPECT_DOUBLE_EQ(min.Value(), 0.0);
+ EXPECT_EQ(min.Count(), 0u);
+
+ const double value = 3.14159;
+
+ // Loop two times to verify Reset
+ for (int j = 0; j < 2; ++j)
+ {
+ for (unsigned int i = 1; i <= 10; ++i)
+ {
+ min.InsertData(value);
+ EXPECT_DOUBLE_EQ(min.Value(), value);
+ EXPECT_EQ(min.Count(), i);
+ }
+
+ // Reset
+ min.Reset();
+ EXPECT_DOUBLE_EQ(min.Value(), 0.0);
+ EXPECT_EQ(min.Count(), 0u);
+ }
+}
+
+//////////////////////////////////////////////////
+TEST(SignalStatsTest, SignalMinimumAlternatingValues)
+{
+ // Values with alternating sign, increasing magnitude
+ // Should always match negative value
+ math::SignalMinimum min;
+ EXPECT_DOUBLE_EQ(min.Value(), 0.0);
+ EXPECT_EQ(min.Count(), 0u);
+
+ const double value = 3.14159;
+
+ // Loop two times to verify Reset
+ for (int j = 0; j < 2; ++j)
+ {
+ for (unsigned int i = 1; i <= 10; ++i)
+ {
+ min.InsertData(value * i);
+ min.InsertData(-value * i);
+ EXPECT_DOUBLE_EQ(min.Value(), -value * i);
+ EXPECT_EQ(min.Count(), i*2);
+ }
+
+ // Reset
+ min.Reset();
+ EXPECT_DOUBLE_EQ(min.Value(), 0.0);
+ EXPECT_EQ(min.Count(), 0u);
+ }
+}
+
+//////////////////////////////////////////////////
TEST(SignalStatsTest, SignalRootMeanSquare)
{
{
@@ -119,7 +264,7 @@ TEST(SignalStatsTest, SignalRootMeanSquare)
for (unsigned int i = 1; i <= 10; ++i)
{
rms.InsertData(value);
- EXPECT_NEAR(rms.Value(), value, 1e-10);
+ EXPECT_DOUBLE_EQ(rms.Value(), value);
EXPECT_EQ(rms.Count(), i);
}
@@ -145,11 +290,11 @@ TEST(SignalStatsTest, SignalRootMeanSquare)
for (unsigned int i = 1; i <= 10; ++i)
{
rms.InsertData(value);
- EXPECT_NEAR(rms.Value(), value, 1e-10);
+ EXPECT_DOUBLE_EQ(rms.Value(), value);
EXPECT_EQ(rms.Count(), i*2-1);
rms.InsertData(-value);
- EXPECT_NEAR(rms.Value(), value, 1e-10);
+ EXPECT_DOUBLE_EQ(rms.Value(), value);
EXPECT_EQ(rms.Count(), i*2);
}
@@ -191,7 +336,7 @@ TEST(SignalStatsTest, SignalMaxAbsoluteValue)
for (unsigned int i = 1; i <= 10; ++i)
{
max.InsertData(value);
- EXPECT_NEAR(max.Value(), value, 1e-10);
+ EXPECT_DOUBLE_EQ(max.Value(), value);
EXPECT_EQ(max.Count(), i);
}
@@ -217,11 +362,11 @@ TEST(SignalStatsTest, SignalMaxAbsoluteValue)
for (unsigned int i = 1; i <= 10; ++i)
{
max.InsertData(value * i);
- EXPECT_NEAR(max.Value(), value * i, 1e-10);
+ EXPECT_DOUBLE_EQ(max.Value(), value * i);
EXPECT_EQ(max.Count(), i*2-1);
max.InsertData(-value * i);
- EXPECT_NEAR(max.Value(), value * i, 1e-10);
+ EXPECT_DOUBLE_EQ(max.Value(), value * i);
EXPECT_EQ(max.Count(), i*2);
}
@@ -341,6 +486,10 @@ TEST(SignalStatsTest, SignalStats)
math::SignalStats stats;
EXPECT_TRUE(stats.Map().empty());
+ EXPECT_TRUE(stats.InsertStatistic("max"));
+ EXPECT_FALSE(stats.InsertStatistic("max"));
+ EXPECT_FALSE(stats.Map().empty());
+
EXPECT_TRUE(stats.InsertStatistic("maxAbs"));
EXPECT_FALSE(stats.InsertStatistic("maxAbs"));
EXPECT_FALSE(stats.Map().empty());
@@ -349,6 +498,10 @@ TEST(SignalStatsTest, SignalStats)
EXPECT_FALSE(stats.InsertStatistic("mean"));
EXPECT_FALSE(stats.Map().empty());
+ EXPECT_TRUE(stats.InsertStatistic("min"));
+ EXPECT_FALSE(stats.InsertStatistic("min"));
+ EXPECT_FALSE(stats.Map().empty());
+
EXPECT_TRUE(stats.InsertStatistic("rms"));
EXPECT_FALSE(stats.InsertStatistic("rms"));
EXPECT_FALSE(stats.Map().empty());
@@ -362,9 +515,11 @@ TEST(SignalStatsTest, SignalStats)
// Map with no data
std::map<std::string, double> map = stats.Map();
EXPECT_FALSE(map.empty());
- EXPECT_EQ(map.size(), 4u);
+ EXPECT_EQ(map.size(), 6u);
+ EXPECT_EQ(map.count("max"), 1u);
EXPECT_EQ(map.count("maxAbs"), 1u);
EXPECT_EQ(map.count("mean"), 1u);
+ EXPECT_EQ(map.count("min"), 1u);
EXPECT_EQ(map.count("rms"), 1u);
EXPECT_EQ(map.count("var"), 1u);
EXPECT_EQ(map.count("FakeStatistic"), 0u);
@@ -389,15 +544,23 @@ TEST(SignalStatsTest, SignalStats)
EXPECT_FALSE(stats.InsertStatistics("var,FakeStatistic"));
EXPECT_EQ(stats.Map().size(), 4u);
+ EXPECT_FALSE(stats.InsertStatistics("max,FakeStatistic"));
+ EXPECT_EQ(stats.Map().size(), 5u);
+
+ EXPECT_FALSE(stats.InsertStatistics("min,FakeStatistic"));
+ EXPECT_EQ(stats.Map().size(), 6u);
+
EXPECT_FALSE(stats.InsertStatistics("FakeStatistic"));
- EXPECT_EQ(stats.Map().size(), 4u);
+ EXPECT_EQ(stats.Map().size(), 6u);
// Map with no data
std::map<std::string, double> map = stats.Map();
EXPECT_FALSE(map.empty());
- EXPECT_EQ(map.size(), 4u);
+ EXPECT_EQ(map.size(), 6u);
+ EXPECT_EQ(map.count("max"), 1u);
EXPECT_EQ(map.count("maxAbs"), 1u);
EXPECT_EQ(map.count("mean"), 1u);
+ EXPECT_EQ(map.count("min"), 1u);
EXPECT_EQ(map.count("rms"), 1u);
EXPECT_EQ(map.count("var"), 1u);
EXPECT_EQ(map.count("FakeStatistic"), 0u);
@@ -406,8 +569,8 @@ TEST(SignalStatsTest, SignalStats)
{
// Add some statistics
math::SignalStats stats;
- EXPECT_TRUE(stats.InsertStatistics("maxAbs,mean,rms"));
- EXPECT_EQ(stats.Map().size(), 3u);
+ EXPECT_TRUE(stats.InsertStatistics("max,maxAbs,mean,min,rms"));
+ EXPECT_EQ(stats.Map().size(), 5u);
// No data yet
EXPECT_EQ(stats.Count(), 0u);
@@ -420,17 +583,21 @@ TEST(SignalStatsTest, SignalStats)
{
std::map<std::string, double> map = stats.Map();
- EXPECT_NEAR(map["maxAbs"], value, 1e-10);
- EXPECT_NEAR(map["rms"], value, 1e-10);
- EXPECT_NEAR(map["mean"], 0.0, 1e-10);
+ EXPECT_DOUBLE_EQ(map["max"], value);
+ EXPECT_DOUBLE_EQ(map["maxAbs"], value);
+ EXPECT_DOUBLE_EQ(map["min"], -value);
+ EXPECT_DOUBLE_EQ(map["rms"], value);
+ EXPECT_DOUBLE_EQ(map["mean"], 0.0);
}
stats.Reset();
- EXPECT_EQ(stats.Map().size(), 3u);
+ EXPECT_EQ(stats.Map().size(), 5u);
EXPECT_EQ(stats.Count(), 0u);
{
std::map<std::string, double> map = stats.Map();
+ EXPECT_DOUBLE_EQ(map["max"], 0.0);
EXPECT_DOUBLE_EQ(map["maxAbs"], 0.0);
+ EXPECT_DOUBLE_EQ(map["min"], 0.0);
EXPECT_DOUBLE_EQ(map["rms"], 0.0);
EXPECT_DOUBLE_EQ(map["mean"], 0.0);
}
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/ignition-math2.git
More information about the debian-science-commits
mailing list