[ignition-math2] 02/04: Imported Upstream version 2.4.1
Jose Luis Rivero
jrivero-guest at moszumanska.debian.org
Sat Jul 16 00:44:46 UTC 2016
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 b0465c9b624521ae55aefa86fc5e72826f0fbae7
Author: Jose Luis Rivero <jrivero at osrfoundation.org>
Date: Sat Jul 16 00:43:36 2016 +0000
Imported Upstream version 2.4.1
---
.hg_archival.txt | 9 +-
CMakeLists.txt | 110 ++---
Changelog.md | 20 +-
appveyor.yml | 22 +
cmake/DefaultCFlags.cmake | 2 +-
cmake/TestUtils.cmake | 2 +-
cmake/Utils.cmake | 18 +-
cmake/config.hh.in | 2 +-
cmake/cpack_options.cmake.in | 8 +-
cmake/ignition-math-config.cmake.in | 41 +-
configure.bat | 4 +-
examples/CMakeLists.txt | 10 +-
include/ignition/math/AffineException.hh | 4 +-
include/ignition/math/Angle.hh | 4 +-
include/ignition/math/Box.hh | 75 +++-
include/ignition/math/BoxPrivate.hh | 15 +-
include/ignition/math/CMakeLists.txt | 8 +-
include/ignition/math/Filter.hh | 4 +-
include/ignition/math/Frustum.hh | 4 +-
include/ignition/math/FrustumPrivate.hh | 4 +-
include/ignition/math/Helpers.hh | 191 ++++++---
include/ignition/math/IndexException.hh | 4 +-
include/ignition/math/Inertial.hh | 206 ++++++++++
include/ignition/math/Kmeans.hh | 4 +-
include/ignition/math/KmeansPrivate.hh | 4 +-
include/ignition/math/Line2.hh | 4 +-
include/ignition/math/Line3.hh | 191 +--------
include/ignition/math/MassMatrix3.hh | 530 +++++++++++++++++++++++-
include/ignition/math/MassMatrix3.ipynb | 134 +++++++
include/ignition/math/Matrix3.hh | 49 ++-
include/ignition/math/Matrix4.hh | 4 +-
include/ignition/math/Plane.hh | 9 +-
include/ignition/math/Pose3.hh | 4 +-
include/ignition/math/Quaternion.hh | 144 ++++++-
include/ignition/math/Rand.hh | 16 +-
include/ignition/math/RotationSpline.hh | 4 +-
include/ignition/math/RotationSplinePrivate.hh | 4 +-
include/ignition/math/SignalStats.hh | 9 +-
include/ignition/math/SignalStatsPrivate.hh | 18 +-
include/ignition/math/Spline.hh | 4 +-
include/ignition/math/SplinePrivate.hh | 4 +-
include/ignition/math/Temperature.hh | 375 +++++++++++++++++
include/ignition/math/Triangle.hh | 4 +-
include/ignition/math/Triangle3.hh | 285 -------------
include/ignition/math/Vector2.hh | 6 +-
include/ignition/math/Vector3.hh | 23 +-
include/ignition/math/Vector3Stats.hh | 4 +-
include/ignition/math/Vector3StatsPrivate.hh | 4 +-
include/ignition/math/Vector4.hh | 4 +-
src/Angle_TEST.cc | 10 +-
src/Box.cc | 91 +++++
src/BoxPrivate.cc | 27 --
src/Box_TEST.cc | 219 +++++++++-
src/CMakeLists.txt | 22 +-
src/Frustum_TEST.cc | 25 +-
src/Helpers.cc | 31 +-
src/Helpers_TEST.cc | 107 +++++
src/Inertial_TEST.cc | 349 ++++++++++++++++
src/Kmeans_TEST.cc | 6 +-
src/Line2_TEST.cc | 3 +
src/Line3_TEST.cc | 126 ------
src/MassMatrix3_TEST.cc | 534 ++++++++++++++++++++++++-
src/Matrix3_TEST.cc | 71 +++-
src/Plane_TEST.cc | 3 +
src/Pose_TEST.cc | 1 -
src/Quaternion_TEST.cc | 108 ++++-
src/Rand.cc | 2 +-
src/Rand_TEST.cc | 16 +-
src/SignalStats.cc | 6 +
src/SignalStats_TEST.cc | 15 +
src/Temperature.cc | 325 +++++++++++++++
src/Temperature_TEST.cc | 178 +++++++++
src/Triangle3_TEST.cc | 258 ------------
src/Vector2_TEST.cc | 5 +-
src/Vector3_TEST.cc | 4 +
tools/code_check.sh | 6 +-
tools/cppcheck_rules/header_guard.rule | 10 +
tools/cppcheck_rules/namespace_AZ.rule | 9 +
78 files changed, 3977 insertions(+), 1198 deletions(-)
diff --git a/.hg_archival.txt b/.hg_archival.txt
index 29641d2..53b413f 100644
--- a/.hg_archival.txt
+++ b/.hg_archival.txt
@@ -1,5 +1,6 @@
repo: e97318167882c5edd21067dd86576efadebd68ed
-node: 6e625a118d4525b0c88b6d57dfdd50cf76512656
-branch: default
-latesttag: ignition-math2_2.2.3
-latesttagdistance: 59
+node: 255fe947055f8de476430ca1ba5cb4e42b61ee0d
+branch: ign-math2
+latesttag: ignition-math2_2.4.0
+latesttagdistance: 16
+changessincelatesttag: 17
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a71026a..93f4d1b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,35 +1,28 @@
-cmake_minimum_required(VERSION 2.8.12 FATAL_ERROR)
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
set (IGN_PROJECT_NAME "math")
-set (PROJECT_MAJOR_VERSION 2)
-set (PROJECT_MINOR_VERSION 3)
-set (PROJECT_PATCH_VERSION 0)
-
-project (ignition-${IGN_PROJECT_NAME}${PROJECT_MAJOR_VERSION})
-set(PROJECT_NAME_NO_VERSION "ignition-${IGN_PROJECT_NAME}")
-string (TOLOWER ${PROJECT_NAME_NO_VERSION} PROJECT_NAME_NO_VERSION_LOWER)
-string (TOUPPER ${PROJECT_NAME_NO_VERSION} PROJECT_NAME_NO_VERSION_UPPER)
+project (ignition-${IGN_PROJECT_NAME})
string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
-set(PROJECT_EXPORT_NAME ${PROJECT_NAME_LOWER})
-set(PROJECT_LIBRARY_TARGET_NAME ${PROJECT_NAME_LOWER})
+
+set (PROJECT_MAJOR_VERSION 2)
+set (PROJECT_MINOR_VERSION 4)
+set (PROJECT_PATCH_VERSION 1)
set (PROJECT_VERSION ${PROJECT_MAJOR_VERSION}.${PROJECT_MINOR_VERSION})
set (PROJECT_VERSION_FULL
${PROJECT_MAJOR_VERSION}.${PROJECT_MINOR_VERSION}.${PROJECT_PATCH_VERSION})
-
-message (STATUS "${PROJECT_NAME_NO_VERSION} version ${PROJECT_VERSION_FULL}")
+message (STATUS "${PROJECT_NAME} version ${PROJECT_VERSION_FULL}")
set (project_cmake_dir ${PROJECT_SOURCE_DIR}/cmake
CACHE PATH "Location of CMake scripts")
include (${project_cmake_dir}/Utils.cmake)
-include (CMakePackageConfigHelpers)
########################################
-# Package Creation:
+# Package Creation:
include (${project_cmake_dir}/cpack.cmake)
set (CPACK_PACKAGE_VERSION "${PROJECT_VERSION_FULL}")
set (CPACK_PACKAGE_VERSION_MAJOR "${PROJECT_MAJOR_VERSION}")
@@ -99,28 +92,26 @@ if(LD_LIBRARY_PATH)
endif()
-set (INCLUDE_INSTALL_DIR "${CMAKE_INSTALL_INCLUDEDIR}")
-set (INCLUDE_INSTALL_DIR_POSTFIX "ignition/${IGN_PROJECT_NAME}${PROJECT_MAJOR_VERSION}")
-set (INCLUDE_INSTALL_DIR_FULL "${INCLUDE_INSTALL_DIR}/${INCLUDE_INSTALL_DIR_POSTFIX}")
+set (INCLUDE_INSTALL_DIR "${CMAKE_INSTALL_INCLUDEDIR}/ignition")
set (LIB_INSTALL_DIR ${CMAKE_INSTALL_LIBDIR})
set (BIN_INSTALL_DIR ${CMAKE_INSTALL_BINDIR})
-
+
set (USE_FULL_RPATH OFF CACHE BOOL "Set to true to enable full rpath")
if (USE_FULL_RPATH)
# use, i.e. don't skip the full RPATH for the build tree
set(CMAKE_SKIP_BUILD_RPATH FALSE)
-
+
# when building, don't use the install RPATH already
# (but later on when installing)
- set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
-
+ set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+
set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${LIB_INSTALL_DIR}")
-
+
# add the automatically determined parts of the RPATH
# which point to directories outside the build tree to the install RPATH
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
-
+
# the RPATH to be used when installing, but only if its not a system directory
list(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/${LIB_INSTALL_DIR}" isSystemDir)
if("${isSystemDir}" STREQUAL "-1")
@@ -142,10 +133,10 @@ message (STATUS "----------------------------------------\n")
MESSAGE(STATUS "Checking ignition build type")
# Set the default build type
if (NOT CMAKE_BUILD_TYPE)
- set (CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING
+ set (CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING
"Choose the type of build, options are: Debug Release RelWithDebInfo Profile Check" FORCE)
endif (NOT CMAKE_BUILD_TYPE)
-# TODO: still convert to uppercase to keep backwards compatibility with
+# TODO: still convert to uppercase to keep backwards compatibility with
# uppercase old supported and deprecated modes
string(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE_UPPERCASE)
@@ -176,7 +167,7 @@ unset (CMAKE_C_FLAGS_ALL CACHE)
unset (CMAKE_CXX_FLAGS CACHE)
# Check if warning options are avaliable for the compiler and return WARNING_CXX_FLAGS variable
-# MSVC generates tons of warnings on gtest code.
+# MSVC generates tons of warnings on gtest code.
# Recommended to use /W4 instead of /Wall
if (MSVC)
set(WARN_LEVEL "/W4")
@@ -189,6 +180,9 @@ filter_valid_compiler_warnings(${WARN_LEVEL} -Wextra -Wno-long-long
-Wfloat-equal -Wshadow -Winit-self -Wswitch-default
-Wmissing-include-dirs -pedantic)
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}${WARNING_CXX_FLAGS}")
+if (DEFINED EXTRA_CMAKE_CXX_FLAGS)
+ set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${EXTRA_CMAKE_CXX_FLAGS}")
+endif()
#################################################
# OS Specific initialization
@@ -233,12 +227,16 @@ else (build_errors)
########################################
# Write the config.h file
- configure_file ("${project_cmake_dir}/config.hh.in"
- "${PROJECT_BINARY_DIR}/include/ignition/${IGN_PROJECT_NAME}/config.hh")
+ configure_file (${project_cmake_dir}/config.hh.in
+ ${PROJECT_BINARY_DIR}/include/ignition/${IGN_PROJECT_NAME}/config.hh)
ign_install_includes(
- "${INCLUDE_INSTALL_DIR_POSTFIX}/ignition/${IGN_PROJECT_NAME}"
- "${PROJECT_BINARY_DIR}/include/ignition/${IGN_PROJECT_NAME}/config.hh")
+ ${IGN_PROJECT_NAME}${PROJECT_MAJOR_VERSION}/ignition/${IGN_PROJECT_NAME}
+ ${PROJECT_BINARY_DIR}/include/ignition/${IGN_PROJECT_NAME}/config.hh)
+ include_directories(
+ ${PROJECT_SOURCE_DIR}/include
+ ${PROJECT_BINARY_DIR}/include
+ )
link_directories(${PROJECT_BINARY_DIR}/src)
if (DEFINED CMAKE_CXX_FLAGS)
@@ -248,7 +246,7 @@ else (build_errors)
endif()
message (STATUS "Build Type: ${CMAKE_BUILD_TYPE}")
message (STATUS "Install path: ${CMAKE_INSTALL_PREFIX}")
-
+
if (BUILD_IGNITION)
set(TEST_TYPE "UNIT")
@@ -259,7 +257,7 @@ else (build_errors)
########################################
# Make the package config files
- set (pkgconfig_files ${PROJECT_NAME_NO_VERSION_LOWER})
+ set (pkgconfig_files ${PROJECT_NAME_LOWER})
foreach (pkgconfig ${pkgconfig_files})
configure_file(${CMAKE_SOURCE_DIR}/cmake/pkgconfig/${pkgconfig}.in
@@ -270,53 +268,31 @@ else (build_errors)
########################################
# Make the cmake config files
- set(PKG_NAME ${PROJECT_NAME_NO_VERSION_UPPER})
- set(PKG_LIBRARIES ${PROJECT_NAME_LOWER})
- set(cmake_conf_file "${PROJECT_NAME_LOWER}-config.cmake")
- set(cmake_conf_version_file "${PROJECT_NAME_LOWER}-config-version.cmake")
- set(cmake_targets_file "${PROJECT_NAME_LOWER}-targets.cmake")
-
+ set(PKG_NAME ${PROJECT_NAME_UPPER})
+ set(PKG_LIBRARIES ${PROJECT_NAME_LOWER}${PROJECT_MAJOR_VERSION})
+ set(cmake_conf_file "${PROJECT_NAME_LOWER}${PROJECT_MAJOR_VERSION}-config.cmake")
+ set(cmake_conf_version_file "${PROJECT_NAME_LOWER}${PROJECT_MAJOR_VERSION}-config-version.cmake")
+
set(PKG_DEPENDS)
- if(WIN32 AND NOT CYGWIN)
- set(CMAKE_CONFIG_INSTALL_DIR CMake)
- else()
- set(CMAKE_CONFIG_INSTALL_DIR ${LIB_INSTALL_DIR}/cmake/${PROJECT_NAME_LOWER}/)
- endif()
-
- configure_package_config_file(
- "${CMAKE_CURRENT_SOURCE_DIR}/cmake/${PROJECT_NAME_NO_VERSION_LOWER}-config.cmake.in"
- "${CMAKE_CURRENT_BINARY_DIR}/${cmake_conf_file}"
- PATH_VARS LIB_INSTALL_DIR INCLUDE_INSTALL_DIR_FULL
- INSTALL_DESTINATION ${CMAKE_CONFIG_INSTALL_DIR})
+ configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/cmake/${PROJECT_NAME_LOWER}-config.cmake.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/${cmake_conf_file}" @ONLY)
# Use write_basic_package_version_file to generate a ConfigVersion file that
# allow users of gazebo to specify the API or version to depend on
# TODO: keep this instruction until deprecate Ubuntu/Precise and update with
- # https://github.com/Kitware/CMake/blob/v2.8.8/Modules/CMakePackageConfigHelpers.cmake
+ # https://github.com/Kitware/CMake/blob/v2.8.8/Modules/CMakePackageConfigHelpers.cmake
include(WriteBasicConfigVersionFile)
write_basic_config_version_file(
${CMAKE_CURRENT_BINARY_DIR}/${cmake_conf_version_file}
VERSION "${PROJECT_VERSION_FULL}"
COMPATIBILITY SameMajorVersion)
- install(FILES
+ install(FILES
${CMAKE_CURRENT_BINARY_DIR}/${cmake_conf_file}
${CMAKE_CURRENT_BINARY_DIR}/${cmake_conf_version_file}
- DESTINATION ${CMAKE_CONFIG_INSTALL_DIR} COMPONENT cmake)
-
- # Create *-targets.cmake file for build directory
- # TODO: keep this instruction until we depend on CMake 3
- # then change the first line from TARGETS ...
- # to EXPORT ... to match the format used in install(EXPORT
- export(TARGETS ${PROJECT_LIBRARY_TARGET_NAME}
- FILE ${CMAKE_BINARY_DIR}/${cmake_targets_file})
-
- # Install *-targets.cmake file
- install(EXPORT ${PROJECT_EXPORT_NAME}
- DESTINATION ${CMAKE_CONFIG_INSTALL_DIR}
- FILE ${cmake_targets_file})
-
+ DESTINATION ${LIB_INSTALL_DIR}/cmake/${PROJECT_NAME_LOWER}${PROJECT_MAJOR_VERSION}/ COMPONENT cmake)
########################################
# If present, load platform-specific build hooks. This system is used,
@@ -328,5 +304,5 @@ else (build_errors)
add_subdirectory(cmake/packager-hooks)
endif()
- message(STATUS "Configuration successful.")
+ message(STATUS "Configuration successful. Type make to compile ${PROJECT_NAME_LOWER}")
endif(build_errors)
diff --git a/Changelog.md b/Changelog.md
index d5678cf..8dcc5d0 100644
--- a/Changelog.md
+++ b/Changelog.md
@@ -1,4 +1,22 @@
-## Igniton Math 2.x
+## Ignition Math 2.x
+
+### Ignition Math 2.4
+
+#### Ignition Math 2.4.1
+
+1. Combine inertial properties of different objects, returning the equivalent
+ inertial properties as if the objects were welded together.
+ * [Pull request 115](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/115)
+
+#### Ignition Math 2.4.0
+
+1. New MassMatrix3 class
+ * [Pull request 112](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/112)
+1. MassMatrix3 helper functions
+ * [Pull request 110](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/110)
+1. Added Temperature class
+ * A contribution from Shintaro Noda
+ * [Pull request 104](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/104)
### Ignition Math 2.3.0
diff --git a/appveyor.yml b/appveyor.yml
new file mode 100644
index 0000000..9057014
--- /dev/null
+++ b/appveyor.yml
@@ -0,0 +1,22 @@
+os:
+ - Visual Studio 2013
+ - Visual Studio 2015
+
+configuration:
+ - Debug
+ - Release
+
+environment:
+ CTEST_OUTPUT_ON_FAILURE: 1
+
+build_script:
+ - md build
+ - cd build
+ - cmake .. -DEXTRA_CMAKE_CXX_FLAGS="-WX"
+ - cmake --build . --config %CONFIGURATION%
+
+test_script:
+ - cmake --build . --config %CONFIGURATION% --target RUN_TESTS
+
+after_build:
+ - cmake --build . --config %CONFIGURATION% --target INSTALL
diff --git a/cmake/DefaultCFlags.cmake b/cmake/DefaultCFlags.cmake
index a2717c8..247a56a 100644
--- a/cmake/DefaultCFlags.cmake
+++ b/cmake/DefaultCFlags.cmake
@@ -30,7 +30,7 @@ set (CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_C_FLAGS_COVERAGE} -fno-elide-constructors
# Set all the global build flags
if (UNIX)
set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}")
- set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}")
+ set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}} -std=c++11")
set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}")
set (CMAKE_SHARED_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}")
set (CMAKE_MODULE_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}")
diff --git a/cmake/TestUtils.cmake b/cmake/TestUtils.cmake
index 231a3dd..0fbb0b9 100644
--- a/cmake/TestUtils.cmake
+++ b/cmake/TestUtils.cmake
@@ -28,7 +28,7 @@ macro (ign_build_tests)
target_link_libraries(${BINARY_NAME}
gtest.lib
gtest_main.lib
- ignition-math${PROJECT_MAJOR_VERSION})
+ ignition-math${PROJECT_MAJOR_VERSION}.lib)
else()
message(FATAL_ERROR "Unsupported platform")
endif()
diff --git a/cmake/Utils.cmake b/cmake/Utils.cmake
index 74e16bf..4bca480 100644
--- a/cmake/Utils.cmake
+++ b/cmake/Utils.cmake
@@ -83,13 +83,13 @@ macro (ign_install_includes _subdir)
endmacro()
#################################################
-macro (ign_install_library _name _exportName)
+macro (ign_install_library _name)
set_target_properties(${_name} PROPERTIES SOVERSION ${PROJECT_MAJOR_VERSION} VERSION ${PROJECT_VERSION_FULL})
- install (TARGETS ${_name} EXPORT ${_exportName} DESTINATION ${LIB_INSTALL_DIR} COMPONENT shlib)
+ install (TARGETS ${_name} DESTINATION ${LIB_INSTALL_DIR} COMPONENT shlib)
endmacro ()
#################################################
-macro (ign_install_executable _name )
+macro (ign_install_executable _name)
set_target_properties(${_name} PROPERTIES VERSION ${PROJECT_VERSION_FULL})
install (TARGETS ${_name} DESTINATION ${BIN_INSTALL_DIR})
manpage(${_name} 1)
@@ -98,7 +98,7 @@ endmacro ()
#################################################
macro (ign_setup_unix)
# USE_HOST_CFLAGS (default TRUE)
- # Will check building host machine for proper cflags
+ # Will check building host machine for proper cflags
if(NOT DEFINED USE_HOST_CFLAGS OR USE_HOST_CFLAGS)
message(STATUS "Enable host CFlags")
include (${project_cmake_dir}/HostCFlags.cmake)
@@ -114,7 +114,9 @@ endmacro()
#################################################
macro (ign_setup_windows)
- add_definitions("/EHsc")
+ if(MSVC)
+ add_definitions("/EHsc")
+ endif()
endmacro()
#################################################
@@ -153,7 +155,7 @@ if (NOT DEFINED ENABLE_TESTS_COMPILATION)
set (ENABLE_TESTS_COMPILATION True)
endif()
-# Define testing macros as empty and redefine them if support is found and
+# Define testing macros as empty and redefine them if support is found and
# ENABLE_TESTS_COMPILATION is set to true
macro (ign_build_tests)
endmacro()
@@ -164,10 +166,10 @@ endif()
#################################################
# Macro to setup supported compiler warnings
-# Based on work of Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST.
+# Based on work of Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST.
include(CheckCXXCompilerFlag)
-macro(filter_valid_compiler_warnings)
+macro(filter_valid_compiler_warnings)
foreach(flag ${ARGN})
CHECK_CXX_COMPILER_FLAG(${flag} R${flag})
if(${R${flag}})
diff --git a/cmake/config.hh.in b/cmake/config.hh.in
index b5956a8..1789eef 100644
--- a/cmake/config.hh.in
+++ b/cmake/config.hh.in
@@ -1,4 +1,4 @@
-/* Config.hh. Generated by CMake for @PROJECT_NAME_NO_VERSION at . */
+/* Config.hh. Generated by CMake for @PROJECT_NAME at . */
/* Version number */
#define IGNITION_MATH_MAJOR_VERSION ${PROJECT_MAJOR_VERSION}
diff --git a/cmake/cpack_options.cmake.in b/cmake/cpack_options.cmake.in
index 7aaddbd..90fe4dd 100644
--- a/cmake/cpack_options.cmake.in
+++ b/cmake/cpack_options.cmake.in
@@ -1,8 +1,8 @@
-set(CPACK_PACKAGE_NAME "@PROJECT_NAME_NO_VERSION@")
+set(CPACK_PACKAGE_NAME "@PROJECT_NAME@")
set(CPACK_PACKAGE_VENDOR "osrfoundation.org")
set(CPACK_PACKAGE_DESCRIPTION_SUMMARY
"A set of @IGN_PROJECT_NAME@ classes for robot applications.")
-set(CPACK_PACKAGE_INSTALL_DIRECTORY "@PROJECT_NAME_NO_VERSION_LOWER@")
+set(CPACK_PACKAGE_INSTALL_DIRECTORY "@PROJECT_NAME_LOWER@")
set(CPACK_RESOURCE_FILE_LICENSE "@CMAKE_CURRENT_SOURCE_DIR@/LICENSE")
set(CPACK_RESOURCE_FILE_README "@CMAKE_CURRENT_SOURCE_DIR@/README.md")
set(CPACK_PACKAGE_DESCRIPTION_FILE "@CMAKE_CURRENT_SOURCE_DIR@/README.md")
@@ -23,6 +23,6 @@ set(CPACK_RPM_PACKAGE_DESCRIPTION
"A set of @IGN_PROJECT_NAME@ classes for robot applications.")
set (CPACK_PACKAGE_FILE_NAME
- "@PROJECT_NAME_LOWER at -@PROJECT_VERSION_FULL@")
+ "@PROJECT_NAME_LOWER@@PROJECT_MAJOR_VERSION at -@PROJECT_VERSION_FULL@")
set (CPACK_SOURCE_PACKAGE_FILE_NAME
- "@PROJECT_NAME_LOWER at -@PROJECT_VERSION_FULL@")
+ "@PROJECT_NAME_LOWER@@PROJECT_MAJOR_VERSION at -@PROJECT_VERSION_FULL@")
diff --git a/cmake/ignition-math-config.cmake.in b/cmake/ignition-math-config.cmake.in
index 71d5652..bd7f32f 100644
--- a/cmake/ignition-math-config.cmake.in
+++ b/cmake/ignition-math-config.cmake.in
@@ -3,14 +3,39 @@ if (@PKG_NAME at _CONFIG_INCLUDED)
endif()
set(@PKG_NAME at _CONFIG_INCLUDED TRUE)
- at PACKAGE_INIT@
+list(APPEND @PKG_NAME at _INCLUDE_DIRS "@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_INCLUDEDIR@/ignition/@IGN_PROJECT_NAME@@PROJECT_MAJOR_VERSION@")
-if(NOT TARGET @PROJECT_LIBRARY_TARGET_NAME@)
- include("${CMAKE_CURRENT_LIST_DIR}/@cmake_targets_file@")
+list(APPEND @PKG_NAME at _LIBRARY_DIRS "@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@")
+
+list(APPEND @PKG_NAME at _CXX_FLAGS -std=c++11)
+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()
-# Compatibility
-set(@PKG_NAME at _LIBRARIES @PROJECT_LIBRARY_TARGET_NAME@)
-set(@PKG_NAME at _INCLUDE_DIRS "@PACKAGE_INCLUDE_INSTALL_DIR_FULL@")
-set(@PKG_NAME at _LIBRARY_DIRS "@PACKAGE_LIB_INSTALL_DIR@")
-set(@PKG_NAME at _LDFLAGS "-L at PACKAGE_LIB_INSTALL_DIR@")
\ No newline at end of file
+foreach(lib @PKG_LIBRARIES@)
+ set(onelib "${lib}-NOTFOUND")
+ find_library(onelib ${lib}
+ PATHS "@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@"
+ NO_DEFAULT_PATH
+ )
+ if(NOT onelib)
+ message(FATAL_ERROR "Library '${lib}' in package @PKG_NAME@ is not installed properly")
+ endif()
+ list(APPEND @PKG_NAME at _LIBRARIES ${onelib})
+endforeach()
+
+foreach(dep @PKG_DEPENDS@)
+ if(NOT ${dep}_FOUND)
+ find_package(${dep} REQUIRED)
+ endif()
+ list(APPEND @PKG_NAME at _INCLUDE_DIRS "${${dep}_INCLUDE_DIRS}")
+ list(APPEND @PKG_NAME at _LIBRARIES "${${dep_lib}_LIBRARIES}")
+endforeach()
+
+list(APPEND @PKG_NAME at _LDFLAGS "-L at CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@")
diff --git a/configure.bat b/configure.bat
index 8e2ffc8..cd99e18 100644
--- a/configure.bat
+++ b/configure.bat
@@ -1,5 +1,5 @@
- at set build_type=Debug
+ at set build_type=Release
@if not "%1"=="" set build_type=%1
@echo Configuring for build type %build_type%
-cmake -G "NMake Makefiles" -DCMAKE_INSTALL_PREFIX="install\%build_type%" -DCMAKE_BUILD_TYPE="%build_type%" -DENABLE_TESTS_COMPILATION:BOOL=False ..
+cmake -G "NMake Makefiles" -DCMAKE_INSTALL_PREFIX="install\%build_type%" -DCMAKE_BUILD_TYPE="%build_type%" -DENABLE_TESTS_COMPILATION:BOOL=True ..
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index 01db4b1..012cb32 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -1,10 +1,10 @@
-cmake_minimum_required(VERSION 2.8.11 FATAL_ERROR)
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
# Find the Ignition-Math library
-find_package(ignition-math2 REQUIRED)
+find_package(ignition-math QUIET REQUIRED)
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${IGNITION-MATH_CXX_FLAGS}")
+include_directories(${IGNITION-MATH_INCLUDE_DIRS})
+link_directories(${IGNITION-MATH_LIBRARY_DIRS})
add_executable(vector2_example vector2_example.cc)
-target_link_libraries(vector2_example ${IGNITION-MATH_LIBRARIES})
-
add_executable(triangle_example triangle_example.cc)
-target_link_libraries(triangle_example ${IGNITION-MATH_LIBRARIES})
diff --git a/include/ignition/math/AffineException.hh b/include/ignition/math/AffineException.hh
index 92e947b..c678737 100644
--- a/include/ignition/math/AffineException.hh
+++ b/include/ignition/math/AffineException.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_AFFINE_EXCEPTION_HH_
-#define _IGNITION_AFFINE_EXCEPTION_HH_
+#ifndef IGNITION_MATH_AFFINE_EXCEPTION_HH_
+#define IGNITION_MATH_AFFINE_EXCEPTION_HH_
#include <stdexcept>
#include <ignition/math/Helpers.hh>
diff --git a/include/ignition/math/Angle.hh b/include/ignition/math/Angle.hh
index da1dcb1..196cdbb 100644
--- a/include/ignition/math/Angle.hh
+++ b/include/ignition/math/Angle.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_ANGLE_HH_
-#define _IGNITION_ANGLE_HH_
+#ifndef IGNITION_MATH_ANGLE_HH_
+#define IGNITION_MATH_ANGLE_HH_
#include <iostream>
#include <ignition/math/Helpers.hh>
diff --git a/include/ignition/math/Box.hh b/include/ignition/math/Box.hh
index e8125c0..5173e9c 100644
--- a/include/ignition/math/Box.hh
+++ b/include/ignition/math/Box.hh
@@ -14,12 +14,14 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_BOX_HH_
-#define _IGNITION_BOX_HH_
+#ifndef IGNITION_MATH_BOX_HH_
+#define IGNITION_MATH_BOX_HH_
#include <iostream>
+#include <tuple>
#include <ignition/math/Helpers.hh>
#include <ignition/math/Vector3.hh>
+#include <ignition/math/Line3.hh>
namespace ignition
{
@@ -152,6 +154,75 @@ namespace ignition
/// \return True if the point is inside the box.
public: bool Contains(const Vector3d &_p) const;
+ /// \brief Check if a ray (origin, direction) intersects the box.
+ /// \param[in] _origin Origin of the ray.
+ /// \param[in] _dir Direction of the ray. This ray will be normalized.
+ /// \param[in] _min Minimum allowed distance.
+ /// \param[in] _max Maximum allowed distance.
+ /// \return A boolean
+ public: bool IntersectCheck(const Vector3d &_origin, const Vector3d &_dir,
+ const double _min, const double _max) const;
+
+ /// \brief Check if a ray (origin, direction) intersects the box.
+ /// \param[in] _origin Origin of the ray.
+ /// \param[in] _dir Direction of the ray. This ray will be normalized.
+ /// \param[in] _min Minimum allowed distance.
+ /// \param[in] _max Maximum allowed distance.
+ /// \return A boolean and double tuple. The boolean value is true
+ /// if the line intersects the box.
+ ///
+ /// The double is the distance from
+ /// the ray's start to the closest intersection point on the box,
+ /// minus the _min distance. For example, if _min == 0.5 and the
+ /// intersection happens at a distance of 2.0 from _origin then returned
+ /// distance is 1.5.
+ ///
+ /// The double value is zero when the boolean value is false.
+ public: std::tuple<bool, double> IntersectDist(
+ const Vector3d &_origin, const Vector3d &_dir,
+ const double _min, const double _max) const;
+
+ /// \brief Check if a ray (origin, direction) intersects the box.
+ /// \param[in] _origin Origin of the ray.
+ /// \param[in] _dir Direction of the ray. This ray will be normalized.
+ /// \param[in] _min Minimum allowed distance.
+ /// \param[in] _max Maximum allowed distance.
+ /// \return A boolean, double, Vector3d tuple. The boolean value is true
+ /// if the line intersects the box.
+ ///
+ /// The double is the distance from the ray's start to the closest
+ /// intersection point on the box,
+ /// minus the _min distance. For example, if _min == 0.5 and the
+ /// intersection happens at a distance of 2.0 from _origin then returned
+ /// distance is 1.5.
+ /// The double value is zero when the boolean value is false. The
+ ///
+ /// Vector3d is the intersection point on the box. The Vector3d value
+ /// is zero if the boolean value is false.
+ public: std::tuple<bool, double, Vector3d> Intersect(
+ const Vector3d &_origin, const Vector3d &_dir,
+ const double _min, const double _max) const;
+
+ /// \brief Check if a line intersects the box.
+ /// \param[in] _line The line to check against this box.
+ /// \return A boolean, double, Vector3d tuple. The boolean value is true
+ /// if the line intersects the box. The double is the distance from
+ /// the line's start to the closest intersection point on the box.
+ /// The double value is zero when the boolean value is false. The
+ /// Vector3d is the intersection point on the box. The Vector3d value
+ /// is zero if the boolean value is false.
+ public: std::tuple<bool, double, Vector3d> Intersect(
+ const Line3d &_line) const;
+
+ /// \brief Clip a line to a dimension of the box.
+ /// This is a helper function to Intersects
+ /// \param[in] _d Dimension of the box(0, 1, or 2).
+ /// \param[in] _line Line to clip
+ /// \param[in,out] _low Close distance
+ /// \param[in,out] _high Far distance
+ private: bool ClipLine(const int _d, const Line3d &_line,
+ double &_low, double &_high) const;
+
/// \brief Private data pointer
private: BoxPrivate *dataPtr;
};
diff --git a/include/ignition/math/BoxPrivate.hh b/include/ignition/math/BoxPrivate.hh
index 4af61fb..26d2d4d 100644
--- a/include/ignition/math/BoxPrivate.hh
+++ b/include/ignition/math/BoxPrivate.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_BOX_PRIVATE_HH_
-#define _IGNITION_BOX_PRIVATE_HH_
+#ifndef IGNITION_MATH_BOX_PRIVATE_HH_
+#define IGNITION_MATH_BOX_PRIVATE_HH_
#include <ignition/math/Vector3.hh>
@@ -27,21 +27,18 @@ namespace ignition
/// \brief Private data for Box class
class BoxPrivate
{
- /// \brief Constructor
- public: BoxPrivate();
-
/// \brief Enumeration of extents
public: enum Extent {EXTENT_NULL, EXTENT_FINITE};
/// \brief Minimum corner of the box
- public: Vector3d min;
+ public: Vector3d min = Vector3d::Zero;
/// \brief Maximum corner of the box
- public: Vector3d max;
+ public: Vector3d max = Vector3d::Zero;
- /// \brief When set to EXTENT_NULL (in the default constructor)
+ /// \brief When set to EXTENT_NULL (the default value)
/// the min and max are not valid positions
- public: Extent extent;
+ public: Extent extent = EXTENT_NULL;
};
}
}
diff --git a/include/ignition/math/CMakeLists.txt b/include/ignition/math/CMakeLists.txt
index e5ef371..c64f77d 100644
--- a/include/ignition/math/CMakeLists.txt
+++ b/include/ignition/math/CMakeLists.txt
@@ -8,6 +8,7 @@ set (headers
Frustum.hh
Helpers.hh
IndexException.hh
+ Inertial.hh
Kmeans.hh
Line2.hh
Line3.hh
@@ -21,10 +22,11 @@ set (headers
RotationSpline.hh
SignalStats.hh
Spline.hh
+ Temperature.hh
Triangle.hh
- Triangle3.hh
Vector2.hh
Vector3.hh
+ Vector3Stats.hh
Vector4.hh
)
@@ -37,5 +39,5 @@ endforeach()
configure_file (${CMAKE_CURRENT_SOURCE_DIR}/math.hh.in
${CMAKE_CURRENT_BINARY_DIR}/math.hh)
-ign_install_includes("${INCLUDE_INSTALL_DIR_POSTFIX}/ignition" ${CMAKE_CURRENT_BINARY_DIR}/math.hh)
-ign_install_includes("${INCLUDE_INSTALL_DIR_POSTFIX}/ignition/${IGN_PROJECT_NAME}" ${headers})
+ign_install_includes("${IGN_PROJECT_NAME}${PROJECT_MAJOR_VERSION}/ignition" ${CMAKE_CURRENT_BINARY_DIR}/math.hh)
+ign_install_includes("${IGN_PROJECT_NAME}${PROJECT_MAJOR_VERSION}/ignition/${IGN_PROJECT_NAME}" ${headers})
diff --git a/include/ignition/math/Filter.hh b/include/ignition/math/Filter.hh
index bea8f29..9cb4073 100644
--- a/include/ignition/math/Filter.hh
+++ b/include/ignition/math/Filter.hh
@@ -15,8 +15,8 @@
*
*/
-#ifndef _IGNITION_FILTER_HH_
-#define _IGNITION_FILTER_HH_
+#ifndef IGNITION_MATH_FILTER_HH_
+#define IGNITION_MATH_FILTER_HH_
#include <ignition/math/Helpers.hh>
#include <ignition/math/Vector3.hh>
diff --git a/include/ignition/math/Frustum.hh b/include/ignition/math/Frustum.hh
index 307bfa5..bb8b3ea 100644
--- a/include/ignition/math/Frustum.hh
+++ b/include/ignition/math/Frustum.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_FRUSTUM_HH_
-#define _IGNITION_FRUSTUM_HH_
+#ifndef IGNITION_MATH_FRUSTUM_HH_
+#define IGNITION_MATH_FRUSTUM_HH_
#include <ignition/math/Plane.hh>
#include <ignition/math/Angle.hh>
diff --git a/include/ignition/math/FrustumPrivate.hh b/include/ignition/math/FrustumPrivate.hh
index 59a1b77..1d5e020 100644
--- a/include/ignition/math/FrustumPrivate.hh
+++ b/include/ignition/math/FrustumPrivate.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_FRUSTUM_PRIVATE_HH_
-#define _IGNITION_FRUSTUM_PRIVATE_HH_
+#ifndef IGNITION_MATH_FRUSTUM_PRIVATE_HH_
+#define IGNITION_MATH_FRUSTUM_PRIVATE_HH_
#include <array>
diff --git a/include/ignition/math/Helpers.hh b/include/ignition/math/Helpers.hh
index 9705cc7..ddfe23d 100644
--- a/include/ignition/math/Helpers.hh
+++ b/include/ignition/math/Helpers.hh
@@ -14,12 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_MATH_FUNCTIONS_HH_
-#define _IGNITION_MATH_FUNCTIONS_HH_
-
-#ifndef _USE_MATH_DEFINES
-# define _USE_MATH_DEFINES
-#endif
+#ifndef IGNITION_MATH_FUNCTIONS_HH_
+#define IGNITION_MATH_FUNCTIONS_HH_
#include <cmath>
#include <algorithm>
@@ -27,6 +23,8 @@
#include <string>
#include <iostream>
#include <vector>
+#include <tuple>
+#include <cstdint>
/// \brief Double maximum value. This value will be similar to 1.79769e+308
#define IGN_DBL_MAX std::numeric_limits<double>::max()
@@ -49,6 +47,35 @@
/// \brief Float lowest value, equivalent to -IGN_FLT_MAX
#define IGN_FLT_LOW std::numeric_limits<float>::lowest()
+/// \brief Float positive infinite value
+#define IGN_FLT_INF std::numeric_limits<float>::infinity()
+
+/// \brief 16bit unsigned integer maximum value
+#define IGN_UINT16_MAX std::numeric_limits<uint16_t>::max()
+
+/// \brief 16bit unsigned integer minimum value
+#define IGN_UINT16_MIN std::numeric_limits<uint16_t>::min()
+
+/// \brief 16bit unsigned integer lowest value. This is equivalent to
+/// IGN_UINT16_MIN, and is defined here for completeness.
+#define IGN_UINT16_LOW std::numeric_limits<uint16_t>::lowest()
+
+/// \brief 16-bit unsigned integer positive infinite value
+#define IGN_UINT16_INF std::numeric_limits<uint16_t>::infinity()
+
+/// \brief 16bit integer maximum value
+#define IGN_INT16_MAX std::numeric_limits<int16_t>::max()
+
+/// \brief 16bit integer minimum value
+#define IGN_INT16_MIN std::numeric_limits<int16_t>::min()
+
+/// \brief 16bit integer lowest value. This is equivalent to IGN_INT16_MIN,
+/// and is defined here for completeness.
+#define IGN_INT16_LOW std::numeric_limits<int16_t>::lowest()
+
+/// \brief 16-bit integer positive infinite value
+#define IGN_INT16_INF std::numeric_limits<int16_t>::infinity()
+
/// \brief 32bit unsigned integer maximum value
#define IGN_UINT32_MAX std::numeric_limits<uint32_t>::max()
@@ -59,6 +86,9 @@
/// IGN_UINT32_MIN, and is defined here for completeness.
#define IGN_UINT32_LOW std::numeric_limits<uint32_t>::lowest()
+/// \brief 32-bit unsigned integer positive infinite value
+#define IGN_UINT32_INF std::numeric_limits<uint32_t>::infinity()
+
/// \brief 32bit integer maximum value
#define IGN_INT32_MAX std::numeric_limits<int32_t>::max()
@@ -69,25 +99,47 @@
/// and is defined here for completeness.
#define IGN_INT32_LOW std::numeric_limits<int32_t>::lowest()
+/// \brief 32-bit integer positive infinite value
+#define IGN_INT32_INF std::numeric_limits<int32_t>::infinity()
+
+/// \brief 64bit unsigned integer maximum value
+#define IGN_UINT64_MAX std::numeric_limits<uint64_t>::max()
+
+/// \brief 64bit unsigned integer minimum value
+#define IGN_UINT64_MIN std::numeric_limits<uint64_t>::min()
+
+/// \brief 64bit unsigned integer lowest value. This is equivalent to
+/// IGN_UINT64_MIN, and is defined here for completeness.
+#define IGN_UINT64_LOW std::numeric_limits<uint64_t>::lowest()
+
+/// \brief 64-bit unsigned integer positive infinite value
+#define IGN_UINT64_INF std::numeric_limits<uint64_t>::infinity()
+
+/// \brief 64bit integer maximum value
+#define IGN_INT64_MAX std::numeric_limits<int64_t>::max()
+
+/// \brief 64bit integer minimum value
+#define IGN_INT64_MIN std::numeric_limits<int64_t>::min()
+
+/// \brief 64bit integer lowest value. This is equivalent to IGN_INT64_MIN,
+/// and is defined here for completeness.
+#define IGN_INT64_LOW std::numeric_limits<int64_t>::lowest()
+
+/// \brief 64-bit integer positive infinite value
+#define IGN_INT64_INF std::numeric_limits<int64_t>::infinity()
+
/// \brief Define IGN_PI, IGN_PI_2, and IGN_PI_4.
/// This was put here for Windows support.
#ifdef M_PI
#define IGN_PI M_PI
#define IGN_PI_2 M_PI_2
#define IGN_PI_4 M_PI_4
+#define IGN_SQRT2 M_SQRT2
#else
#define IGN_PI 3.14159265358979323846
#define IGN_PI_2 1.57079632679489661923
#define IGN_PI_4 0.78539816339744830962
-#endif
-
-/// \brief Define IGN_FP_VOLATILE for FP equality comparisons
-/// Use volatile parameters when checking floating point equality on
-/// the 387 math coprocessor to work around bugs from the 387 extra precision
-#if defined __FLT_EVAL_METHOD__ && __FLT_EVAL_METHOD__ == 2
-#define IGN_FP_VOLATILE volatile
-#else
-#define IGN_FP_VOLATILE
+#define IGN_SQRT2 1.41421356237309504880
#endif
/// \brief Compute sphere volume
@@ -109,6 +161,39 @@
/// \param[in] _v Vector3d that contains the box's dimensions.
#define IGN_BOX_VOLUME_V(_v) (_v.X() *_v.Y() * _v.Z())
+/** \def IGNITION_VISIBLE
+ * Use to represent "symbol visible" if supported
+ */
+
+/** \def IGNITION_HIDDEN
+ * Use to represent "symbol hidden" if supported
+ */
+
+#if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL
+ #ifdef __GNUC__
+ #define IGNITION_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define IGNITION_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define IGNITION_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define IGNITION_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define IGNITION_HIDDEN
+#else
+ #if __GNUC__ >= 4
+ #define IGNITION_VISIBLE __attribute__ ((visibility ("default")))
+ #define IGNITION_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define IGNITION_VISIBLE
+ #define IGNITION_HIDDEN
+ #endif
+#endif
+
namespace ignition
{
/// \brief Math classes and function useful in robot applications.
@@ -257,8 +342,7 @@ namespace ignition
inline bool equal(const T &_a, const T &_b,
const T &_epsilon = 1e-6)
{
- IGN_FP_VOLATILE T diff = std::abs(_a - _b);
- return diff <= _epsilon;
+ return std::abs(_a - _b) <= _epsilon;
}
/// \brief get value at a specified precision
@@ -268,8 +352,8 @@ namespace ignition
template<typename T>
inline T precision(const T &_a, const unsigned int &_precision)
{
- return std::round(_a * std::pow(10, _precision))
- / std::pow(10, _precision);
+ auto p = std::pow(10, _precision);
+ return static_cast<T>(std::round(_a * p) / p);
}
/// \brief Sort two numbers, such that _a <= _b
@@ -346,7 +430,7 @@ namespace ignition
p++;
}
- double acc = 0;
+ int acc = 0;
while (*p >= '0' && *p <= '9')
acc = acc * 10 + *p++ - '0';
@@ -356,7 +440,7 @@ namespace ignition
return NAN_I;
}
- return static_cast<int>(s * acc);
+ return s * acc;
}
/// \brief parse string into float
@@ -420,40 +504,43 @@ namespace ignition
}
return s * acc;
}
- }
-}
-
-/** \def IGNITION_VISIBLE
- * Use to represent "symbol visible" if supported
- */
-/** \def IGNITION_HIDDEN
- * Use to represent "symbol hidden" if supported
- */
-#if defined _WIN32 || defined __CYGWIN__
- #ifdef BUILDING_DLL
- #ifdef __GNUC__
- #define IGNITION_VISIBLE __attribute__ ((dllexport))
- #else
- #define IGNITION_VISIBLE __declspec(dllexport)
- #endif
- #else
- #ifdef __GNUC__
- #define IGNITION_VISIBLE __attribute__ ((dllimport))
- #else
- #define IGNITION_VISIBLE __declspec(dllimport)
- #endif
- #endif
- #define IGNITION_HIDDEN
+ // Degrade precision on Windows, which cannot handle 'long double'
+ // values properly. See the implementation of Unpair.
+#ifdef _MSC_VER
+ using PairInput = uint16_t;
+ using PairOutput = uint32_t;
#else
- #if __GNUC__ >= 4
- #define IGNITION_VISIBLE __attribute__ ((visibility ("default")))
- #define IGNITION_HIDDEN __attribute__ ((visibility ("hidden")))
- #else
- #define IGNITION_VISIBLE
- #define IGNITION_HIDDEN
- #endif
+ using PairInput = uint32_t;
+ using PairOutput = uint64_t;
#endif
+ /// \brief A pairing function that maps two values to a unique third
+ /// value. This is an implement of Szudzik's function.
+ /// \param[in] _a First value, must be a non-negative integer. On
+ /// Windows this value is uint16_t. On Linux/OSX this value is uint32_t.
+ /// \param[in] _b Second value, must be a non-negative integer. On
+ /// Windows this value is uint16_t. On Linux/OSX this value is uint32_t.
+ /// \return A unique non-negative integer value. On Windows the return
+ /// value is uint32_t. On Linux/OSX the return value is uint64_t
+ /// \sa Unpair
+ PairOutput IGNITION_VISIBLE Pair(const PairInput _a, const PairInput _b);
+
+ /// \brief The reverse of the Pair function. Accepts a key, produced
+ /// from the Pair function, and returns a tuple consisting of the two
+ /// non-negative integer values used to create the _key.
+ /// \param[in] _key A non-negative integer generated from the Pair
+ /// function. On Windows this value is uint32_t. On Linux/OSX, this
+ /// value is uint64_t.
+ /// \return A tuple that consists of the two non-negative integers that
+ /// will generate _key when used with the Pair function. On Windows the
+ /// tuple contains two uint16_t values. On Linux/OSX the tuple contains
+ /// two uint32_t values.
+ /// \sa Pair
+ std::tuple<PairInput, PairInput> IGNITION_VISIBLE Unpair(
+ const PairOutput _key);
+ }
+}
+
#endif
diff --git a/include/ignition/math/IndexException.hh b/include/ignition/math/IndexException.hh
index 341fed7..3049c26 100644
--- a/include/ignition/math/IndexException.hh
+++ b/include/ignition/math/IndexException.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_INDEX_EXCEPTION_HH_
-#define _IGNITION_INDEX_EXCEPTION_HH_
+#ifndef IGNITION_MATH_INDEX_EXCEPTION_HH_
+#define IGNITION_MATH_INDEX_EXCEPTION_HH_
#include <stdexcept>
#include <ignition/math/Helpers.hh>
diff --git a/include/ignition/math/Inertial.hh b/include/ignition/math/Inertial.hh
new file mode 100644
index 0000000..bd9f93a
--- /dev/null
+++ b/include/ignition/math/Inertial.hh
@@ -0,0 +1,206 @@
+/*
+ * Copyright (C) 2016 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_MATH_INERTIAL_HH_
+#define IGNITION_MATH_INERTIAL_HH_
+
+#include "ignition/math/MassMatrix3.hh"
+#include "ignition/math/Pose3.hh"
+
+namespace ignition
+{
+ namespace math
+ {
+ /// \class Inertial Inertial.hh ignition/math/Inertial.hh
+ /// \brief A class for inertial information about a rigid body
+ /// consisting of the scalar mass, a 3x3 symmetric moment
+ /// of inertia matrix, and center of mass reference frame pose.
+ template<typename T>
+ class Inertial
+ {
+ /// \brief Default Constructor
+ public: Inertial()
+ {}
+
+ /// \brief Constructor.
+ /// \param[in] _massMatrix Mass and inertia matrix.
+ /// \param[in] _pose Pose of center of mass reference frame.
+ public: Inertial(const MassMatrix3<T> &_massMatrix,
+ const Pose3<T> &_pose)
+ : massMatrix(_massMatrix), pose(_pose)
+ {}
+
+ /// \brief Copy constructor.
+ /// \param[in] _inertial Inertial element to copy
+ public: Inertial(const Inertial<T> &_inertial)
+ : massMatrix(_inertial.MassMatrix()), pose(_inertial.Pose())
+ {}
+
+ /// \brief Destructor.
+ public: virtual ~Inertial() {}
+
+ /// \brief Set the mass and inertia matrix.
+ /// \param[in] _m New MassMatrix3 object.
+ /// \return True if the MassMatrix3 is valid.
+ public: bool SetMassMatrix(const MassMatrix3<T> &_m)
+ {
+ this->massMatrix = _m;
+ return this->massMatrix.IsValid();
+ }
+
+ /// \brief Get the mass and inertia matrix.
+ /// \return The MassMatrix3 object.
+ public: const MassMatrix3<T> &MassMatrix() const
+ {
+ return this->massMatrix;
+ }
+
+ /// \brief Set the pose of center of mass reference frame.
+ /// \param[in] _pose New pose.
+ /// \return True if the MassMatrix3 is valid.
+ public: bool SetPose(const Pose3<T> &_pose)
+ {
+ this->pose = _pose;
+ return this->massMatrix.IsValid();
+ }
+
+ /// \brief Get the pose of center of mass reference frame.
+ /// \return The pose of center of mass reference frame.
+ public: const Pose3<T> &Pose() const
+ {
+ return this->pose;
+ }
+
+ /// \brief Get the moment of inertia matrix expressed in the
+ /// base coordinate frame.
+ /// \return Rotated moment of inertia matrix.
+ public: Matrix3<T> MOI() const
+ {
+ auto R = Matrix3<T>(this->pose.Rot());
+ return R * this->massMatrix.MOI() * R.Transposed();
+ }
+
+ /// \brief Equal operator.
+ /// \param[in] _inertial Inertial to copy.
+ /// \return Reference to this object.
+ public: Inertial &operator=(const Inertial<T> &_inertial)
+ {
+ this->massMatrix = _inertial.MassMatrix();
+ this->pose = _inertial.Pose();
+
+ return *this;
+ }
+
+ /// \brief Equality comparison operator.
+ /// \param[in] _inertial Inertial to copy.
+ /// \return true if each component is equal within a default tolerance,
+ /// false otherwise
+ public: bool operator==(const Inertial<T> &_inertial) const
+ {
+ return (this->pose == _inertial.Pose()) &&
+ (this->massMatrix == _inertial.MassMatrix());
+ }
+
+ /// \brief Inequality test operator
+ /// \param[in] _inertial Inertial<T> to test
+ /// \return True if not equal (using the default tolerance of 1e-6)
+ public: bool operator!=(const Inertial<T> &_inertial) const
+ {
+ return !(*this == _inertial);
+ }
+
+ /// \brief Adds inertial properties to current object.
+ /// The mass, center of mass location, and inertia matrix are updated
+ /// as long as the total mass is positive.
+ /// \param[in] _inertial Inertial to add.
+ /// \return Reference to this object.
+ public: Inertial<T> &operator+=(const Inertial<T> &_inertial)
+ {
+ T m1 = this->massMatrix.Mass();
+ T m2 = _inertial.MassMatrix().Mass();
+
+ // Total mass
+ T mass = m1 + m2;
+
+ // Only continue if total mass is positive
+ if (mass <= 0)
+ {
+ return *this;
+ }
+
+ auto com1 = this->Pose().Pos();
+ auto com2 = _inertial.Pose().Pos();
+ // New center of mass location in base frame
+ auto com = (m1*com1 + m2*com2) / mass;
+
+ // Components of new moment of inertia matrix
+ Vector3<T> ixxyyzz;
+ Vector3<T> ixyxzyz;
+ // First add matrices in base frame
+ {
+ auto moi = this->MOI() + _inertial.MOI();
+ ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
+ ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
+ }
+ // Then account for parallel axis theorem
+ {
+ auto dc = com1 - com;
+ ixxyyzz.X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
+ ixxyyzz.Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
+ ixxyyzz.Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
+ ixxyyzz.X() -= m1 * dc[0] * dc[1];
+ ixxyyzz.Y() -= m1 * dc[0] * dc[2];
+ ixxyyzz.Z() -= m1 * dc[1] * dc[2];
+ }
+ {
+ auto dc = com2 - com;
+ ixxyyzz.X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
+ ixxyyzz.Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
+ ixxyyzz.Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
+ ixxyyzz.X() -= m2 * dc[0] * dc[1];
+ ixxyyzz.Y() -= m2 * dc[0] * dc[2];
+ ixxyyzz.Z() -= m2 * dc[1] * dc[2];
+ }
+ this->massMatrix = MassMatrix3<T>(mass, ixxyyzz, ixyxzyz);
+ this->pose = Pose3<T>(com, Quaternion<T>::Identity);
+
+ return *this;
+ }
+
+ /// \brief Adds inertial properties to current object.
+ /// The mass, center of mass location, and inertia matrix are updated
+ /// as long as the total mass is positive.
+ /// \param[in] _inertial Inertial to add.
+ /// \return Sum of inertials as new object.
+ public: const Inertial<T> operator+(const Inertial<T> &_inertial) const
+ {
+ return Inertial<T>(*this) += _inertial;
+ }
+
+ /// \brief Mass and inertia matrix of the object expressed in the
+ /// center of mass reference frame.
+ private: MassMatrix3<T> massMatrix;
+
+ /// \brief Pose offset of center of mass reference frame relative
+ /// to a base frame.
+ private: Pose3<T> pose;
+ };
+
+ typedef Inertial<double> Inertiald;
+ typedef Inertial<float> Inertialf;
+ }
+}
+#endif
diff --git a/include/ignition/math/Kmeans.hh b/include/ignition/math/Kmeans.hh
index 9664dec..0d8705e 100644
--- a/include/ignition/math/Kmeans.hh
+++ b/include/ignition/math/Kmeans.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_KMEANS_HH_
-#define _IGNITION_KMEANS_HH_
+#ifndef IGNITION_MATH_KMEANS_HH_
+#define IGNITION_MATH_KMEANS_HH_
#include <vector>
#include <ignition/math/Vector3.hh>
diff --git a/include/ignition/math/KmeansPrivate.hh b/include/ignition/math/KmeansPrivate.hh
index 1dd7f2a..2ee0471 100644
--- a/include/ignition/math/KmeansPrivate.hh
+++ b/include/ignition/math/KmeansPrivate.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_KMEANS_PRIVATE_HH_
-#define _IGNITION_KMEANS_PRIVATE_HH_
+#ifndef IGNITION_MATH_KMEANS_PRIVATE_HH_
+#define IGNITION_MATH_KMEANS_PRIVATE_HH_
#include <vector>
#include <ignition/math/Vector3.hh>
diff --git a/include/ignition/math/Line2.hh b/include/ignition/math/Line2.hh
index fbb962b..e436a14 100644
--- a/include/ignition/math/Line2.hh
+++ b/include/ignition/math/Line2.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_LINE2_HH_
-#define _IGNITION_LINE2_HH_
+#ifndef IGNITION_MATH_LINE2_HH_
+#define IGNITION_MATH_LINE2_HH_
#include <algorithm>
#include <ignition/math/Vector2.hh>
diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh
index 6042f9a..81eedb3 100644
--- a/include/ignition/math/Line3.hh
+++ b/include/ignition/math/Line3.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_LINE3_HH_
-#define _IGNITION_LINE3_HH_
+#ifndef IGNITION_MATH_LINE3_HH_
+#define IGNITION_MATH_LINE3_HH_
#include <algorithm>
#include <ignition/math/Vector3.hh>
@@ -144,193 +144,6 @@ namespace ignition
return this->pts[0].Distance(this->pts[1]);
}
- /// \brief Get the shortest line between this line and the
- /// provided line.
- ///
- /// In the case when the two lines are parallel, we choose the first
- /// point of this line and the closest point in the provided line.
- /// \param[in] _line Line to compare against this.
- /// \param[out] _result The shortest line between _line and this.
- /// \return True if a solution was found. False if a solution is not
- /// possible.
- public: bool Distance(const Line3<T> &_line, Line3<T> &_result,
- const double _epsilon = 1e-6) const
- {
- Vector3<T> p13 = this->pts[0] - _line[0];
- Vector3<T> p43 = _line[1] - _line[0];
-
- if (std::abs(p43.X()) < _epsilon && std::abs(p43.Y()) < _epsilon &&
- std::abs(p43.Z()) < _epsilon)
- {
- return false;
- }
-
- Vector3<T> p21 = this->pts[1] - this->pts[0];
-
- if (std::abs(p21.X()) < _epsilon && std::abs(p21.Y()) < _epsilon &&
- std::abs(p21.Z()) < _epsilon)
- {
- return false;
- }
-
- double d1343 = p13.Dot(p43);
- double d4321 = p43.Dot(p21);
- double d1321 = p13.Dot(p21);
- double d4343 = p43.Dot(p43);
- double d2121 = p21.Dot(p21);
-
- double denom = d2121 * d4343 - d4321 * d4321;
-
- // In this case, we choose the first point in this line,
- // and the closest point in the provided line.
- if (std::abs(denom) < _epsilon)
- {
- double d1 = this->pts[0].Distance(_line[0]);
- double d2 = this->pts[0].Distance(_line[1]);
-
- double d3 = this->pts[1].Distance(_line[0]);
- double d4 = this->pts[1].Distance(_line[1]);
-
- if (d1 <= d2 && d1 <= d3 && d1 <= d4)
- {
- _result.SetA(this->pts[0]);
- _result.SetB(_line[0]);
- }
- else if (d2 <= d3 && d2 <= d4)
- {
- _result.SetA(this->pts[0]);
- _result.SetB(_line[1]);
- }
- else if (d3 <= d4)
- {
- _result.SetA(this->pts[1]);
- _result.SetB(_line[0]);
- }
- else
- {
- _result.SetA(this->pts[1]);
- _result.SetB(_line[1]);
- }
-
- return true;
- }
-
- double numer = d1343 * d4321 - d1321 * d4343;
-
- double mua = clamp(numer / denom, 0.0, 1.0);
- double mub = clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0);
-
- _result.Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub));
-
- return true;
- }
-
- /// \brief Check if this line intersects the given line segment.
- /// \param[in] _line The line to check for intersection.
- /// \param[in] _epsilon The error bounds within which the intersection
- /// check will return true.
- /// \return True if an intersection was found.
- public: bool Intersect(const Line3<T> &_line,
- double _epsilon = 1e-6) const
- {
- static math::Vector3<T> ignore;
- return this->Intersect(_line, ignore, _epsilon);
- }
-
- /// \brief Test if this line and the given line are coplanar.
- /// \param[in] _line Line to check against.
- /// \param[in] _epsilon The error bounds within which the
- /// check will return true.
- /// \return True if the two lines are coplanar.
- public: bool Coplanar(const Line3<T> &_line,
- const double _epsilon = 1e-6) const
- {
- return std::abs((_line[0] - this->pts[0]).Dot(
- (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0])))
- <= _epsilon;
- }
-
- /// \brief Test if this line and the given line are parallel.
- /// \param[in] _line Line to check against.
- /// \param[in] _epsilon The error bounds within which the
- /// check will return true.
- /// \return True if the two lines are parallel.
- public: bool Parallel(const Line3<T> &_line,
- const double _epsilon = 1e-6) const
- {
- return (this->pts[1] - this->pts[0]).Cross(
- _line[1] - _line[0]).Length() <= _epsilon;
- }
-
- /// \brief Check if this line intersects the given line segment. The
- /// point of intersection is returned in the _pt parameter.
- /// \param[in] _line The line to check for intersection.
- /// \param[out] _pt The point of intersection. This value is only
- /// valid if the return value is true.
- /// \param[in] _epsilon The error bounds within which the intersection
- /// check will return true.
- /// \return True if an intersection was found.
- public: bool Intersect(const Line3<T> &_line, math::Vector3<T> &_pt,
- double _epsilon = 1e-6) const
- {
- // Handle special case when lines are parallel
- if (this->Parallel(_line, _epsilon))
- {
- // Check if _line's starting point is on the line.
- if (this->Within(_line[0], _epsilon))
- {
- _pt = _line[0];
- return true;
- }
- // Check if _line's ending point is on the line.
- else if (this->Within(_line[1], _epsilon))
- {
- _pt = _line[1];
- return true;
- }
- // Otherwise return false.
- else
- return false;
- }
-
- // Get the line that is the shortest distance between this and _line
- math::Line3<T> distLine;
- this->Distance(_line, distLine, _epsilon);
-
- // If the length of the line is less than epsilon, then they
- // intersect.
- if (distLine.Length() < _epsilon)
- {
- _pt = distLine[0];
- return true;
- }
-
- return false;
- }
-
- /// \brief Check if the given point is between the start and end
- /// points of the line segment.
- /// \param[in] _pt Point to check.
- /// \param[in] _epsilon The error bounds within which the within
- /// check will return true.
- /// \return True if the point is on the segement.
- public: bool Within(const math::Vector3<T> &_pt,
- double _epsilon = 1e-6) const
- {
- return _pt.X() <= std::max(this->pts[0].X(),
- this->pts[1].X()) + _epsilon &&
- _pt.X() >= std::min(this->pts[0].X(),
- this->pts[1].X()) - _epsilon &&
- _pt.Y() <= std::max(this->pts[0].Y(),
- this->pts[1].Y()) + _epsilon &&
- _pt.Y() >= std::min(this->pts[0].Y(),
- this->pts[1].Y()) - _epsilon &&
- _pt.Z() <= std::max(this->pts[0].Z(),
- this->pts[1].Z()) + _epsilon &&
- _pt.Z() >= std::min(this->pts[0].Z(),
- this->pts[1].Z()) - _epsilon;
- }
-
/// \brief Equality operator.
/// \param[in] _line Line to compare for equality.
/// \return True if the given line is equal to this line
diff --git a/include/ignition/math/MassMatrix3.hh b/include/ignition/math/MassMatrix3.hh
index 0fff65d..73d8441 100644
--- a/include/ignition/math/MassMatrix3.hh
+++ b/include/ignition/math/MassMatrix3.hh
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2015 Open Source Robotics Foundation
+ * Copyright (C) 2015-2016 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.
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_MASSMATRIX3_HH_
-#define _IGNITION_MASSMATRIX3_HH_
+#ifndef IGNITION_MATH_MASSMATRIX3_HH_
+#define IGNITION_MATH_MASSMATRIX3_HH_
#include <algorithm>
#include <string>
@@ -44,11 +44,11 @@ namespace ignition
/// \brief Constructor.
/// \param[in] _mass Mass value in kg if using metric.
- /// \param[in] _Ixxyyzz Diagonal moments of inertia.
- /// \param[in] _Ixyxzyz Off-diagonal moments of inertia
+ /// \param[in] _ixxyyzz Diagonal moments of inertia.
+ /// \param[in] _ixyxzyz Off-diagonal moments of inertia
public: MassMatrix3(const T &_mass,
const Vector3<T> &_ixxyyzz,
- const Vector3<T> &_ixyxzyz )
+ const Vector3<T> &_ixyxzyz)
: mass(_mass), Ixxyyzz(_ixxyyzz), Ixyxzyz(_ixyxzyz)
{}
@@ -317,9 +317,14 @@ namespace ignition
/// \brief Compute principal moments of inertia,
/// which are the eigenvalues of the moment of inertia matrix.
- /// \param[in] _tol Relative tolerance.
- /// \return Principal moments of inertia. If the matrix is
- /// already diagonal, they are returned in the existing order.
+ /// \param[in] _tol Relative tolerance given by absolute value
+ /// of _tol.
+ /// Negative values of _tol are interpreted as a flag that
+ /// causes principal moments to always be sorted from smallest
+ /// to largest.
+ /// \return Principal moments of inertia.
+ /// If the matrix is already diagonal and _tol is positive,
+ /// they are returned in the existing order.
/// Otherwise, the moments are sorted from smallest to largest.
public: Vector3<T> PrincipalMoments(const T _tol = 1e-6) const
{
@@ -370,13 +375,514 @@ namespace ignition
// sort the moments from smallest to largest
T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0;
- T moment1 = (b + 2*sqrt(p) * cos((delta + 2*M_PI)/3.0)) / 3.0;
- T moment2 = (b + 2*sqrt(p) * cos((delta - 2*M_PI)/3.0)) / 3.0;
+ T moment1 = (b + 2*sqrt(p) * cos((delta + 2*IGN_PI)/3.0)) / 3.0;
+ T moment2 = (b + 2*sqrt(p) * cos((delta - 2*IGN_PI)/3.0)) / 3.0;
sort3(moment0, moment1, moment2);
return Vector3<T>(moment0, moment1, moment2);
}
- /// \brief Mass the object. Default is 0.0.
+ /// \brief Compute rotational offset of principal axes.
+ /// \param[in] _tol Relative tolerance given by absolute value
+ /// of _tol.
+ /// Negative values of _tol are interpreted as a flag that
+ /// causes principal moments to always be sorted from smallest
+ /// to largest.
+ /// \return Quaternion representing rotational offset of principal axes.
+ /// With a rotation matrix constructed from this quaternion R(q)
+ /// and a diagonal matrix L with principal moments on the diagonal,
+ /// the original moment of inertia matrix MOI can be reconstructed
+ /// with MOI = R(q).Transpose() * L * R(q)
+ public: Quaternion<T> PrincipalAxesOffset(const T _tol = 1e-6) const
+ {
+ // Compute tolerance relative to maximum value of inertia diagonal
+ T tol = _tol * this->Ixxyyzz.Max();
+ Vector3<T> moments = this->PrincipalMoments(tol);
+ if (moments.Equal(this->Ixxyyzz, tol))
+ {
+ // matrix is already aligned with principal axes
+ // this includes case when all three moments are
+ // approximately equal
+ // return identity rotation
+ return Quaternion<T>::Identity;
+ }
+
+ // Algorithm based on http://arxiv.org/abs/1306.6291v4
+ // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
+ // Matrix, by Maarten Kronenburg
+ // A real, symmetric matrix can be diagonalized by an orthogonal matrix
+ // (due to the finite-dimensional spectral theorem
+ // https://en.wikipedia.org/wiki/Spectral_theorem
+ // #Hermitian_maps_and_Hermitian_matrices ),
+ // and another name for orthogonal matrix is rotation matrix.
+ // Section 5 of the paper shows how to compute Euler angles
+ // phi1, phi2, and phi3 that map to a rotation matrix.
+ // In some cases, there are multiple possible values for a given angle,
+ // such as phi1, that are denoted as phi11, phi12, phi11a, phi12a, etc.
+ // Similar variable names are used to the paper so that the paper
+ // can be used as an additional reference.
+
+ // f1, f2 defined in equations 5.5, 5.6
+ Vector2<T> f1(this->Ixyxzyz[0], -this->Ixyxzyz[1]);
+ Vector2<T> f2(this->Ixxyyzz[1] - this->Ixxyyzz[2],
+ -2*this->Ixyxzyz[2]);
+
+ // Check if two moments are equal, since different equations are used
+ // The moments vector is already sorted, so just check adjacent values.
+ Vector2<T> momentsDiff(moments[0] - moments[1],
+ moments[1] - moments[2]);
+
+ // index of unequal moment
+ int unequalMoment = -1;
+ if (equal<T>(momentsDiff[0], 0, std::abs(tol)))
+ unequalMoment = 2;
+ else if (equal<T>(momentsDiff[1], 0, std::abs(tol)))
+ unequalMoment = 0;
+
+ if (unequalMoment >= 0)
+ {
+ // moments[1] is the repeated value
+ // it is not equal to moments[unequalMoment]
+ // momentsDiff3 = lambda - lambda3
+ T momentsDiff3 = moments[1] - moments[unequalMoment];
+ // eq 5.21:
+ // s = cos(phi2)^2 = (A_11 - lambda3) / (lambda - lambda3)
+ // s >= 0 since A_11 is in range [lambda, lambda3]
+ T s = (this->Ixxyyzz[0] - moments[unequalMoment]) / momentsDiff3;
+ // set phi3 to zero for repeated moments (eq 5.23)
+ T phi3 = 0;
+ // phi2 = +- acos(sqrt(s))
+ // start with just the positive value
+ // also clamp the acos argument to prevent NaN's
+ T phi2 = acos(clamp<T>(ClampedSqrt(s), -1, 1));
+
+ // The paper defines variables phi11 and phi12
+ // which are candidate values of angle phi1.
+ // phi12 is straightforward to compute as a function of f2 and g2.
+ // eq 5.25:
+ Vector2<T> g2(momentsDiff3 * s, 0);
+ // combining eq 5.12 and 5.14, and subtracting psi2
+ // instead of multiplying by its rotation matrix:
+ math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
+ phi12.Normalize();
+
+ // The paragraph prior to equation 5.16 describes how to choose
+ // the candidate value of phi1 based on the length
+ // of the f1 and f2 vectors.
+ // * When |f1| != 0 and |f2| != 0, then one should choose the
+ // value of phi2 so that phi11 = phi12
+ // * When |f1| == 0 and f2 != 0, then phi1 = phi12
+ // phi11 can be ignored, and either sign of phi2 can be used
+ // * The case of |f2| == 0 can be ignored at this point in the code
+ // since having a repeated moment when |f2| == 0 implies that
+ // the matrix is diagonal. But this function returns a unit
+ // quaternion for diagonal matrices, so we can assume |f2| != 0
+ // See MassMatrix3.ipynb for a more complete discussion.
+ //
+ // Since |f2| != 0, we only need to consider |f1|
+ // * |f1| == 0: phi1 = phi12
+ // * |f1| != 0: choose phi2 so that phi11 == phi12
+ // In either case, phi1 = phi12,
+ // and the sign of phi2 must be chosen to make phi11 == phi12
+ T phi1 = phi12.Radian();
+
+ bool f1small = f1.SquaredLength() < std::pow(tol, 2);
+ if (!f1small)
+ {
+ // a: phi2 > 0
+ // eq. 5.24
+ Vector2<T> g1a(0, 0.5*momentsDiff3 * sin(2*phi2));
+ // combining eq 5.11 and 5.13, and subtracting psi1
+ // instead of multiplying by its rotation matrix:
+ math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
+ phi11a.Normalize();
+
+ // b: phi2 < 0
+ // eq. 5.24
+ Vector2<T> g1b(0, 0.5*momentsDiff3 * sin(-2*phi2));
+ // combining eq 5.11 and 5.13, and subtracting psi1
+ // instead of multiplying by its rotation matrix:
+ math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
+ phi11b.Normalize();
+
+ // choose sign of phi2
+ // based on whether phi11a or phi11b is closer to phi12
+ // use sin and cos to account for angle wrapping
+ T erra = std::pow(sin(phi1) - sin(phi11a.Radian()), 2)
+ + std::pow(cos(phi1) - cos(phi11a.Radian()), 2);
+ T errb = std::pow(sin(phi1) - sin(phi11b.Radian()), 2)
+ + std::pow(cos(phi1) - cos(phi11b.Radian()), 2);
+ if (errb < erra)
+ {
+ phi2 *= -1;
+ }
+ }
+
+ // I determined these arguments using trial and error
+ Quaternion<T> result = Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
+
+ // Previous equations assume repeated moments are at the beginning
+ // of the moments vector (moments[0] == moments[1]).
+ // We have the vectors sorted by size, so it's possible that the
+ // repeated moments are at the end (moments[1] == moments[2]).
+ // In this case (unequalMoment == 0), we apply an extra
+ // rotation that exchanges moment[0] and moment[2]
+ // Rotation matrix = [ 0 0 1]
+ // [ 0 1 0]
+ // [-1 0 0]
+ // That is equivalent to a 90 degree pitch
+ if (unequalMoment == 0)
+ result *= Quaternion<T>(0, IGN_PI_2, 0);
+
+ return result;
+ }
+
+ // No repeated principal moments
+ // eq 5.1:
+ T v = (std::pow(this->Ixyxzyz[0], 2) + std::pow(this->Ixyxzyz[1], 2)
+ +(this->Ixxyyzz[0] - moments[2])
+ *(this->Ixxyyzz[0] + moments[2] - moments[0] - moments[1]))
+ / ((moments[1] - moments[2]) * (moments[2] - moments[0]));
+ // value of w depends on v
+ T w;
+ if (v < std::abs(tol))
+ {
+ // first sentence after eq 5.4:
+ // "In the case that v = 0, then w = 1."
+ w = 1;
+ }
+ else
+ {
+ // eq 5.2:
+ w = (this->Ixxyyzz[0] - moments[2] + (moments[2] - moments[1])*v)
+ / ((moments[0] - moments[1]) * v);
+ }
+ // initialize values of angle phi1, phi2, phi3
+ T phi1 = 0;
+ // eq 5.3: start with positive value
+ T phi2 = acos(clamp<T>(ClampedSqrt(v), -1, 1));
+ // eq 5.4: start with positive value
+ T phi3 = acos(clamp<T>(ClampedSqrt(w), -1, 1));
+
+ // compute g1, g2 for phi2,phi3 >= 0
+ // equations 5.7, 5.8
+ Vector2<T> g1(
+ 0.5* (moments[0]-moments[1])*ClampedSqrt(v)*sin(2*phi3),
+ 0.5*((moments[0]-moments[1])*w + moments[1]-moments[2])*sin(2*phi2));
+ Vector2<T> g2(
+ (moments[0]-moments[1])*(1 + (v-2)*w) + (moments[1]-moments[2])*v,
+ (moments[0]-moments[1])*sin(phi2)*sin(2*phi3));
+
+ // The paragraph prior to equation 5.16 describes how to choose
+ // the candidate value of phi1 based on the length
+ // of the f1 and f2 vectors.
+ // * The case of |f1| == |f2| == 0 implies a repeated moment,
+ // which should not be possible at this point in the code
+ // * When |f1| != 0 and |f2| != 0, then one should choose the
+ // value of phi2 so that phi11 = phi12
+ // * When |f1| == 0 and f2 != 0, then phi1 = phi12
+ // phi11 can be ignored, and either sign of phi2, phi3 can be used
+ // * When |f2| == 0 and f1 != 0, then phi1 = phi11
+ // phi12 can be ignored, and either sign of phi2, phi3 can be used
+ bool f1small = f1.SquaredLength() < std::pow(tol, 2);
+ bool f2small = f2.SquaredLength() < std::pow(tol, 2);
+ if (f1small && f2small)
+ {
+ // this should never happen
+ // f1small && f2small implies a repeated moment
+ // return invalid quaternion
+ return Quaternion<T>::Zero;
+ }
+ else if (f1small)
+ {
+ // use phi12 (equations 5.12, 5.14)
+ math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
+ phi12.Normalize();
+ phi1 = phi12.Radian();
+ }
+ else if (f2small)
+ {
+ // use phi11 (equations 5.11, 5.13)
+ math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
+ phi11.Normalize();
+ phi1 = phi11.Radian();
+ }
+ else
+ {
+ // check for when phi11 == phi12
+ // eqs 5.11, 5.13:
+ math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
+ phi11.Normalize();
+ // eqs 5.12, 5.14:
+ math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
+ phi12.Normalize();
+ T err = std::pow(sin(phi11.Radian()) - sin(phi12.Radian()), 2)
+ + std::pow(cos(phi11.Radian()) - cos(phi12.Radian()), 2);
+ phi1 = phi11.Radian();
+ math::Vector2<T> signsPhi23(1, 1);
+ // case a: phi2 <= 0
+ {
+ Vector2<T> g1a = Vector2<T>(1, -1) * g1;
+ Vector2<T> g2a = Vector2<T>(1, -1) * g2;
+ math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
+ math::Angle phi12a(0.5*(Angle2(g2a, tol) - Angle2(f2, tol)));
+ phi11a.Normalize();
+ phi12a.Normalize();
+ T erra = std::pow(sin(phi11a.Radian()) - sin(phi12a.Radian()), 2)
+ + std::pow(cos(phi11a.Radian()) - cos(phi12a.Radian()), 2);
+ if (erra < err)
+ {
+ err = erra;
+ phi1 = phi11a.Radian();
+ signsPhi23.Set(-1, 1);
+ }
+ }
+ // case b: phi3 <= 0
+ {
+ Vector2<T> g1b = Vector2<T>(-1, 1) * g1;
+ Vector2<T> g2b = Vector2<T>(1, -1) * g2;
+ math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
+ math::Angle phi12b(0.5*(Angle2(g2b, tol) - Angle2(f2, tol)));
+ phi11b.Normalize();
+ phi12b.Normalize();
+ T errb = std::pow(sin(phi11b.Radian()) - sin(phi12b.Radian()), 2)
+ + std::pow(cos(phi11b.Radian()) - cos(phi12b.Radian()), 2);
+ if (errb < err)
+ {
+ err = errb;
+ phi1 = phi11b.Radian();
+ signsPhi23.Set(1, -1);
+ }
+ }
+ // case c: phi2,phi3 <= 0
+ {
+ Vector2<T> g1c = Vector2<T>(-1, -1) * g1;
+ Vector2<T> g2c = g2;
+ math::Angle phi11c(Angle2(g1c, tol) - Angle2(f1, tol));
+ math::Angle phi12c(0.5*(Angle2(g2c, tol) - Angle2(f2, tol)));
+ phi11c.Normalize();
+ phi12c.Normalize();
+ T errc = std::pow(sin(phi11c.Radian()) - sin(phi12c.Radian()), 2)
+ + std::pow(cos(phi11c.Radian()) - cos(phi12c.Radian()), 2);
+ if (errc < err)
+ {
+ err = errc;
+ phi1 = phi11c.Radian();
+ signsPhi23.Set(-1, -1);
+ }
+ }
+
+ // apply sign changes
+ phi2 *= signsPhi23[0];
+ phi3 *= signsPhi23[1];
+ }
+
+ // I determined these arguments using trial and error
+ return Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
+ }
+
+ /// \brief Get dimensions and rotation offset of uniform box
+ /// with equivalent mass and moment of inertia.
+ /// To compute this, the Matrix3 is diagonalized.
+ /// The eigenvalues on the diagonal and the rotation offset
+ /// of the principal axes are returned.
+ /// \param[in] _size Dimensions of box aligned with principal axes.
+ /// \param[in] _rot Rotational offset of principal axes.
+ /// \param[in] _tol Relative tolerance.
+ /// \return True if box properties were computed successfully.
+ public: bool EquivalentBox(Vector3<T> &_size,
+ Quaternion<T> &_rot,
+ const T _tol = 1e-6) const
+ {
+ if (!this->IsPositive())
+ {
+ // inertia is not positive, cannot compute equivalent box
+ return false;
+ }
+
+ Vector3<T> moments = this->PrincipalMoments(_tol);
+ if (!ValidMoments(moments))
+ {
+ // principal moments don't satisfy the triangle identity
+ return false;
+ }
+
+ // The reason for checking that the principal moments satisfy
+ // the triangle inequality
+ // I1 + I2 - I3 >= 0
+ // is to ensure that the arguments to sqrt in these equations
+ // are positive and the box size is real.
+ _size.X(sqrt(6*(moments.Y() + moments.Z() - moments.X()) / this->mass));
+ _size.Y(sqrt(6*(moments.Z() + moments.X() - moments.Y()) / this->mass));
+ _size.Z(sqrt(6*(moments.X() + moments.Y() - moments.Z()) / this->mass));
+
+ _rot = this->PrincipalAxesOffset(_tol);
+
+ if (_rot == Quaternion<T>::Zero)
+ {
+ // _rot is an invalid quaternion
+ return false;
+ }
+
+ return true;
+ }
+
+ /// \brief Set inertial properties based on mass and equivalent box.
+ /// \param[in] _mass Mass to set.
+ /// \param[in] _size Size of equivalent box.
+ /// \param[in] _rot Rotational offset of equivalent box.
+ /// \return True if inertial properties were set successfully.
+ public: bool SetFromBox(const T _mass,
+ const Vector3<T> &_size,
+ const Quaternion<T> &_rot = Quaternion<T>::Identity)
+ {
+ // Check that _mass and _size are strictly positive
+ // and that quatenion is valid
+ if (_mass <= 0 || _size.Min() <= 0 || _rot == Quaternion<T>::Zero)
+ {
+ return false;
+ }
+ this->Mass(_mass);
+ return this->SetFromBox(_size, _rot);
+ }
+
+ /// \brief Set inertial properties based on equivalent box
+ /// using the current mass value.
+ /// \param[in] _size Size of equivalent box.
+ /// \param[in] _rot Rotational offset of equivalent box.
+ /// \return True if inertial properties were set successfully.
+ public: bool SetFromBox(const Vector3<T> &_size,
+ const Quaternion<T> &_rot = Quaternion<T>::Identity)
+ {
+ // Check that _mass and _size are strictly positive
+ // and that quatenion is valid
+ if (this->Mass() <= 0 || _size.Min() <= 0 ||
+ _rot == Quaternion<T>::Zero)
+ {
+ return false;
+ }
+
+ // Diagonal matrix L with principal moments
+ Matrix3<T> L;
+ T x2 = std::pow(_size.X(), 2);
+ T y2 = std::pow(_size.Y(), 2);
+ T z2 = std::pow(_size.Z(), 2);
+ L(0, 0) = this->mass / 12.0 * (y2 + z2);
+ L(1, 1) = this->mass / 12.0 * (z2 + x2);
+ L(2, 2) = this->mass / 12.0 * (x2 + y2);
+ Matrix3<T> R(_rot);
+ return this->MOI(R * L * R.Transposed());
+ }
+
+ /// \brief Set inertial properties based on mass and equivalent cylinder
+ /// aligned with Z axis.
+ /// \param[in] _mass Mass to set.
+ /// \param[in] _length Length of cylinder along Z axis.
+ /// \param[in] _radius Radius of cylinder.
+ /// \param[in] _rot Rotational offset of equivalent cylinder.
+ /// \return True if inertial properties were set successfully.
+ public: bool SetFromCylinderZ(const T _mass,
+ const T _length,
+ const T _radius,
+ const Quaternion<T> &_rot = Quaternion<T>::Identity)
+ {
+ // Check that _mass, _radius and _length are strictly positive
+ // and that quatenion is valid
+ if (_mass <= 0 || _length <= 0 || _radius <= 0 ||
+ _rot == Quaternion<T>::Zero)
+ {
+ return false;
+ }
+ this->Mass(_mass);
+ return this->SetFromCylinderZ(_length, _radius, _rot);
+ }
+
+ /// \brief Set inertial properties based on equivalent cylinder
+ /// aligned with Z axis using the current mass value.
+ /// \param[in] _length Length of cylinder along Z axis.
+ /// \param[in] _radius Radius of cylinder.
+ /// \param[in] _rot Rotational offset of equivalent cylinder.
+ /// \return True if inertial properties were set successfully.
+ public: bool SetFromCylinderZ(const T _length,
+ const T _radius,
+ const Quaternion<T> &_rot)
+ {
+ // Check that _mass and _size are strictly positive
+ // and that quatenion is valid
+ if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 ||
+ _rot == Quaternion<T>::Zero)
+ {
+ return false;
+ }
+
+ // Diagonal matrix L with principal moments
+ T radius2 = std::pow(_radius, 2);
+ Matrix3<T> L;
+ L(0, 0) = this->mass / 12.0 * (3*radius2 + std::pow(_length, 2));
+ L(1, 1) = L(0, 0);
+ L(2, 2) = this->mass / 2.0 * radius2;
+ Matrix3<T> R(_rot);
+ return this->MOI(R * L * R.Transposed());
+ }
+
+ /// \brief Set inertial properties based on mass and equivalent sphere.
+ /// \param[in] _mass Mass to set.
+ /// \param[in] _radius Radius of equivalent, uniform sphere.
+ /// \return True if inertial properties were set successfully.
+ public: bool SetFromSphere(const T _mass, const T _radius)
+ {
+ // Check that _mass and _radius are strictly positive
+ if (_mass <= 0 || _radius <= 0)
+ {
+ return false;
+ }
+ this->Mass(_mass);
+ return this->SetFromSphere(_radius);
+ }
+
+ /// \brief Set inertial properties based on equivalent sphere
+ /// using the current mass value.
+ /// \param[in] _radius Radius of equivalent, uniform sphere.
+ /// \return True if inertial properties were set successfully.
+ public: bool SetFromSphere(const T _radius)
+ {
+ // Check that _mass and _radius are strictly positive
+ if (this->Mass() <= 0 || _radius <= 0)
+ {
+ return false;
+ }
+
+ // Diagonal matrix L with principal moments
+ T radius2 = std::pow(_radius, 2);
+ Matrix3<T> L;
+ L(0, 0) = 0.4 * this->mass * radius2;
+ L(1, 1) = 0.4 * this->mass * radius2;
+ L(2, 2) = 0.4 * this->mass * radius2;
+ return this->MOI(L);
+ }
+
+ /// \brief Square root of positive numbers, otherwise zero.
+ /// \param[in] _x Number to be square rooted.
+ /// \return sqrt(_x) if _x > 0, otherwise 0
+ private: static inline T ClampedSqrt(const T &_x)
+ {
+ if (_x <= 0)
+ return 0;
+ return sqrt(_x);
+ }
+
+ /// \brief Angle formed by direction of a Vector2.
+ /// \param[in] _v Vector whose direction is to be computed.
+ /// \param[in] _eps Minimum length of vector required for computing angle.
+ /// \return Angle formed between vector and X axis,
+ /// or zero if vector has length less than 1e-6.
+ private: static T Angle2(const Vector2<T> &_v, const T _eps = 1e-6)
+ {
+ if (_v.SquaredLength() < std::pow(_eps, 2))
+ return 0;
+ return atan2(_v[1], _v[0]);
+ }
+
+ /// \brief Mass of the object. Default is 0.0.
private: T mass;
/// \brief Principal moments of inertia. Default is (0.0 0.0 0.0)
diff --git a/include/ignition/math/MassMatrix3.ipynb b/include/ignition/math/MassMatrix3.ipynb
new file mode 100644
index 0000000..0e94e96
--- /dev/null
+++ b/include/ignition/math/MassMatrix3.ipynb
@@ -0,0 +1,134 @@
+{
+ "metadata": {
+ "name": "",
+ "signature": "sha256:806f7b6e2812a7f1e8ff67e708d852d06c6ed452860f8c9bb02bb4ff9ea5fdbf"
+ },
+ "nbformat": 3,
+ "nbformat_minor": 0,
+ "worksheets": [
+ {
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "# Diagonalization of 3x3 symmetric matrix\n",
+ "\n",
+ "The `MassMatrix3` class uses an algorithm described in [A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric Matrix](http://arxiv.org/abs/1306.6291v4) by M.J. Kronenburg. This document proves that when a matrix $\\textbf{A}$ has a repeated eigenvalue and the vector $\\textbf{f}_2$ has length 0, then $\\textbf{A}$ is a diagonal matrix.\n",
+ "\n",
+ "The symmetric 3x3 matrix $\\textbf{A}$ has diagonal elements $A_{11}$, $A_{22}$, and $A_{33}$ and off-diagonal terms $A_{12}$, $A_{13}$, and $A_{23}$. The vector $\\textbf{f}_2$ is defined in (5.6) as\n",
+ "\n",
+ "$\\textbf{f}_2 = [A_{22} - A_{33}, -2A_{23}]^T$\n",
+ "\n",
+ "When $\\textbf{f}_2$ has length zero, the following are true:\n",
+ "\n",
+ "$A_{22} = A_{33} = A$\n",
+ "\n",
+ "$A_{23} = 0$\n",
+ "\n",
+ "In Section 4, conditions are given for which a matrix will have at least 1 repeated eigenvalue $\\lambda$ based on the coefficients of its characteristic equation:\n",
+ "\n",
+ "$\\lambda^3 - b \\lambda^2 + c \\lambda + d = 0$\n",
+ "\n",
+ "With $p = b^2 - 3c$ and $q = 2b^3 - 9bc - 27d$, there is at least one repeated eigenvalue when $q^2 = 4p^3$\n",
+ "\n",
+ "These conditions are manipulated symbolically below:"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "collapsed": false,
+ "input": [
+ "from sympy import *"
+ ],
+ "language": "python",
+ "metadata": {},
+ "outputs": []
+ },
+ {
+ "cell_type": "code",
+ "collapsed": false,
+ "input": [
+ "# Identify condition in which there is a repeated eigenvalue (Q^2 - 4P^3 = 0)\n",
+ "B, C, D, P, Q = symbols('B C D P Q')\n",
+ "P = B**2 - 3*C\n",
+ "Q = 2*B**3 - 9*B*C - 27*D\n",
+ "simplify(expand(Q**2 - 4*P**3))"
+ ],
+ "language": "python",
+ "metadata": {},
+ "outputs": [
+ {
+ "metadata": {},
+ "output_type": "pyout",
+ "prompt_number": 13,
+ "text": [
+ "-108*B**3*D - 27*B**2*C**2 + 486*B*C*D + 108*C**3 + 729*D**2"
+ ]
+ }
+ ],
+ "prompt_number": 13
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "First the condition in which there is a repeated eigen value ($q^2 = 4p^3$) is expressed in terms of $b$, $c$, and $d$ and simplified to:\n",
+ "\n",
+ "$-108b^3d - 27b^2c^2 + 486bcd + 108c^3 + 729d^2 = 0$"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "collapsed": false,
+ "input": [
+ "# Expand expression for which there is a repeated eigenvalue\n",
+ "A11, A12, A13, A = symbols('A11 A12 A13 A')\n",
+ "b, c, d = symbols('b c d')\n",
+ "b = A11 + 2*A\n",
+ "c = A11*A + A11*A + A**2 - A12**2 - A13**2\n",
+ "d = A*A13**2 + A*A12**2 - A11*A**2\n",
+ "factor(simplify(-108*b**3*d - 27*b**2*c**2 + 486*b*c*d + 108*c**3 + 729*d**2))"
+ ],
+ "language": "python",
+ "metadata": {},
+ "outputs": [
+ {
+ "metadata": {},
+ "output_type": "pyout",
+ "prompt_number": 16,
+ "text": [
+ "-27*(A12**2 + A13**2)**2*(A**2 - 2*A*A11 + A11**2 + 4*A12**2 + 4*A13**2)"
+ ]
+ }
+ ],
+ "prompt_number": 16
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "This condition is then expressed in terms of $A11$, $A$, $A12$, and $A13$ corresponding to $\\textbf{f}_2 = 0$.\n",
+ "After simplification and factoring, there are 2 cases for which the expression is equal to zero:\n",
+ "\n",
+ "$A_{12}^2 + A_{13}^2 = 0$\n",
+ "\n",
+ "$4A_{12}^2 + 4A_{13}^2 + (A - A_{11})^2 = 0$\n",
+ "\n",
+ "In both cases, $A_{12} = A_{13} = 0$ is a pre-requisite. Since $A_{23} = 0$ was already assumed, this implies that\n",
+ "the matrix must be diagonal if it has any repeated eigenvalues and $\\textbf{f}_2 = 0$."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "collapsed": false,
+ "input": [],
+ "language": "python",
+ "metadata": {},
+ "outputs": []
+ }
+ ],
+ "metadata": {}
+ }
+ ]
+}
\ No newline at end of file
diff --git a/include/ignition/math/Matrix3.hh b/include/ignition/math/Matrix3.hh
index ceeac54..8ac2d90 100644
--- a/include/ignition/math/Matrix3.hh
+++ b/include/ignition/math/Matrix3.hh
@@ -15,8 +15,8 @@
*
*/
-#ifndef _IGNITION_MATRIX3_HH_
-#define _IGNITION_MATRIX3_HH_
+#ifndef IGNITION_MATH_MATRIX3_HH_
+#define IGNITION_MATH_MATRIX3_HH_
#include <algorithm>
#include <cstring>
@@ -27,6 +27,8 @@ namespace ignition
{
namespace math
{
+ template <typename T> class Quaternion;
+
/// \class Matrix3 Matrix3.hh ignition/math/Matrix3.hh
/// \brief A 3x3 matrix class
template<typename T>
@@ -156,6 +158,49 @@ namespace ignition
this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
}
+ /// \brief Set the matrix to represent rotation from
+ /// vector _v1 to vector _v2, so that
+ /// _v2.Normalize() == this * _v1.Normalize() holds.
+ ///
+ /// \param[in] _v1 The first vector
+ /// \param[in] _v2 The second vector
+ public: void From2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
+ {
+ const T _v1LengthSquared = _v1.SquaredLength();
+ if (_v1LengthSquared <= 0.0)
+ {
+ // zero vector - we can't handle this
+ this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
+ return;
+ }
+
+ const T _v2LengthSquared = _v2.SquaredLength();
+ if (_v2LengthSquared <= 0.0)
+ {
+ // zero vector - we can't handle this
+ this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
+ return;
+ }
+
+ const T dot = _v1.Dot(_v2) / sqrt(_v1LengthSquared * _v2LengthSquared);
+ if (fabs(dot - 1.0) <= 1e-6)
+ {
+ // the vectors are parallel
+ this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
+ return;
+ }
+ else if (fabs(dot + 1.0) <= 1e-6)
+ {
+ // the vectors are opposite
+ this->Set(-1, 0, 0, 0, -1, 0, 0, 0, -1);
+ return;
+ }
+
+ const Vector3<T> cross = _v1.Cross(_v2).Normalize();
+
+ this->Axis(cross, acos(dot));
+ }
+
/// \brief Set a column
/// \param[in] _c The colum index (0, 1, 2)
/// \param[in] _v The value to set in each row of the column
diff --git a/include/ignition/math/Matrix4.hh b/include/ignition/math/Matrix4.hh
index 6580d71..057c28e 100644
--- a/include/ignition/math/Matrix4.hh
+++ b/include/ignition/math/Matrix4.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_MATRIX4_HH_
-#define _IGNITION_MATRIX4_HH_
+#ifndef IGNITION_MATH_MATRIX4_HH_
+#define IGNITION_MATH_MATRIX4_HH_
#include <algorithm>
#include <ignition/math/Helpers.hh>
diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh
index b993ced..b708ccd 100644
--- a/include/ignition/math/Plane.hh
+++ b/include/ignition/math/Plane.hh
@@ -15,8 +15,8 @@
*
*/
-#ifndef _IGNITION_PLANE_HH_
-#define _IGNITION_PLANE_HH_
+#ifndef IGNITION_MATH_PLANE_HH_
+#define IGNITION_MATH_PLANE_HH_
#include <ignition/math/Box.hh>
#include <ignition/math/Vector3.hh>
@@ -53,17 +53,16 @@ namespace ignition
/// \brief Constructor
public: Plane()
+ : d(0.0)
{
- this->d = 0.0;
}
/// \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)
+ : normal(_normal), d(_offset)
{
- this->normal = _normal;
- this->d = _offset;
}
/// \brief Constructor
diff --git a/include/ignition/math/Pose3.hh b/include/ignition/math/Pose3.hh
index cc943d6..5aaf860 100644
--- a/include/ignition/math/Pose3.hh
+++ b/include/ignition/math/Pose3.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_POSE_HH_
-#define _IGNITION_POSE_HH_
+#ifndef IGNITION_MATH_POSE_HH_
+#define IGNITION_MATH_POSE_HH_
#include <ignition/math/Quaternion.hh>
#include <ignition/math/Vector3.hh>
diff --git a/include/ignition/math/Quaternion.hh b/include/ignition/math/Quaternion.hh
index 086c0df..746c39c 100644
--- a/include/ignition/math/Quaternion.hh
+++ b/include/ignition/math/Quaternion.hh
@@ -14,17 +14,20 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_QUATERNION_HH_
-#define _IGNITION_QUATERNION_HH_
+#ifndef IGNITION_MATH_QUATERNION_HH_
+#define IGNITION_MATH_QUATERNION_HH_
#include <ignition/math/Helpers.hh>
#include <ignition/math/Angle.hh>
#include <ignition/math/Vector3.hh>
+#include <ignition/math/Matrix3.hh>
namespace ignition
{
namespace math
{
+ template <typename T> class Matrix3;
+
/// \class Quaternion Quaternion.hh ignition/math/Quaternion.hh
/// \brief A quaternion class
template<typename T>
@@ -33,6 +36,9 @@ namespace ignition
/// \brief math::Quaternion(1, 0, 0, 0)
public: static const Quaternion Identity;
+ /// \brief math::Quaternion(0, 0, 0, 0)
+ public: static const Quaternion Zero;
+
/// \brief Default Constructor
public: Quaternion()
: qw(1), qx(0), qy(0), qz(0)
@@ -74,6 +80,14 @@ namespace ignition
this->Euler(_rpy);
}
+ /// \brief Construct from rotation matrix.
+ /// \param[in] _mat rotation matrix (must be orthogonal, the function
+ /// doesn't check it)
+ public: explicit Quaternion(const Matrix3<T> &_mat)
+ {
+ this->Matrix(_mat);
+ }
+
/// \brief Copy constructor
/// \param[in] _qt Quaternion<T> to copy
public: Quaternion(const Quaternion<T> &_qt)
@@ -407,6 +421,129 @@ namespace ignition
}
}
+ /// \brief Set from a rotation matrix.
+ /// \param[in] _mat rotation matrix (must be orthogonal, the function
+ /// doesn't check it)
+ ///
+ /// Implementation inspired by
+ /// http://www.euclideanspace.com/maths/geometry/rotations/
+ /// conversions/matrixToQuaternion/
+ void Matrix(const Matrix3<T> &_mat)
+ {
+ const T trace = _mat(0, 0) + _mat(1, 1) + _mat(2, 2);
+ if (trace > 0.0000001)
+ {
+ qw = sqrt(1 + trace) / 2;
+ const T s = 1.0 / (4 * qw);
+ qx = (_mat(2, 1) - _mat(1, 2)) * s;
+ qy = (_mat(0, 2) - _mat(2, 0)) * s;
+ qz = (_mat(1, 0) - _mat(0, 1)) * s;
+ }
+ else if (_mat(0, 0) > _mat(1, 1) && _mat(0, 0) > _mat(2, 2))
+ {
+ qx = sqrt(1.0 + _mat(0, 0) - _mat(1, 1) - _mat(2, 2)) / 2;
+ const T s = 1.0 / (4 * qx);
+ qw = (_mat(2, 1) - _mat(1, 2)) * s;
+ qy = (_mat(1, 0) + _mat(0, 1)) * s;
+ qz = (_mat(0, 2) + _mat(2, 0)) * s;
+ }
+ else if (_mat(1, 1) > _mat(2, 2))
+ {
+ qy = sqrt(1.0 - _mat(0, 0) + _mat(1, 1) - _mat(2, 2)) / 2;
+ const T s = 1.0 / (4 * qy);
+ qw = (_mat(0, 2) - _mat(2, 0)) * s;
+ qx = (_mat(0, 1) + _mat(1, 0)) * s;
+ qz = (_mat(1, 2) + _mat(2, 1)) * s;
+ }
+ else
+ {
+ qz = sqrt(1.0 - _mat(0, 0) - _mat(1, 1) + _mat(2, 2)) / 2;
+ const T s = 1.0 / (4 * qz);
+ qw = (_mat(1, 0) - _mat(0, 1)) * s;
+ qx = (_mat(0, 2) + _mat(2, 0)) * s;
+ qy = (_mat(1, 2) + _mat(2, 1)) * s;
+ }
+ }
+
+ /// \brief Set this quaternion to represent rotation from
+ /// vector _v1 to vector _v2, so that
+ /// _v2.Normalize() == this * _v1.Normalize() holds.
+ ///
+ /// \param[in] _v1 The first vector
+ /// \param[in] _v2 The second vector
+ ///
+ /// Implementation inspired by
+ /// http://stackoverflow.com/a/11741520/1076564
+ public: void From2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
+ {
+ // generally, we utilize the fact that a quat (w, x, y, z) represents
+ // rotation of angle 2*w about axis (x, y, z)
+ //
+ // so we want to take get a vector half-way between no rotation and the
+ // double rotation, which is
+ // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ] / 2
+ // if _v1 and _v2 are unit quaternions
+ //
+ // since we normalize the result anyway, we can omit the division,
+ // getting the result:
+ // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ].Normalized()
+ //
+ // if _v1 and _v2 are not normalized, the magnitude (1 + _v1 dot _v2)
+ // is multiplied by k = norm(_v1)*norm(_v2)
+
+ const T kCosTheta = _v1.Dot(_v2);
+ const T k = sqrt(_v1.SquaredLength() * _v2.SquaredLength());
+
+ if (fabs(kCosTheta/k + 1) < 1e-6)
+ {
+ // the vectors are opposite
+ // any vector orthogonal to _v1
+ Vector3<T> other;
+ {
+ const Vector3<T> _v1Abs(_v1.Abs());
+ if (_v1Abs.X() < _v1Abs.Y())
+ {
+ if (_v1Abs.X() < _v1Abs.Z())
+ {
+ other = {1, 0, 0};
+ }
+ else
+ {
+ other = {0, 0, 1};
+ }
+ }
+ else
+ {
+ if (_v1Abs.Y() < _v1Abs.Z())
+ {
+ other = {0, 1, 0};
+ }
+ else
+ {
+ other = {0, 0, 1};
+ }
+ }
+ }
+
+ const Vector3<T> axis(_v1.Cross(other).Normalize());
+
+ qw = 0;
+ qx = axis.X();
+ qy = axis.Y();
+ qz = axis.Z();
+ }
+ else
+ {
+ // the vectors are in general position
+ const Vector3<T> axis(_v1.Cross(_v2));
+ qw = kCosTheta + k;
+ qx = axis.X();
+ qy = axis.Y();
+ qz = axis.Z();
+ this->Normalize();
+ }
+ }
+
/// \brief Scale a Quaternion<T>ion
/// \param[in] _scale Amount to scale this rotation
public: void Scale(T _scale)
@@ -902,6 +1039,9 @@ namespace ignition
template<typename T> const Quaternion<T>
Quaternion<T>::Identity(1, 0, 0, 0);
+ template<typename T> const Quaternion<T>
+ Quaternion<T>::Zero(0, 0, 0, 0);
+
typedef Quaternion<double> Quaterniond;
typedef Quaternion<float> Quaternionf;
typedef Quaternion<int> Quaternioni;
diff --git a/include/ignition/math/Rand.hh b/include/ignition/math/Rand.hh
index 4925a3b..bbe2e09 100644
--- a/include/ignition/math/Rand.hh
+++ b/include/ignition/math/Rand.hh
@@ -14,12 +14,13 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_RAND_HH_
-#define _IGNITION_RAND_HH_
+#ifndef IGNITION_MATH_RAND_HH_
+#define IGNITION_MATH_RAND_HH_
#include <random>
#include <cmath>
#include <cstdint>
+#include <memory>
#include <ignition/math/Helpers.hh>
namespace ignition
@@ -73,8 +74,17 @@ namespace ignition
/// \param[in] _sigma Sigma value for the distribution
public: static int32_t IntNormal(int _mean, int _sigma);
+#ifdef _WIN32
+// Disable warning C4251 which is triggered by
+// std::unique_ptr
+#pragma warning(push)
+#pragma warning(disable: 4251)
+#endif
/// \brief The random number generator.
- private: static GeneratorType *randGenerator;
+ private: static std::unique_ptr<GeneratorType> randGenerator;
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
/// \brief Random number seed.
private: static uint32_t seed;
diff --git a/include/ignition/math/RotationSpline.hh b/include/ignition/math/RotationSpline.hh
index ba0092e..f1638ab 100644
--- a/include/ignition/math/RotationSpline.hh
+++ b/include/ignition/math/RotationSpline.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_ROTATIONSPLINE_HH_
-#define _IGNITION_ROTATIONSPLINE_HH_
+#ifndef IGNITION_MATH_ROTATIONSPLINE_HH_
+#define IGNITION_MATH_ROTATIONSPLINE_HH_
#include <ignition/math/IndexException.hh>
#include <ignition/math/Quaternion.hh>
diff --git a/include/ignition/math/RotationSplinePrivate.hh b/include/ignition/math/RotationSplinePrivate.hh
index 68ea659..786a796 100644
--- a/include/ignition/math/RotationSplinePrivate.hh
+++ b/include/ignition/math/RotationSplinePrivate.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_ROTATIONSPLINE_PRIVATE_HH_
-#define _IGNITION_ROTATIONSPLINE_PRIVATE_HH_
+#ifndef IGNITION_MATH_ROTATIONSPLINE_PRIVATE_HH_
+#define IGNITION_MATH_ROTATIONSPLINE_PRIVATE_HH_
#include <vector>
#include "ignition/math/Quaternion.hh"
diff --git a/include/ignition/math/SignalStats.hh b/include/ignition/math/SignalStats.hh
index e66b38c..59fda74 100644
--- a/include/ignition/math/SignalStats.hh
+++ b/include/ignition/math/SignalStats.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_SIGNAL_STATS_HH_
-#define _IGNITION_SIGNAL_STATS_HH_
+#ifndef IGNITION_MATH_SIGNAL_STATS_HH_
+#define IGNITION_MATH_SIGNAL_STATS_HH_
#include <map>
#include <string>
@@ -216,6 +216,11 @@ namespace ignition
/// \brief Forget all previous data.
public: void Reset();
+ /// \brief Assignment operator
+ /// \param[in] _v A SignalStats to copy
+ /// \return this
+ public: SignalStats &operator=(const SignalStats &_s);
+
/// \brief Pointer to private data.
protected: SignalStatsPrivate *dataPtr;
};
diff --git a/include/ignition/math/SignalStatsPrivate.hh b/include/ignition/math/SignalStatsPrivate.hh
index befb00c..1c956e2 100644
--- a/include/ignition/math/SignalStatsPrivate.hh
+++ b/include/ignition/math/SignalStatsPrivate.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_SIGNAL_STATS_PRIVATE_HH_
-#define _IGNITION_SIGNAL_STATS_PRIVATE_HH_
+#ifndef IGNITION_MATH_SIGNAL_STATS_PRIVATE_HH_
+#define IGNITION_MATH_SIGNAL_STATS_PRIVATE_HH_
#include <memory>
#include <vector>
@@ -36,6 +36,13 @@ namespace ignition
/// \brief Count of data values in mean.
public: unsigned int count;
+
+ /// \brief Clone the SignalStatisticPrivate object. Used for implementing
+ /// copy semantics.
+ public: SignalStatisticPrivate *Clone() const
+ {
+ return new SignalStatisticPrivate(*this);
+ }
};
class SignalStatistic;
@@ -53,6 +60,13 @@ namespace ignition
{
/// \brief Vector of `SignalStatistic`s.
public: SignalStatistic_V stats;
+
+ /// \brief Clone the SignalStatsPrivate object. Used for implementing
+ /// copy semantics.
+ public: SignalStatsPrivate *Clone() const
+ {
+ return new SignalStatsPrivate(*this);
+ }
};
}
}
diff --git a/include/ignition/math/Spline.hh b/include/ignition/math/Spline.hh
index f22767a..b8dce43 100644
--- a/include/ignition/math/Spline.hh
+++ b/include/ignition/math/Spline.hh
@@ -16,8 +16,8 @@
*/
// Note: Originally cribbed from Ogre3d. Modified to implement Cardinal
// spline and catmull-rom spline
-#ifndef _IGNITION_SPLINE_HH_
-#define _IGNITION_SPLINE_HH_
+#ifndef IGNITION_MATH_SPLINE_HH_
+#define IGNITION_MATH_SPLINE_HH_
#include <ignition/math/Helpers.hh>
#include <ignition/math/IndexException.hh>
diff --git a/include/ignition/math/SplinePrivate.hh b/include/ignition/math/SplinePrivate.hh
index f1a4762..58399e2 100644
--- a/include/ignition/math/SplinePrivate.hh
+++ b/include/ignition/math/SplinePrivate.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_SPLINE_PRIVATE_HH_
-#define _IGNITION_SPLINE_PRIVATE_HH_
+#ifndef IGNITION_MATH_SPLINE_PRIVATE_HH_
+#define IGNITION_MATH_SPLINE_PRIVATE_HH_
#include <vector>
#include <ignition/math/Vector3.hh>
diff --git a/include/ignition/math/Temperature.hh b/include/ignition/math/Temperature.hh
new file mode 100644
index 0000000..5d756e7
--- /dev/null
+++ b/include/ignition/math/Temperature.hh
@@ -0,0 +1,375 @@
+/*
+ * Copyright (C) 2016 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_MATH_TEMPERATURE_HH_
+#define IGNITION_MATH_TEMPERATURE_HH_
+
+#include <iostream>
+#include <memory>
+
+#include "ignition/math/Helpers.hh"
+
+namespace ignition
+{
+ namespace math
+ {
+ // Forward declare private data class.
+ class TemperaturePrivate;
+
+ /// \brief A class that stores temperature information, and allows
+ /// conversion between different units.
+ ///
+ /// This class is mostly for convenience. It can be used to easily
+ /// convert between temperature units and encapsulate temperature values.
+ ///
+ /// The default unit is Kelvin. Most functions that accept a double
+ /// value will assume the double is Kelvin. The exceptions are a few of
+ /// the conversion functions, such as CelsiusToFahrenheit. Similarly,
+ /// most doubles that are returned will be in Kelvin.
+ ///
+ /// ## Example usage ##
+ ///
+ /// ### Convert from Kelvin to Celsius ###
+ ///
+ /// double celsius = ignition::math::Temperature::KelvinToCelsius(2.5);
+ ///
+ /// ### Create and use a Temperature object ###
+ ///
+ /// ignition::math::Temperature temp(123.5);
+ /// std::cout << "Temperature in Kelvin = " << temp << std::endl;
+ /// std::cout << "Temperature in Celsius = "
+ /// << temp.Celsius() << std::endl;
+ ///
+ /// temp += 100.0;
+ /// std::cout << "Temperature + 100.0 = " << temp << "K" << std::endl;
+ ///
+ /// ignition::math::Temperature newTemp(temp);
+ /// newTemp += temp + 23.5;
+ /// std::cout << "Copied the temp object and added 23.5K. newTemp = "
+ /// << newTemp.Fahrenheit() << "F" << std::endl;
+ ///
+ class IGNITION_VISIBLE Temperature
+ {
+ /// \brief Default constructor
+ public: Temperature();
+
+ /// \brief Kelvin value constructor. This is a conversion constructor
+ /// \param[in] _temp Temperature in Kelvin
+ // cppcheck-suppress noExplicitConstructor
+ public: Temperature(const double _temp);
+
+ /// \brief Copy constructor
+ /// \param[in] _temp Temperature object to copy.
+ public: Temperature(const Temperature &_temp);
+
+ /// \brief Destructor
+ public: virtual ~Temperature();
+
+ /// \brief Convert Kelvin to Celsius
+ /// \param[in] _temp Temperature in Kelvin
+ /// \return Temperature in Celsius
+ public: static double KelvinToCelsius(const double _temp);
+
+ /// \brief Convert Kelvin to Fahrenheit
+ /// \param[in] _temp Temperature in Kelvin
+ /// \return Temperature in Fahrenheit
+ public: static double KelvinToFahrenheit(const double _temp);
+
+ /// \brief Convert Celsius to Fahrenheit
+ /// \param[in] _temp Temperature in Celsius
+ /// \return Temperature in Fahrenheit
+ public: static double CelsiusToFahrenheit(const double _temp);
+
+ /// \brief Convert Celsius to Kelvin
+ /// \param[in] _temp Temperature in Celsius
+ /// \return Temperature in Kelvin
+ public: static double CelsiusToKelvin(const double _temp);
+
+ /// \brief Convert Fahrenheit to Celsius
+ /// \param[in] _temp Temperature in Fahrenheit
+ /// \return Temperature in Celsius
+ public: static double FahrenheitToCelsius(const double _temp);
+
+ /// \brief Convert Fahrenheit to Kelvin
+ /// \param[in] _temp Temperature in Fahrenheit
+ /// \return Temperature in Kelvin
+ public: static double FahrenheitToKelvin(const double _temp);
+
+ /// \brief Set the temperature from a Kelvin value
+ /// \param[in] _temp Temperature in Kelvin
+ public: void SetKelvin(const double _temp);
+
+ /// \brief Set the temperature from a Celsius value
+ /// \param[in] _temp Temperature in Celsius
+ public: void SetCelsius(const double _temp);
+
+ /// \brief Set the temperature from a Fahrenheit value
+ /// \param[in] _temp Temperature in Fahrenheit
+ public: void SetFahrenheit(const double _temp);
+
+ /// \brief Get the temperature in Kelvin
+ /// \return Temperature in Kelvin
+ public: double Kelvin() const;
+
+ /// \brief Get the temperature in Celsius
+ /// \return Temperature in Celsius
+ public: double Celsius() const;
+
+ /// \brief Get the temperature in Fahrenheit
+ /// \return Temperature in Fahrenheit
+ public: double Fahrenheit() const;
+
+ /// \brief Accessor operator
+ /// \return Temperature in Kelvin
+ /// \sa Kelvin()
+ public: double operator()() const;
+
+ /// \brief Assignment operator
+ /// \param[in] _temp Temperature in Kelvin
+ /// \return Reference to this instance
+ public: Temperature &operator=(const double _temp);
+
+ /// \brief Assignment operator
+ /// \param[in] _temp Temperature object
+ /// \return Reference to this instance
+ public: Temperature &operator=(const Temperature &_temp);
+
+ /// \brief Addition operator
+ /// \param[in] _temp Temperature in Kelvin
+ /// \return Resulting temperature
+ public: Temperature operator+(const double _temp);
+
+ /// \brief Addition operator
+ /// \param[in] _temp Temperature object
+ /// \return Resulting temperature
+ public: Temperature operator+(const Temperature &_temp);
+
+ /// \brief Addition operator for double type.
+ /// \param[in] _t Temperature in kelvin
+ /// \param[in] _temp Temperature object
+ /// \return Resulting temperature
+ public: friend Temperature operator+(double _t, const Temperature &_temp)
+ {
+ return _t + _temp.Kelvin();
+ }
+
+ /// \brief Addition assignment operator
+ /// \param[in] _temp Temperature in Kelvin
+ /// \return Reference to this instance
+ public: const Temperature &operator+=(const double _temp);
+
+ /// \brief Addition assignment operator
+ /// \param[in] _temp Temperature object
+ /// \return Reference to this instance
+ public: const Temperature &operator+=(const Temperature &_temp);
+
+ /// \brief Subtraction operator
+ /// \param[in] _temp Temperature in Kelvin
+ /// \return Resulting temperature
+ public: Temperature operator-(const double _temp);
+
+ /// \brief Subtraction operator
+ /// \param[in] _temp Temperature object
+ /// \return Resulting temperature
+ public: Temperature operator-(const Temperature &_temp);
+
+ /// \brief Subtraction operator for double type.
+ /// \param[in] _t Temperature in kelvin
+ /// \param[in] _temp Temperature object
+ /// \return Resulting temperature
+ public: friend Temperature operator-(double _t, const Temperature &_temp)
+ {
+ return _t - _temp.Kelvin();
+ }
+
+ /// \brief Subtraction assignment operator
+ /// \param[in] _temp Temperature in Kelvin
+ /// \return Reference to this instance
+ public: const Temperature &operator-=(const double _temp);
+
+ /// \brief Subtraction assignment operator
+ /// \param[in] _temp Temperature object
+ /// \return Reference to this instance
+ public: const Temperature &operator-=(const Temperature &_temp);
+
+ /// \brief Multiplication operator
+ /// \param[in] _temp Temperature in Kelvin
+ /// \return Resulting temperature
+ public: Temperature operator*(const double _temp);
+
+ /// \brief Multiplication operator
+ /// \param[in] _temp Temperature object
+ /// \return Resulting temperature
+ public: Temperature operator*(const Temperature &_temp);
+
+ /// \brief Multiplication operator for double type.
+ /// \param[in] _t Temperature in kelvin
+ /// \param[in] _temp Temperature object
+ /// \return Resulting temperature
+ public: friend Temperature operator*(double _t, const Temperature &_temp)
+ {
+ return _t * _temp.Kelvin();
+ }
+
+ /// \brief Multiplication assignment operator
+ /// \param[in] _temp Temperature in Kelvin
+ /// \return Reference to this instance
+ public: const Temperature &operator*=(const double _temp);
+
+ /// \brief Multiplication assignment operator
+ /// \param[in] _temp Temperature object
+ /// \return Reference to this instance
+ public: const Temperature &operator*=(const Temperature &_temp);
+
+ /// \brief Division operator
+ /// \param[in] _temp Temperature in Kelvin
+ /// \return Resulting temperature
+ public: Temperature operator/(const double _temp);
+
+ /// \brief Division operator
+ /// \param[in] _temp Temperature object
+ /// \return Resulting temperature
+ public: Temperature operator/(const Temperature &_temp);
+
+ /// \brief Division operator for double type.
+ /// \param[in] _t Temperature in kelvin
+ /// \param[in] _temp Temperature object
+ /// \return Resulting temperature
+ public: friend Temperature operator/(double _t, const Temperature &_temp)
+ {
+ return _t / _temp.Kelvin();
+ }
+
+ /// \brief Division assignment operator
+ /// \param[in] _temp Temperature in Kelvin
+ /// \return Reference to this instance
+ public: const Temperature &operator/=(const double _temp);
+
+ /// \brief Division assignment operator
+ /// \param[in] _temp Temperature object
+ /// \return Reference to this instance
+ public: const Temperature &operator/=(const Temperature &_temp);
+
+ /// \brief Equal to operator
+ /// \param[in] _temp The temperature to compare
+ /// \return true if the temperatures are the same, false otherwise
+ public: bool operator==(const Temperature &_temp) const;
+
+ /// \brief Equal to operator, where the value of _temp is assumed to
+ /// be in Kelvin
+ /// \param[in] _temp The temperature (in Kelvin) to compare
+ /// \return true if the temperatures are the same, false otherwise
+ public: bool operator==(const double _temp) const;
+
+ /// \brief Inequality to operator
+ /// \param[in] _temp The temperature to compare
+ /// \return false if the temperatures are the same, true otherwise
+ public: bool operator!=(const Temperature &_temp) const;
+
+ /// \brief Inequality to operator, where the value of _temp is assumed to
+ /// be in Kelvin
+ /// \param[in] _temp The temperature (in Kelvin) to compare
+ /// \return false if the temperatures are the same, true otherwise
+ public: bool operator!=(const double _temp) const;
+
+ /// \brief Less than to operator
+ /// \param[in] _temp The temperature to compare
+ /// \return True if this is less than _temp.
+ public: bool operator<(const Temperature &_temp) const;
+
+ /// \brief Less than operator, where the value of _temp is assumed to
+ /// be in Kelvin
+ /// \param[in] _temp The temperature (in Kelvin) to compare
+ /// \return True if this is less than _temp.
+ public: bool operator<(const double _temp) const;
+
+ /// \brief Less than or equal to operator
+ /// \param[in] _temp The temperature to compare
+ /// \return True if this is less than or equal _temp.
+ public: bool operator<=(const Temperature &_temp) const;
+
+ /// \brief Less than or equal operator,
+ /// where the value of _temp is assumed to be in Kelvin
+ /// \param[in] _temp The temperature (in Kelvin) to compare
+ /// \return True if this is less than or equal to _temp.
+ public: bool operator<=(const double _temp) const;
+
+ /// \brief Greater than operator
+ /// \param[in] _temp The temperature to compare
+ /// \return True if this is greater than _temp.
+ public: bool operator>(const Temperature &_temp) const;
+
+ /// \brief Greater than operator, where the value of _temp is assumed to
+ /// be in Kelvin
+ /// \param[in] _temp The temperature (in Kelvin) to compare
+ /// \return True if this is greater than _temp.
+ public: bool operator>(const double _temp) const;
+
+ /// \brief Greater than or equal to operator
+ /// \param[in] _temp The temperature to compare
+ /// \return True if this is greater than or equal to _temp.
+ public: bool operator>=(const Temperature &_temp) const;
+
+ /// \brief Greater than equal operator,
+ /// where the value of _temp is assumed to be in Kelvin
+ /// \param[in] _temp The temperature (in Kelvin) to compare
+ /// \return True if this is greater than or equal to _temp.
+ public: bool operator>=(const double _temp) const;
+
+ /// \brief Stream insertion operator
+ /// \param[in] _out the output stream
+ /// \param[in] _temp Temperature to write to the stream
+ /// \return the output stream
+ public: friend std::ostream &operator<<(std::ostream &_out,
+ const ignition::math::Temperature &_temp)
+ {
+ _out << _temp.Kelvin();
+ return _out;
+ }
+
+ /// \brief Stream extraction operator
+ /// \param[in] _in the input stream
+ /// \param[in] _temp Temperature to read from to the stream. Assumes
+ /// temperature value is in Kelvin.
+ /// \return the input stream
+ public: friend std::istream &operator>>(std::istream &_in,
+ ignition::math::Temperature &_temp)
+ {
+ // Skip white spaces
+ _in.setf(std::ios_base::skipws);
+
+ double kelvin;
+ _in >> kelvin;
+
+ _temp.SetKelvin(kelvin);
+ return _in;
+ }
+
+#ifdef _WIN32
+// Disable warning C4251 which is triggered by
+// std::unique_ptr
+#pragma warning(push)
+#pragma warning(disable: 4251)
+#endif
+ /// \brief Private data pointer.
+ private: std::unique_ptr<TemperaturePrivate> dataPtr;
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
+ };
+ }
+}
+#endif
diff --git a/include/ignition/math/Triangle.hh b/include/ignition/math/Triangle.hh
index 8de58d8..8d4d5a4 100644
--- a/include/ignition/math/Triangle.hh
+++ b/include/ignition/math/Triangle.hh
@@ -15,8 +15,8 @@
*
*/
-#ifndef _IGNITION_TRIANGLE_HH_
-#define _IGNITION_TRIANGLE_HH_
+#ifndef IGNITION_MATH_TRIANGLE_HH_
+#define IGNITION_MATH_TRIANGLE_HH_
#include <set>
#include <ignition/math/Line2.hh>
diff --git a/include/ignition/math/Triangle3.hh b/include/ignition/math/Triangle3.hh
deleted file mode 100644
index 69b68fd..0000000
--- a/include/ignition/math/Triangle3.hh
+++ /dev/null
@@ -1,285 +0,0 @@
-/*
- * 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_TRIANGLE3_HH_
-#define _IGNITION_TRIANGLE3_HH_
-
-#include <ignition/math/Line3.hh>
-#include <ignition/math/Plane.hh>
-#include <ignition/math/Vector3.hh>
-#include <ignition/math/IndexException.hh>
-
-namespace ignition
-{
- namespace math
- {
- /// \class Triangle3 Triangle3.hh ignition/math/Triangle3.hh
- /// \brief A 3-dimensional triangle and related functions.
- template<typename T>
- class Triangle3
- {
- /// \brief Default constructor
- public: Triangle3() = default;
-
- /// \brief Constructor.
- ///
- /// Keep in mind that the triangle normal
- /// is determined by the order of these vertices. Search
- /// the internet for "triangle winding" for more information.
- /// \param[in] _pt1 First point that defines the triangle.
- /// \param[in] _pt2 Second point that defines the triangle.
- /// \param[in] _pt3 Third point that defines the triangle.
- public: Triangle3(const Vector3<T> &_pt1,
- const Vector3<T> &_pt2,
- const Vector3<T> &_pt3)
- {
- this->Set(_pt1, _pt2, _pt3);
- }
-
- /// \brief Set one vertex of the triangle.
- ///
- /// Keep in mind that the triangle normal
- /// is determined by the order of these vertices. Search
- /// the internet for "triangle winding" for more information.
- ///
- /// \param[in] _index Index of the point to set.
- /// \param[in] _pt Value of the point to set.
- /// \throws IndexException if _index is > 2.
- public: void Set(const unsigned int _index, const Vector3<T> &_pt)
- {
- if (_index > 2)
- throw IndexException();
- else
- this->pts[_index] = _pt;
- }
-
- /// \brief Set all vertices of the triangle.
- ///
- /// Keep in mind that the triangle normal
- /// is determined by the order of these vertices. Search
- /// the internet for "triangle winding" for more information.
- ///
- /// \param[in] _pt1 First point that defines the triangle.
- /// \param[in] _pt2 Second point that defines the triangle.
- /// \param[in] _pt3 Third point that defines the triangle.
- public: void Set(const Vector3<T> &_pt1,
- const Vector3<T> &_pt2,
- const Vector3<T> &_pt3)
- {
- this->pts[0] = _pt1;
- this->pts[1] = _pt2;
- this->pts[2] = _pt3;
- }
-
- /// \brief Get whether this triangle is valid, based on triangle
- /// inequality: the sum of the lengths of any two sides must be greater
- /// than the length of the remaining side.
- /// \return True if the triangle inequality holds
- public: bool Valid() const
- {
- T a = this->Side(0).Length();
- T b = this->Side(1).Length();
- T c = this->Side(2).Length();
- return (a+b) > c && (b+c) > a && (c+a) > b;
- }
-
- /// \brief Get a line segment for one side of the triangle.
- /// \param[in] _index Index of the side to retrieve, where
- /// 0 == Line3(pt1, pt2),
- /// 1 == Line3(pt2, pt3),
- /// 2 == Line3(pt3, pt1)
- /// \return Line segment of the requested side.
- /// \throws IndexException if _index is > 2.
- public: Line3<T> Side(const unsigned int _index) const
- {
- if (_index > 2)
- throw IndexException();
- else if (_index == 0)
- return Line3<T>(this->pts[0], this->pts[1]);
- else if (_index == 1)
- return Line3<T>(this->pts[1], this->pts[2]);
- else
- return Line3<T>(this->pts[2], this->pts[0]);
- }
-
- /// \brief Check if this triangle completely contains the given line
- /// segment.
- /// \param[in] _line Line to check.
- /// \return True if the line's start and end points are both inside
- /// this triangle.
- public: bool Contains(const Line3<T> &_line) const
- {
- return this->Contains(_line[0]) && this->Contains(_line[1]);
- }
-
- /// \brief Get whether this triangle contains the given point.
- /// \param[in] _pt Point to check.
- /// \return True if the point is inside or on the triangle.
- public: bool Contains(const Vector3<T> &_pt) const
- {
- // Make sure the point is on the same plane as the triangle
- if (Planed(this->Normal()).Side(_pt) == Planed::NO_SIDE)
- {
- Vector3d v0 = this->pts[2] - this->pts[0];
- Vector3d v1 = this->pts[1] - this->pts[0];
- Vector3d v2 = _pt - this->pts[0];
-
- double dot00 = v0.Dot(v0);
- double dot01 = v0.Dot(v1);
- double dot02 = v0.Dot(v2);
- double dot11 = v1.Dot(v1);
- double dot12 = v1.Dot(v2);
-
- // Compute barycentric coordinates
- double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
- double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
- double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
-
- // Check if point is in triangle
- return (u >= 0) && (v >= 0) && (u + v <= 1);
- }
- else
- {
- return false;
- }
- }
-
- /// \brief Get the triangle's normal vector.
- /// \return The normal vector for the triangle.
- public: Vector3d Normal() const
- {
- return Vector3d::Normal(this->pts[0], this->pts[1], this->pts[2]);
- }
-
- /// \brief Get whether the given line intersects an edge of this triangle.
- ///
- /// The returned intersection point is one of:
- ///
- /// * If the line is coplanar with the triangle:
- /// * The point on the closest edge of the triangle that the line
- /// intersects.
- /// OR
- /// * The first point on the line, if the line is completely contained
- /// * If the line is not coplanar, the point on the triangle that the
- /// line intersects.
- ///
- /// \param[in] _line Line to check.
- /// \param[out] _ipt1 Return value of the first intersection point,
- /// only valid if the return value of the function is true.
- /// \return True if the given line intersects this triangle.
- public: bool Intersects(const Line3<T> &_line, Vector3<T> &_ipt1) const
- {
- // Triangle normal
- Vector3d norm = this->Normal();
-
- // Ray direction to intersect with triangle
- Vector3d dir = (_line[1] - _line[0]).Normalize();
-
- double denom = norm.Dot(dir);
-
- // Handle the case when the line is not co-planar with the triangle
- if (!math::equal(denom, 0.0))
- {
- // Distance from line start to triangle intersection
- double intersection =
- -norm.Dot(_line[0] - this->pts[0]) / denom;
-
- // Make sure the ray intersects the triangle
- if (intersection < 1.0 || intersection > _line.Length())
- return false;
-
- // Return point of intersection
- _ipt1 = _line[0] + (dir * intersection);
-
- return true;
- }
- // Line co-planar with triangle
- else
- {
- // If the line is completely inside the triangle
- if (this->Contains(_line))
- {
- _ipt1 = _line[0];
- return true;
- }
- // If the line intersects the first side
- else if (_line.Intersect(this->Side(0), _ipt1))
- {
- return true;
- }
- // If the line intersects the second side
- else if (_line.Intersect(this->Side(1), _ipt1))
- {
- return true;
- }
- // If the line intersects the third side
- else if (_line.Intersect(this->Side(2), _ipt1))
- {
- return true;
- }
- }
-
- return false;
- }
-
- /// \brief Get the length of the triangle's perimeter.
- /// \return Sum of the triangle's line segments.
- public: T Perimeter() const
- {
- return this->Side(0).Length() + this->Side(1).Length() +
- this->Side(2).Length();
- }
-
- /// \brief Get the area of this triangle.
- /// \return Triangle's area.
- public: double Area() const
- {
- double s = this->Perimeter() / 2.0;
- T a = this->Side(0).Length();
- T b = this->Side(1).Length();
- T c = this->Side(2).Length();
-
- // Heron's formula
- // http://en.wikipedia.org/wiki/Heron%27s_formula
- return sqrt(s * (s-a) * (s-b) * (s-c));
- }
-
- /// \brief Get one of points that define the triangle.
- /// \param[in] _index: 0, 1, or 2.
- /// \throws IndexException if _index is > 2.
- public: Vector3<T> operator[](size_t _index) const
- {
- if (_index > 2)
- throw IndexException();
- return this->pts[_index];
- }
-
- /// The points of the triangle
- private: Vector3<T> pts[3];
- };
-
- /// \brief Integer specialization of the Triangle class.
- typedef Triangle3<int> Triangle3i;
-
- /// \brief Double specialization of the Triangle class.
- typedef Triangle3<double> Triangle3d;
-
- /// \brief Float specialization of the Triangle class.
- typedef Triangle3<float> Triangle3f;
- }
-}
-#endif
diff --git a/include/ignition/math/Vector2.hh b/include/ignition/math/Vector2.hh
index f19722f..e770fc8 100644
--- a/include/ignition/math/Vector2.hh
+++ b/include/ignition/math/Vector2.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_VECTOR2_HH_
-#define _IGNITION_VECTOR2_HH_
+#ifndef IGNITION_MATH_VECTOR2_HH_
+#define IGNITION_MATH_VECTOR2_HH_
#include <ignition/math/IndexException.hh>
@@ -439,7 +439,7 @@ namespace ignition
/// \brief Less than operator.
/// \param[in] _pt Vector to compare.
- /// \return True if this vector's first or second value is less than
+ /// \return True if this vector2 first or second value is less than
/// the given vector's first or second value.
public: bool operator<(const Vector2<T> &_pt) const
{
diff --git a/include/ignition/math/Vector3.hh b/include/ignition/math/Vector3.hh
index d6bfea8..735b69a 100644
--- a/include/ignition/math/Vector3.hh
+++ b/include/ignition/math/Vector3.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_VECTOR3_HH_
-#define _IGNITION_VECTOR3_HH_
+#ifndef IGNITION_MATH_VECTOR3_HH_
+#define IGNITION_MATH_VECTOR3_HH_
#include <iostream>
#include <fstream>
@@ -141,6 +141,15 @@ namespace ignition
return *this;
}
+ /// \brief Return a normalized vector
+ /// \return unit length vector
+ public: Vector3 Normalized() const
+ {
+ Vector3<T> result = *this;
+ result.Normalize();
+ return result;
+ }
+
/// \brief Round to near whole number, return the result.
/// \return the result
public: Vector3 Round()
@@ -688,16 +697,6 @@ namespace ignition
this->data[2] = _v;
}
- /// \brief Less than operator.
- /// \param[in] _pt Vector to compare.
- /// \return True if this vector's X(), Y(), or Z() value is less
- /// than the given vector's corresponding values.
- public: bool operator<(const Vector3<T> &_pt) const
- {
- return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
- this->data[2] < _pt[2];
- }
-
/// \brief Stream insertion operator
/// \param _out output stream
/// \param _pt Vector3 to output
diff --git a/include/ignition/math/Vector3Stats.hh b/include/ignition/math/Vector3Stats.hh
index 9879063..1ec6644 100644
--- a/include/ignition/math/Vector3Stats.hh
+++ b/include/ignition/math/Vector3Stats.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_VECTOR3_STATS_HH_
-#define _IGNITION_VECTOR3_STATS_HH_
+#ifndef IGNITION_MATH_VECTOR3_STATS_HH_
+#define IGNITION_MATH_VECTOR3_STATS_HH_
#include <string>
#include <ignition/math/Helpers.hh>
diff --git a/include/ignition/math/Vector3StatsPrivate.hh b/include/ignition/math/Vector3StatsPrivate.hh
index e68f60d..7ab2a7e 100644
--- a/include/ignition/math/Vector3StatsPrivate.hh
+++ b/include/ignition/math/Vector3StatsPrivate.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_VECTOR3_STATS_PRIVATE_HH_
-#define _IGNITION_VECTOR3_STATS_PRIVATE_HH_
+#ifndef IGNITION_MATH_VECTOR3_STATS_PRIVATE_HH_
+#define IGNITION_MATH_VECTOR3_STATS_PRIVATE_HH_
#include <ignition/math/SignalStats.hh>
diff --git a/include/ignition/math/Vector4.hh b/include/ignition/math/Vector4.hh
index 4a53494..78d51cf 100644
--- a/include/ignition/math/Vector4.hh
+++ b/include/ignition/math/Vector4.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _IGNITION_VECTOR4_HH_
-#define _IGNITION_VECTOR4_HH_
+#ifndef IGNITION_MATH_VECTOR4_HH_
+#define IGNITION_MATH_VECTOR4_HH_
#include <ignition/math/Matrix4.hh>
diff --git a/src/Angle_TEST.cc b/src/Angle_TEST.cc
index 936396d..3c16bb2 100644
--- a/src/Angle_TEST.cc
+++ b/src/Angle_TEST.cc
@@ -15,7 +15,6 @@
*
*/
-#define _USE_MATH_DEFINES
#include <gtest/gtest.h>
#include <cmath>
@@ -109,3 +108,12 @@ TEST(AngleTest, StreamExtraction)
stream >> angle;
EXPECT_NEAR(*angle, 1.25, 1e-2);
}
+
+/////////////////////////////////////////////////
+TEST(AngleTest, OperatorStreamOut)
+{
+ math::Angle a(0.1);
+ std::ostringstream stream;
+ stream << a;
+ EXPECT_EQ(stream.str(), "0.1");
+}
diff --git a/src/Box.cc b/src/Box.cc
index 3545280..1d488bb 100644
--- a/src/Box.cc
+++ b/src/Box.cc
@@ -239,3 +239,94 @@ bool Box::Contains(const Vector3d &_p) const
_p.Y() >= this->dataPtr->min.Y() && _p.Y() <= this->dataPtr->max.Y() &&
_p.Z() >= this->dataPtr->min.Z() && _p.Z() <= this->dataPtr->max.Z();
}
+
+//////////////////////////////////////////////////
+bool Box::ClipLine(const int _d, const Line3d &_line,
+ double &_low, double &_high) const
+{
+ // dimLow and dimHigh are the results we're calculating for this
+ // current dimension.
+ double dimLow, dimHigh;
+
+ // Find the point of intersection in this dimension only as a fraction of
+ // the total vector http://youtu.be/USjbg5QXk3g?t=3m12s
+ dimLow = (this->dataPtr->min[_d] - _line[0][_d]) /
+ (_line[1][_d] - _line[0][_d]);
+
+ dimHigh = (this->dataPtr->max[_d] - _line[0][_d]) /
+ (_line[1][_d] - _line[0][_d]);
+
+ // Make sure low is less than high
+ if (dimHigh < dimLow)
+ std::swap(dimHigh, dimLow);
+
+ // If this dimension's high is less than the low we got then we definitely
+ // missed. http://youtu.be/USjbg5QXk3g?t=7m16s
+ if (dimHigh < _low)
+ return false;
+
+ // Likewise if the low is less than the high.
+ if (dimLow > _high)
+ return false;
+
+ // Add the clip from this dimension to the previous results
+ // http://youtu.be/USjbg5QXk3g?t=5m32s
+ if (std::isfinite(dimLow))
+ _low = std::max(dimLow, _low);
+
+ if (std::isfinite(dimHigh))
+ _high = std::min(dimHigh, _high);
+
+ return true;
+}
+
+/////////////////////////////////////////////////
+bool Box::IntersectCheck(const Vector3d &_origin, const Vector3d &_dir,
+ const double _min, const double _max) const
+{
+ return std::get<0>(this->Intersect(_origin, _dir, _min, _max));
+}
+
+/////////////////////////////////////////////////
+std::tuple<bool, double> Box::IntersectDist(const Vector3d &_origin,
+ const Vector3d &_dir, const double _min, const double _max) const
+{
+ return std::make_tuple(
+ std::get<0>(this->Intersect(_origin, _dir, _min, _max)),
+ std::get<1>(this->Intersect(_origin, _dir, _min, _max)));
+}
+
+/////////////////////////////////////////////////
+std::tuple<bool, double, Vector3d> Box::Intersect(
+ const Vector3d &_origin, const Vector3d &_dir,
+ const double _min, const double _max) const
+{
+ Vector3d dir = _dir;
+ dir.Normalize();
+ return this->Intersect(Line3d(_origin + dir * _min, _origin + dir * _max));
+}
+
+/////////////////////////////////////////////////
+// Find the intersection of a line from v0 to v1 and an
+// axis-aligned bounding box http://www.youtube.com/watch?v=USjbg5QXk3g
+std::tuple<bool, double, Vector3d> Box::Intersect(const Line3d &_line) const
+{
+ // low and high are the results from all clipping so far.
+ // We'll write our results back out to those parameters.
+ double low = 0;
+ double high = 1;
+
+ if (!this->ClipLine(0, _line, low, high))
+ return std::make_tuple(false, 0, Vector3d::Zero);
+
+ if (!this->ClipLine(1, _line, low, high))
+ return std::make_tuple(false, 0, Vector3d::Zero);
+
+ if (!this->ClipLine(2, _line, low, high))
+ return std::make_tuple(false, 0, Vector3d::Zero);
+
+ // The formula for I: http://youtu.be/USjbg5QXk3g?t=6m24s
+ Vector3d intersection = _line[0] + ((_line[1] - _line[0]) * low);
+
+ return std::make_tuple(true, _line[0].Distance(intersection), intersection);
+}
diff --git a/src/BoxPrivate.cc b/src/BoxPrivate.cc
deleted file mode 100644
index 8f7318b..0000000
--- a/src/BoxPrivate.cc
+++ /dev/null
@@ -1,27 +0,0 @@
-/*
- * 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 "ignition/math/BoxPrivate.hh"
-
-using namespace ignition;
-using namespace math;
-
-//////////////////////////////////////////////////
-BoxPrivate::BoxPrivate()
-: min(0, 0, 0), max(0, 0, 0), extent(EXTENT_NULL)
-{
-}
diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc
index 5945574..8c11efc 100644
--- a/src/Box_TEST.cc
+++ b/src/Box_TEST.cc
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-
#include <gtest/gtest.h>
+#include <cmath>
#include "ignition/math/Box.hh"
@@ -288,3 +288,220 @@ TEST(BoxTest, OperatorStreamOut)
stream << b;
EXPECT_EQ(stream.str(), "Min[0.1 1.2 2.3] Max[1.1 2.2 4.3]");
}
+
+/////////////////////////////////////////////////
+TEST(BoxTest, Intersect)
+{
+ math::Box b(0, 0, 0, 1, 1, 1);
+
+ bool intersect = false;
+ double dist = 0;
+ math::Vector3d pt;
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(-1, 0, 0),
+ math::Vector3d(1, 0, 0), 0, 1000);
+ EXPECT_TRUE(intersect);
+ EXPECT_TRUE(b.IntersectCheck(math::Vector3d(-1, 0, 0),
+ math::Vector3d(1, 0, 0), 0, 1000));
+ EXPECT_DOUBLE_EQ(dist, 1);
+ EXPECT_DOUBLE_EQ(std::get<1>(b.Intersect(math::Vector3d(-1, 0, 0),
+ math::Vector3d(1, 0, 0), 0, 1000)), dist);
+ EXPECT_EQ(pt, math::Vector3d::Zero);
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(1, 0, 0),
+ math::Vector3d(-1, 0, 0), 0, 1000);
+ EXPECT_TRUE(intersect);
+ EXPECT_TRUE(b.IntersectCheck(math::Vector3d(1, 0, 0),
+ math::Vector3d(-1, 0, 0), 0, 1000));
+ EXPECT_DOUBLE_EQ(dist, 0);
+ EXPECT_DOUBLE_EQ(std::get<1>(b.IntersectDist(math::Vector3d(1, 0, 0),
+ math::Vector3d(-1, 0, 0), 0, 1000)), dist);
+ EXPECT_EQ(pt, math::Vector3d(1, 0, 0));
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(2, 2, 0),
+ math::Vector3d(-1, -1, 0), 0, 1000);
+ EXPECT_TRUE(intersect);
+ EXPECT_TRUE(b.IntersectCheck(math::Vector3d(2, 2, 0),
+ math::Vector3d(-1, -1, 0), 0, 1000));
+ EXPECT_DOUBLE_EQ(dist, IGN_SQRT2);
+ EXPECT_DOUBLE_EQ(std::get<1>(b.IntersectDist(math::Vector3d(2, 2, 0),
+ math::Vector3d(-1, -1, 0), 0, 1000)), dist);
+ EXPECT_EQ(pt, math::Vector3d(1, 1, 0));
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(-10, -10, 0),
+ math::Vector3d(1, 1, 0), 0, 1000);
+ EXPECT_TRUE(intersect);
+ EXPECT_TRUE(b.IntersectCheck(math::Vector3d(-10, -10, 0),
+ math::Vector3d(1, 1, 0), 0, 1000));
+ EXPECT_DOUBLE_EQ(dist, std::sqrt(200));
+ EXPECT_DOUBLE_EQ(std::get<1>(b.IntersectDist(math::Vector3d(-10, -10, 0),
+ math::Vector3d(1, 1, 0), 0, 1000)), dist);
+ EXPECT_EQ(pt, math::Vector3d::Zero);
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(-1, -2, 0),
+ math::Vector3d(1, 1, 0), 0, 1000);
+ EXPECT_TRUE(intersect);
+ EXPECT_TRUE(b.IntersectCheck(math::Vector3d(-1, -2, 0),
+ math::Vector3d(1, 1, 0), 0, 1000));
+ EXPECT_DOUBLE_EQ(dist, 2*IGN_SQRT2);
+ EXPECT_DOUBLE_EQ(std::get<1>(b.IntersectDist(math::Vector3d(-1, -2, 0),
+ math::Vector3d(1, 1, 0), 0, 1000)), dist);
+ EXPECT_EQ(pt, math::Vector3d(1, 0, 0));
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(2, 1, 0),
+ math::Vector3d(-1, -1, 0), 0, 1000);
+ EXPECT_TRUE(intersect);
+ EXPECT_TRUE(b.IntersectCheck(math::Vector3d(2, 1, 0),
+ math::Vector3d(-1, -1, 0), 0, 1000));
+ EXPECT_DOUBLE_EQ(dist, IGN_SQRT2);
+ EXPECT_DOUBLE_EQ(std::get<1>(b.IntersectDist(math::Vector3d(2, 1, 0),
+ math::Vector3d(-1, -1, 0), 0, 1000)), dist);
+ EXPECT_EQ(pt, math::Vector3d(1, 0, 0));
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(0.5, 0.5, 2),
+ math::Vector3d(0, 0, -1), 0, 1000);
+ EXPECT_TRUE(intersect);
+ EXPECT_TRUE(b.IntersectCheck(math::Vector3d(0.5, 0.5, 2),
+ math::Vector3d(0, 0, -1), 0, 1000));
+ EXPECT_DOUBLE_EQ(dist, 1);
+ EXPECT_DOUBLE_EQ(std::get<1>(b.IntersectDist(math::Vector3d(0.5, 0.5, 2),
+ math::Vector3d(0, 0, -1), 0, 1000)), dist);
+ EXPECT_EQ(pt, math::Vector3d(0.5, 0.5, 1));
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(0.5, 0.5, 2),
+ math::Vector3d(0, 0, 1), 0, 1000);
+ EXPECT_FALSE(intersect);
+ EXPECT_FALSE(b.IntersectCheck(math::Vector3d(0.5, 0.5, 2),
+ math::Vector3d(0, 0, 1), 0, 1000));
+ EXPECT_EQ(pt, math::Vector3d::Zero);
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(-1, -1, 1),
+ math::Vector3d(0, 0, -1), 0, 1000);
+ EXPECT_FALSE(intersect);
+ EXPECT_FALSE(b.IntersectCheck(math::Vector3d(-1, -1, 1),
+ math::Vector3d(0, 0, -1), 0, 1000));
+ EXPECT_EQ(pt, math::Vector3d::Zero);
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(2, 2, 0),
+ math::Vector3d(1, 1, 0), 0, 1000);
+ EXPECT_FALSE(intersect);
+ EXPECT_FALSE(b.IntersectCheck(math::Vector3d(2, 2, 0),
+ math::Vector3d(1, 1, 0), 0, 1000));
+ EXPECT_EQ(pt, math::Vector3d::Zero);
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(2, 2, 0),
+ math::Vector3d(0, 1, 0), 0, 1000);
+ EXPECT_FALSE(intersect);
+ EXPECT_FALSE(b.IntersectCheck(math::Vector3d(2, 2, 0),
+ math::Vector3d(0, 1, 0), 0, 1000));
+ EXPECT_EQ(pt, math::Vector3d::Zero);
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(0.1, 0.1, 200),
+ math::Vector3d(0, 0, -1), 0, 100);
+ EXPECT_FALSE(intersect);
+ EXPECT_FALSE(b.IntersectCheck(math::Vector3d(0.1, 0.1, 200),
+ math::Vector3d(0, 0, -1), 0, 100));
+ EXPECT_DOUBLE_EQ(dist, 0);
+ EXPECT_DOUBLE_EQ(std::get<1>(b.IntersectDist(math::Vector3d(0.1, 0.1, 200),
+ math::Vector3d(0, 0, -1), 0, 100)), dist);
+ EXPECT_EQ(pt, math::Vector3d::Zero);
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(0.1, 0.1, 1),
+ math::Vector3d(0, 0, -1), 1.0, 1000);
+ EXPECT_TRUE(intersect);
+ EXPECT_TRUE(b.IntersectCheck(math::Vector3d(0.1, 0.1, 1),
+ math::Vector3d(0, 0, -1), 1.0, 1000));
+ EXPECT_DOUBLE_EQ(dist, 0.0);
+ EXPECT_DOUBLE_EQ(std::get<1>(b.IntersectDist(math::Vector3d(0.1, 0.1, 1),
+ math::Vector3d(0, 0, -1), 1.0, 1000)), dist);
+ EXPECT_EQ(pt, math::Vector3d(0.1, 0.1, 0));
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(0.1, 0.1, 1),
+ math::Vector3d(0, 0, -1), 1.1, 1000);
+ EXPECT_FALSE(intersect);
+ EXPECT_FALSE(b.IntersectCheck(math::Vector3d(0.1, 0.1, 1),
+ math::Vector3d(0, 0, -1), 1.1, 1000));
+ EXPECT_EQ(pt, math::Vector3d::Zero);
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(0.1, 0.1, 10),
+ math::Vector3d(0, 0, -1), 1.1, 5);
+ EXPECT_FALSE(intersect);
+ EXPECT_FALSE(b.IntersectCheck(math::Vector3d(0.1, 0.1, 10),
+ math::Vector3d(0, 0, -1), 1.1, 5));
+ EXPECT_DOUBLE_EQ(dist, 0);
+ EXPECT_DOUBLE_EQ(std::get<1>(b.IntersectDist(math::Vector3d(0.1, 0.1, 10),
+ math::Vector3d(0, 0, -1), 1.1, 5)), dist);
+ EXPECT_EQ(pt, math::Vector3d::Zero);
+
+ std::tie(intersect, dist, pt) = b.Intersect(
+ math::Line3d(math::Vector3d(4, 0, 0.5), math::Vector3d(0, 10, 0.5)));
+ EXPECT_FALSE(intersect);
+ EXPECT_DOUBLE_EQ(dist, 0);
+ EXPECT_EQ(pt, math::Vector3d::Zero);
+
+ std::tie(intersect, dist, pt) = b.Intersect(
+ math::Line3d(math::Vector3d(1, -1, 1.5), math::Vector3d(0, 1, 1.5)));
+ EXPECT_FALSE(intersect);
+ EXPECT_DOUBLE_EQ(dist, 0);
+ EXPECT_EQ(pt, math::Vector3d::Zero);
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(0, 0, 1),
+ math::Vector3d(0, 0, -1), 0, 1000);
+ EXPECT_TRUE(intersect);
+ EXPECT_TRUE(b.IntersectCheck(math::Vector3d(0, 0, 1),
+ math::Vector3d(0, 0, -1), 0, 1000));
+ EXPECT_DOUBLE_EQ(dist, 0);
+ EXPECT_DOUBLE_EQ(std::get<1>(b.IntersectDist(math::Vector3d(0, 0, 1),
+ math::Vector3d(0, 0, -1), 0, 1000)), dist);
+ EXPECT_EQ(pt, math::Vector3d(0, 0, 1));
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(0, 0, 0),
+ math::Vector3d(1, 0, 0), 0, 1000);
+ EXPECT_TRUE(intersect);
+ EXPECT_TRUE(b.IntersectCheck(math::Vector3d(0, 0, 0),
+ math::Vector3d(1, 0, 0), 0, 1000));
+ EXPECT_DOUBLE_EQ(dist, 0);
+ EXPECT_DOUBLE_EQ(std::get<1>(b.IntersectDist(math::Vector3d(0, 0, 0),
+ math::Vector3d(1, 0, 0), 0, 1000)), dist);
+ EXPECT_EQ(pt, math::Vector3d::Zero);
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(0, 0, 0),
+ math::Vector3d(-1, 0, 0), 0, 1000);
+ EXPECT_TRUE(intersect);
+ EXPECT_TRUE(b.IntersectCheck(math::Vector3d(0, 0, 0),
+ math::Vector3d(-1, 0, 0), 0, 1000));
+ EXPECT_DOUBLE_EQ(dist, 0);
+ EXPECT_DOUBLE_EQ(std::get<1>(b.IntersectDist(math::Vector3d(0, 0, 0),
+ math::Vector3d(-1, 0, 0), 0, 1000)), dist);
+ EXPECT_EQ(pt, math::Vector3d::Zero);
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(0, 0, 0),
+ math::Vector3d(0, 1, 0), 0, 1000);
+ EXPECT_TRUE(intersect);
+ EXPECT_TRUE(b.IntersectCheck(math::Vector3d(0, 0, 0),
+ math::Vector3d(0, 1, 0), 0, 1000));
+ EXPECT_DOUBLE_EQ(dist, 0);
+ EXPECT_DOUBLE_EQ(std::get<1>(b.IntersectDist(math::Vector3d(0, 0, 0),
+ math::Vector3d(0, 1, 0), 0, 1000)), dist);
+ EXPECT_EQ(pt, math::Vector3d::Zero);
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(0.5, 0.5, 0.5),
+ math::Vector3d(-.707107, 0, -0.707107), 0, 1000);
+ EXPECT_TRUE(intersect);
+ EXPECT_TRUE(b.IntersectCheck(math::Vector3d(0.5, 0.5, 0.5),
+ math::Vector3d(-.707107, 0, -0.707107), 0, 1000));
+ EXPECT_NEAR(dist, 0, 1e-5);
+ EXPECT_NEAR(std::get<1>(b.Intersect(math::Vector3d(0.5, 0.5, 0.5),
+ math::Vector3d(-.707107, 0, -0.707107), 0, 1000)), dist, 1e-5);
+ EXPECT_EQ(pt, math::Vector3d(0.5, 0.5, 0.5));
+
+ std::tie(intersect, dist, pt) = b.Intersect(math::Vector3d(1.2, 0, 0.5),
+ math::Vector3d(-0.707107, 0, -0.707107), 0, 1000);
+ EXPECT_TRUE(intersect);
+ EXPECT_TRUE(b.IntersectCheck(math::Vector3d(1.2, 0, 0.5),
+ math::Vector3d(-0.707107, 0, -0.707107), 0, 1000));
+ EXPECT_NEAR(dist, 0.28284, 1e-5);
+ EXPECT_NEAR(std::get<1>(b.Intersect(math::Vector3d(1.2, 0, 0.5),
+ math::Vector3d(-0.707107, 0, -0.707107), 0, 1000)), dist, 1e-5);
+ EXPECT_EQ(pt, math::Vector3d(1, 0, 0.3));
+}
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 0ec255f..9a72750 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,9 +1,9 @@
include (${project_cmake_dir}/Utils.cmake)
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
set (sources
Angle.cc
Box.cc
- BoxPrivate.cc
Frustum.cc
Helpers.cc
IndexException.cc
@@ -13,6 +13,7 @@ set (sources
RotationSplinePrivate.cc
SignalStats.cc
Spline.cc
+ Temperature.cc
Vector3Stats.cc
)
@@ -22,6 +23,7 @@ set (gtest_sources
Filter_TEST.cc
Frustum_TEST.cc
Helpers_TEST.cc
+ Inertial_TEST.cc
Kmeans_TEST.cc
Line2_TEST.cc
Line3_TEST.cc
@@ -35,26 +37,14 @@ set (gtest_sources
RotationSpline_TEST.cc
SignalStats_TEST.cc
Spline_TEST.cc
+ Temperature_TEST.cc
Triangle_TEST.cc
- Triangle3_TEST.cc
Vector2_TEST.cc
Vector3_TEST.cc
Vector3Stats_TEST.cc
Vector4_TEST.cc
)
-ign_add_library(${PROJECT_LIBRARY_TARGET_NAME} ${sources})
-
-# When the minimum CMake required version will be >= 3.1
-# we could use the target_compile_features() command
-# to enable C++11 support in a platform-independent way.
-if(NOT MSVC)
- target_compile_options(${PROJECT_LIBRARY_TARGET_NAME} PUBLIC "-std=c++11")
-endif()
-
-target_include_directories(${PROJECT_LIBRARY_TARGET_NAME}
- PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
- $<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR_FULL}>)
-
+ign_add_library(ignition-math${PROJECT_MAJOR_VERSION} ${sources})
ign_build_tests(${gtest_sources})
-ign_install_library(${PROJECT_LIBRARY_TARGET_NAME} ${PROJECT_EXPORT_NAME})
+ign_install_library(ignition-math${PROJECT_MAJOR_VERSION})
diff --git a/src/Frustum_TEST.cc b/src/Frustum_TEST.cc
index 66790aa..e3703e7 100644
--- a/src/Frustum_TEST.cc
+++ b/src/Frustum_TEST.cc
@@ -15,7 +15,6 @@
*
*/
-#define _USE_MATH_DEFINES
#include <gtest/gtest.h>
#include "ignition/math/Helpers.hh"
@@ -120,7 +119,7 @@ TEST(FrustumTest, PyramidXAxisNeg)
// Aspect ratio
320.0/240.0,
// Pose
- Pose3d(0, 0, 0, 0, 0, M_PI));
+ Pose3d(0, 0, 0, 0, 0, IGN_PI));
EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 0)));
EXPECT_FALSE(frustum.Contains(Vector3d(-0.5, 0, 0)));
@@ -148,7 +147,7 @@ TEST(FrustumTest, PyramidYAxis)
// Aspect ratio
320.0/320.0,
// Pose
- Pose3d(0, 0, 0, 0, 0, M_PI*0.5));
+ Pose3d(0, 0, 0, 0, 0, IGN_PI*0.5));
EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 0)));
EXPECT_FALSE(frustum.Contains(Vector3d(1, 0, 0)));
@@ -176,7 +175,7 @@ TEST(FrustumTest, PyramidZAxis)
// Aspect ratio
320.0/320.0,
// Pose
- Pose3d(0, 0, 0, 0, M_PI*0.5, 0));
+ Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0));
EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 0)));
EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, -0.9)));
@@ -205,7 +204,7 @@ TEST(FrustumTest, NearFar)
// Aspect ratio
320.0/320.0,
// Pose
- Pose3d(0, 0, 0, 0, M_PI*0.5, 0));
+ Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0));
EXPECT_DOUBLE_EQ(frustum.Near(), 1.0);
EXPECT_DOUBLE_EQ(frustum.Far(), 10.0);
@@ -230,7 +229,7 @@ TEST(FrustumTest, FOV)
// Aspect ratio
320.0/320.0,
// Pose
- Pose3d(0, 0, 0, 0, M_PI*0.5, 0));
+ Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0));
EXPECT_EQ(frustum.FOV(), math::Angle(IGN_DTOR(45)));
@@ -252,7 +251,7 @@ TEST(FrustumTest, AspectRatio)
// Aspect ratio
320.0/320.0,
// Pose
- Pose3d(0, 0, 0, 0, M_PI*0.5, 0));
+ Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0));
EXPECT_DOUBLE_EQ(frustum.AspectRatio(), 320.0/320.0);
@@ -274,13 +273,13 @@ TEST(FrustumTest, Pose)
// Aspect ratio
320.0/320.0,
// Pose
- Pose3d(0, 0, 0, 0, M_PI*0.5, 0));
+ Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0));
- EXPECT_EQ(frustum.Pose(), Pose3d(0, 0, 0, 0, M_PI*0.5, 0));
+ EXPECT_EQ(frustum.Pose(), Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0));
- frustum.SetPose(Pose3d(1, 2, 3, M_PI, 0, 0));
+ frustum.SetPose(Pose3d(1, 2, 3, IGN_PI, 0, 0));
- EXPECT_EQ(frustum.Pose(), Pose3d(1, 2, 3, M_PI, 0, 0));
+ EXPECT_EQ(frustum.Pose(), Pose3d(1, 2, 3, IGN_PI, 0, 0));
}
/////////////////////////////////////////////////
@@ -296,7 +295,7 @@ TEST(FrustumTest, PoseContains)
// Aspect ratio
1920.0/1080.0,
// Pose
- Pose3d(0, -5, 0, 0, 0, M_PI*0.5));
+ Pose3d(0, -5, 0, 0, 0, IGN_PI*0.5));
// Test the near clip boundary
EXPECT_FALSE(frustum.Contains(Vector3d(0, -4.01, 0)));
@@ -422,7 +421,7 @@ TEST(FrustumTest, PoseContains)
EXPECT_FALSE(frustum.Contains(farBottomRightBad));
// Adjust to 45 degrees rotation
- frustum.SetPose(Pose3d(1, 1, 0, 0, 0, -M_PI*0.25));
+ frustum.SetPose(Pose3d(1, 1, 0, 0, 0, -IGN_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/Helpers.cc b/src/Helpers.cc
index d983c61..01077d2 100644
--- a/src/Helpers.cc
+++ b/src/Helpers.cc
@@ -16,9 +16,32 @@
*/
#include "ignition/math/Helpers.hh"
-namespace ignition
+/////////////////////////////////////////////
+ignition::math::PairOutput ignition::math::Pair(
+ const ignition::math::PairInput _a, const ignition::math::PairInput _b)
{
- namespace math
- {
- }
+ // Store in 64bit local variable so that we don't overflow.
+ uint64_t a = _a;
+ uint64_t b = _b;
+
+ // Szudzik's function
+ return _a >= _b ?
+ static_cast<PairOutput>(a * a + a + b) :
+ static_cast<PairOutput>(a + b * b);
+}
+
+/////////////////////////////////////////////
+std::tuple<ignition::math::PairInput, ignition::math::PairInput>
+ignition::math::Unpair(const ignition::math::PairOutput _key)
+{
+ // Must explicitly cast so that the _key is not auto cast to a double
+ uint64_t sqrt = static_cast<uint64_t>(
+ std::floor(std::sqrt(static_cast<long double>(_key))));
+ uint64_t sq = sqrt * sqrt;
+
+ return ((_key - sq) >= sqrt) ?
+ std::make_tuple(static_cast<PairInput>(sqrt),
+ static_cast<PairInput>(_key - sq - sqrt)) :
+ std::make_tuple(static_cast<PairInput>(_key - sq),
+ static_cast<PairInput>(sqrt));
}
diff --git a/src/Helpers_TEST.cc b/src/Helpers_TEST.cc
index 663ad4f..3a0c838 100644
--- a/src/Helpers_TEST.cc
+++ b/src/Helpers_TEST.cc
@@ -17,6 +17,7 @@
#include <gtest/gtest.h>
+#include "ignition/math/Rand.hh"
#include "ignition/math/Vector3.hh"
#include "ignition/math/Helpers.hh"
@@ -319,3 +320,109 @@ TEST(HelpersTest, Volume)
EXPECT_DOUBLE_EQ(IGN_BOX_VOLUME(.1, .2, .3),
IGN_BOX_VOLUME_V(math::Vector3d(0.1, 0.2, 0.3)));
}
+
+/////////////////////////////////////////////////
+TEST(HelpersTest, Pair)
+{
+#ifdef _MSC_VER
+ math::PairInput maxA = IGN_UINT16_MAX;
+ math::PairInput maxB = IGN_UINT16_MAX;
+#else
+ math::PairInput maxA = IGN_UINT32_MAX;
+ math::PairInput maxB = IGN_UINT32_MAX;
+#endif
+
+ math::PairInput maxC, maxD;
+
+ // Maximum parameters should generate a maximum key
+ math::PairOutput maxKey = math::Pair(maxA, maxB);
+#ifdef _MSC_VER
+ EXPECT_EQ(maxKey, IGN_UINT32_MAX);
+#else
+ EXPECT_EQ(maxKey, IGN_UINT64_MAX);
+#endif
+
+ std::tie(maxC, maxD) = math::Unpair(maxKey);
+ EXPECT_EQ(maxC, maxA);
+ EXPECT_EQ(maxD, maxB);
+
+#ifdef _MSC_VER
+ math::PairInput minA = IGN_UINT16_MIN;
+ math::PairInput minB = IGN_UINT16_MIN;
+#else
+ math::PairInput minA = IGN_UINT32_MIN;
+ math::PairInput minB = IGN_UINT32_MIN;
+#endif
+ math::PairInput minC, minD;
+
+ // Minimum parameters should generate a minimum key
+ math::PairOutput minKey = math::Pair(minA, minB);
+#ifdef _MSC_VER
+ EXPECT_EQ(minKey, IGN_UINT32_MIN);
+#else
+ EXPECT_EQ(minKey, IGN_UINT64_MIN);
+#endif
+
+ std::tie(minC, minD) = math::Unpair(minKey);
+ EXPECT_EQ(minC, minA);
+ EXPECT_EQ(minD, minB);
+
+ // Max key != min key
+ EXPECT_TRUE(minKey != maxKey);
+
+ // Just a simple test case
+ {
+ int a = 10;
+ int b = 20;
+ math::PairInput c, d;
+
+ auto key = math::Pair(static_cast<math::PairInput>(a),
+ static_cast<math::PairInput>(b));
+ EXPECT_EQ(key, 410);
+ EXPECT_TRUE(key != maxKey);
+ EXPECT_TRUE(key != minKey);
+
+ std::tie(c, d) = math::Unpair(key);
+ EXPECT_EQ(c, a);
+ EXPECT_EQ(d, b);
+ }
+
+ {
+ math::PairInput c, d;
+ std::set<math::PairOutput> set;
+
+ // Iterate over range of pairs, and check for unique keys.
+ for (uint16_t a = IGN_UINT16_MIN; a < IGN_UINT16_MAX - 500;
+ a += static_cast<uint16_t>(math::Rand::IntUniform(100, 500)))
+ {
+ for (uint16_t b = IGN_UINT16_MIN; b < IGN_UINT16_MAX - 500;
+ b += static_cast<uint16_t>(math::Rand::IntUniform(100, 500)))
+ {
+ math::PairOutput key = math::Pair(a, b);
+ std::tie(c, d) = math::Unpair(key);
+ EXPECT_EQ(a, c);
+ EXPECT_EQ(b, d);
+ EXPECT_TRUE(set.find(key) == set.end());
+ EXPECT_TRUE(key != maxKey);
+ set.insert(key);
+ }
+ }
+
+#ifndef _MSC_VER
+ // Iterate over large numbers, and check for unique keys.
+ for (math::PairInput a = IGN_UINT32_MAX-5000; a < IGN_UINT32_MAX; a++)
+ {
+ for (math::PairInput b = IGN_UINT32_MAX-5000; b < IGN_UINT32_MAX; b++)
+ {
+ math::PairOutput key = math::Pair(a, b);
+ std::tie(c, d) = math::Unpair(key);
+ EXPECT_EQ(a, c);
+ EXPECT_EQ(b, d);
+ EXPECT_TRUE(set.find(key) == set.end());
+ EXPECT_TRUE(key != minKey);
+ set.insert(key);
+ }
+ }
+#endif
+ }
+}
diff --git a/src/Inertial_TEST.cc b/src/Inertial_TEST.cc
new file mode 100644
index 0000000..23a15c2
--- /dev/null
+++ b/src/Inertial_TEST.cc
@@ -0,0 +1,349 @@
+/*
+ * 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 <cmath>
+
+#include "ignition/math/Inertial.hh"
+
+using namespace ignition;
+
+/////////////////////////////////////////////////
+// Simple constructor, test default values
+TEST(Inertiald_Test, Constructor)
+{
+ math::Inertiald inertial;
+ EXPECT_EQ(inertial.Pose(), math::Pose3d::Zero);
+ EXPECT_EQ(inertial.MassMatrix(), math::MassMatrix3d());
+ EXPECT_EQ(inertial.MOI(), math::Matrix3d::Zero);
+}
+
+/////////////////////////////////////////////////
+// Constructor with default arguments
+// Should match simple constructor and with copy constructor
+TEST(Inertiald_Test, ConstructorDefaultValues)
+{
+ math::Inertiald inertial(math::MassMatrix3d(), math::Pose3d::Zero);
+ EXPECT_EQ(inertial, math::Inertiald());
+ EXPECT_EQ(inertial, math::Inertiald(inertial));
+}
+
+/////////////////////////////////////////////////
+// Constructor with non-default arguments
+TEST(Inertiald_Test, ConstructorNonDefaultValues)
+{
+ const double mass = 5.0;
+ const math::Vector3d Ixxyyzz(2.0, 3.0, 4.0);
+ const math::Vector3d Ixyxzyz(0.2, 0.3, 0.4);
+ math::MassMatrix3d m(mass, Ixxyyzz, Ixyxzyz);
+ EXPECT_TRUE(m.IsPositive());
+ EXPECT_TRUE(m.IsValid());
+ const math::Pose3d pose(1, 2, 3, IGN_PI/6, 0, 0);
+ math::Inertiald inertial(m, pose);
+
+ // Should not match simple constructor
+ EXPECT_NE(inertial, math::Inertiald());
+
+ // Should match with copy constructor
+ EXPECT_EQ(inertial, math::Inertiald(inertial));
+
+ // Test accessors
+ EXPECT_EQ(inertial.MassMatrix(), m);
+ EXPECT_EQ(inertial.Pose(), pose);
+ EXPECT_TRUE(inertial.MassMatrix().IsPositive());
+ EXPECT_TRUE(inertial.MassMatrix().IsValid());
+
+ // Test assignment operator
+ math::Inertiald inertial2;
+ EXPECT_NE(inertial, inertial2);
+ inertial2 = inertial;
+ EXPECT_EQ(inertial, inertial2);
+}
+
+/////////////////////////////////////////////////
+TEST(Inertiald_Test, CoverageExtra)
+{
+ // getting full destructor coverage
+ math::Inertiald *p = new math::Inertiald;
+ EXPECT_TRUE(p != NULL);
+ delete p;
+}
+
+/////////////////////////////////////////////////
+TEST(Inertiald_Test, Setters)
+{
+ const double mass = 5.0;
+ const math::Vector3d Ixxyyzz(2.0, 3.0, 4.0);
+ const math::Vector3d Ixyxzyz(0.2, 0.3, 0.4);
+ math::MassMatrix3d m(mass, Ixxyyzz, Ixyxzyz);
+ EXPECT_TRUE(m.IsPositive());
+ EXPECT_TRUE(m.IsValid());
+ const math::Pose3d pose(1, 2, 3, IGN_PI/6, 0, 0);
+ math::Inertiald inertial;
+
+ // Initially invalid
+ EXPECT_FALSE(inertial.SetPose(pose));
+
+ // Valid once valid mass matrix is set
+ EXPECT_TRUE(inertial.SetMassMatrix(m));
+
+ // Verify values
+ EXPECT_EQ(inertial.MassMatrix(), m);
+ EXPECT_EQ(inertial.Pose(), pose);
+
+ // Invalid again if an invalid inertia is set
+ math::MassMatrix3d mInvalid(-1, Ixxyyzz, Ixyxzyz);
+ EXPECT_FALSE(inertial.SetMassMatrix(mInvalid));
+}
+
+/////////////////////////////////////////////////
+TEST(Inertiald_Test, MOI_Diagonal)
+{
+ const double mass = 12.0;
+ const math::Vector3d Ixxyyzz(2.0, 3.0, 4.0);
+ const math::Vector3d Ixyxzyz(0, 0, 0);
+ const math::MassMatrix3d m(mass, Ixxyyzz, Ixyxzyz);
+ EXPECT_TRUE(m.IsPositive());
+ EXPECT_TRUE(m.IsValid());
+
+ // no rotation, expect MOI's to match
+ {
+ const math::Pose3d pose(0, 0, 0, 0, 0, 0);
+ math::Inertiald inertial(m, pose);
+ EXPECT_EQ(inertial.MOI(), m.MOI());
+ }
+
+ // 90 deg rotation about X axis, expect different MOI
+ {
+ const math::Pose3d pose(0, 0, 0, IGN_PI_2, 0, 0);
+ const math::Matrix3d expectedMOI(2, 0, 0, 0, 4, 0, 0, 0, 3);
+ math::Inertiald inertial(m, pose);
+ EXPECT_NE(inertial.MOI(), m.MOI());
+ EXPECT_EQ(inertial.MOI(), expectedMOI);
+ }
+
+ // 90 deg rotation about Y axis, expect different MOI
+ {
+ const math::Pose3d pose(0, 0, 0, 0, IGN_PI_2, 0);
+ const math::Matrix3d expectedMOI(4, 0, 0, 0, 3, 0, 0, 0, 2);
+ math::Inertiald inertial(m, pose);
+ EXPECT_NE(inertial.MOI(), m.MOI());
+ EXPECT_EQ(inertial.MOI(), expectedMOI);
+ }
+
+ // 90 deg rotation about Z axis, expect different MOI
+ {
+ const math::Pose3d pose(0, 0, 0, 0, 0, IGN_PI_2);
+ const math::Matrix3d expectedMOI(3, 0, 0, 0, 2, 0, 0, 0, 4);
+ math::Inertiald inertial(m, pose);
+ EXPECT_NE(inertial.MOI(), m.MOI());
+ EXPECT_EQ(inertial.MOI(), expectedMOI);
+ }
+
+ // 45 deg rotation about Z axis, expect different MOI
+ {
+ const math::Pose3d pose(0, 0, 0, 0, 0, IGN_PI_4);
+ const math::Matrix3d expectedMOI(2.5, -0.5, 0, -0.5, 2.5, 0, 0, 0, 4);
+ math::Inertiald inertial(m, pose);
+ EXPECT_NE(inertial.MOI(), m.MOI());
+ EXPECT_EQ(inertial.MOI(), expectedMOI);
+
+ // double check with a second MassMatrix3 instance
+ math::MassMatrix3d m2;
+ EXPECT_FALSE(m2.Mass(mass));
+ EXPECT_TRUE(m2.MOI(expectedMOI));
+ EXPECT_EQ(inertial.MOI(), m2.MOI());
+ // There are multiple correct rotations due to symmetry
+ const auto rot2 = math::Quaterniond(IGN_PI, 0, IGN_PI_4);
+ EXPECT_TRUE(m2.PrincipalAxesOffset() == pose.Rot() ||
+ m2.PrincipalAxesOffset() == rot2);
+ }
+}
+
+/////////////////////////////////////////////////
+TEST(Inertiald_Test, Addition)
+{
+ // Add two half-cubes together
+ {
+ const double mass = 12.0;
+ const math::Vector3d size(1, 1, 1);
+ math::MassMatrix3d cubeMM3;
+ EXPECT_TRUE(cubeMM3.SetFromBox(mass, size));
+ const math::Inertiald cube(cubeMM3, math::Pose3d::Zero);
+ math::MassMatrix3d half;
+ EXPECT_TRUE(half.SetFromBox(0.5*mass, math::Vector3d(0.5, 1, 1)));
+ math::Inertiald left(half, math::Pose3d(-0.25, 0, 0, 0, 0, 0));
+ math::Inertiald right(half, math::Pose3d(0.25, 0, 0, 0, 0, 0));
+ EXPECT_EQ(cube, left + right);
+ EXPECT_EQ(cube, right + left);
+ // test += operator
+ {
+ math::Inertiald tmp = left;
+ tmp += right;
+ EXPECT_EQ(cube, tmp);
+ }
+ {
+ math::Inertiald tmp = right;
+ tmp += left;
+ EXPECT_EQ(cube, tmp);
+ }
+ // Test EquivalentBox
+ {
+ math::Vector3d size2;
+ math::Quaterniond rot2;
+ EXPECT_TRUE((left + right).MassMatrix().EquivalentBox(size2, rot2));
+ EXPECT_EQ(size, size2);
+ EXPECT_EQ(rot2, math::Quaterniond::Identity);
+ }
+ {
+ math::Vector3d size2;
+ math::Quaterniond rot2;
+ EXPECT_TRUE((right + left).MassMatrix().EquivalentBox(size2, rot2));
+ EXPECT_EQ(size, size2);
+ EXPECT_EQ(rot2, math::Quaterniond::Identity);
+ }
+ }
+
+ // Add two rotated half-cubes together
+ {
+ const double mass = 12.0;
+ const math::Vector3d size(1, 1, 1);
+ math::MassMatrix3d cubeMM3;
+ EXPECT_TRUE(cubeMM3.SetFromBox(mass, size));
+ const math::Inertiald cube(cubeMM3, math::Pose3d(0, 0, 0, IGN_PI_4, 0, 0));
+
+ math::MassMatrix3d half;
+ EXPECT_TRUE(half.SetFromBox(0.5*mass, math::Vector3d(0.5, 1, 1)));
+ math::Inertiald left(half, math::Pose3d(-0.25, 0, 0, IGN_PI_4, 0, 0));
+ math::Inertiald right(half, math::Pose3d(0.25, 0, 0, IGN_PI_4, 0, 0));
+
+ // objects won't match exactly
+ // since inertia matrices will all be in base frame
+ // but mass, center of mass, and base-frame MOI should match
+ EXPECT_NE(cube, left + right);
+ EXPECT_NE(cube, right + left);
+ EXPECT_DOUBLE_EQ(cubeMM3.Mass(), (left + right).MassMatrix().Mass());
+ EXPECT_DOUBLE_EQ(cubeMM3.Mass(), (right + left).MassMatrix().Mass());
+ EXPECT_EQ(cube.Pose().Pos(), (left + right).Pose().Pos());
+ EXPECT_EQ(cube.Pose().Pos(), (right + left).Pose().Pos());
+ EXPECT_EQ(cube.MOI(), (left + right).MOI());
+ EXPECT_EQ(cube.MOI(), (right + left).MOI());
+ }
+
+ // Add eight cubes together into larger cube
+ {
+ const double mass = 12.0;
+ const math::Vector3d size(1, 1, 1);
+ math::MassMatrix3d cubeMM3;
+ EXPECT_TRUE(cubeMM3.SetFromBox(mass, size));
+ const math::Inertiald addedCube =
+ math::Inertiald(cubeMM3, math::Pose3d(-0.5, -0.5, -0.5, 0, 0, 0)) +
+ math::Inertiald(cubeMM3, math::Pose3d(-0.5, 0.5, -0.5, 0, 0, 0)) +
+ math::Inertiald(cubeMM3, math::Pose3d(0.5, -0.5, -0.5, 0, 0, 0)) +
+ math::Inertiald(cubeMM3, math::Pose3d(0.5, 0.5, -0.5, 0, 0, 0)) +
+ math::Inertiald(cubeMM3, math::Pose3d(-0.5, -0.5, 0.5, 0, 0, 0)) +
+ math::Inertiald(cubeMM3, math::Pose3d(-0.5, 0.5, 0.5, 0, 0, 0)) +
+ math::Inertiald(cubeMM3, math::Pose3d(0.5, -0.5, 0.5, 0, 0, 0)) +
+ math::Inertiald(cubeMM3, math::Pose3d(0.5, 0.5, 0.5, 0, 0, 0));
+
+ math::MassMatrix3d trueCubeMM3;
+ EXPECT_TRUE(trueCubeMM3.SetFromBox(8*mass, 2*size));
+ EXPECT_EQ(addedCube, math::Inertiald(trueCubeMM3, math::Pose3d::Zero));
+ }
+
+ // Add eight rotated cubes together into larger cube
+ {
+ const double mass = 12.0;
+ const math::Vector3d size(1, 1, 1);
+ math::MassMatrix3d cubeMM3;
+ EXPECT_TRUE(cubeMM3.SetFromBox(mass, size));
+ const math::Inertiald addedCube =
+ math::Inertiald(cubeMM3, math::Pose3d(-0.5, -0.5, -0.5, 0, 0, 0)) +
+ math::Inertiald(cubeMM3, math::Pose3d(-0.5, 0.5, -0.5, IGN_PI_2, 0, 0)) +
+ math::Inertiald(cubeMM3, math::Pose3d(0.5, -0.5, -0.5, 0, IGN_PI_2, 0)) +
+ math::Inertiald(cubeMM3, math::Pose3d(0.5, 0.5, -0.5, 0, 0, IGN_PI_2)) +
+ math::Inertiald(cubeMM3, math::Pose3d(-0.5, -0.5, 0.5, IGN_PI, 0, 0)) +
+ math::Inertiald(cubeMM3, math::Pose3d(-0.5, 0.5, 0.5, 0, IGN_PI, 0)) +
+ math::Inertiald(cubeMM3, math::Pose3d(0.5, -0.5, 0.5, 0, 0, IGN_PI)) +
+ math::Inertiald(cubeMM3, math::Pose3d(0.5, 0.5, 0.5, 0, 0, 0));
+
+ math::MassMatrix3d trueCubeMM3;
+ EXPECT_TRUE(trueCubeMM3.SetFromBox(8*mass, 2*size));
+ EXPECT_EQ(addedCube, math::Inertiald(trueCubeMM3, math::Pose3d::Zero));
+ }
+}
+
+/////////////////////////////////////////////////
+// Addition operator has different behavior if mass is non-positive
+TEST(Inertiald_Test, AdditionInvalid)
+{
+ // inertias all zero
+ const math::MassMatrix3d m0(0.0, math::Vector3d::Zero, math::Vector3d::Zero);
+ EXPECT_FALSE(m0.IsPositive());
+ EXPECT_FALSE(m0.IsValid());
+
+ // both inertials with zero mass
+ {
+ math::Inertiald left(m0, math::Pose3d(-1, 0, 0, 0, 0, 0));
+ math::Inertiald right(m0, math::Pose3d(1, 0, 0, 0, 0, 0));
+
+ // expect sum to equal left argument
+ EXPECT_EQ(left, left + right);
+ EXPECT_EQ(right, right + left);
+ {
+ math::Inertiald tmp = left;
+ tmp += right;
+ EXPECT_EQ(tmp, left);
+ }
+ {
+ math::Inertiald tmp = right;
+ tmp += left;
+ EXPECT_EQ(tmp, right);
+ }
+ }
+
+ // one inertial with zero inertias should not affect the sum
+ {
+ math::MassMatrix3d m(12.0,
+ math::Vector3d(2, 3, 4),
+ math::Vector3d(0.1, 0.2, 0.3));
+ EXPECT_TRUE(m.IsPositive());
+ EXPECT_TRUE(m.IsValid());
+
+ math::Inertiald i(m, math::Pose3d(-1, 0, 0, 0, 0, 0));
+ math::Inertiald i0(m0, math::Pose3d(1, 0, 0, 0, 0, 0));
+
+ // expect i0 to not affect the sum
+ EXPECT_EQ(i, i + i0);
+ EXPECT_EQ(i, i0 + i);
+ {
+ math::Inertiald tmp = i;
+ tmp += i0;
+ EXPECT_EQ(tmp, i);
+ }
+ {
+ math::Inertiald tmp = i0;
+ tmp += i;
+ EXPECT_EQ(tmp, i);
+ }
+
+ EXPECT_TRUE((i + i0).MassMatrix().IsPositive());
+ EXPECT_TRUE((i0 + i).MassMatrix().IsPositive());
+ EXPECT_TRUE((i + i0).MassMatrix().IsValid());
+ EXPECT_TRUE((i0 + i).MassMatrix().IsValid());
+ }
+}
diff --git a/src/Kmeans_TEST.cc b/src/Kmeans_TEST.cc
index 754c6c2..28b843e 100644
--- a/src/Kmeans_TEST.cc
+++ b/src/Kmeans_TEST.cc
@@ -43,7 +43,7 @@ TEST(KmeansTest, Kmeans)
// ::GetObservations()
std::vector<math::Vector3d> obsCopy;
obsCopy = kmeans.Observations();
- for (auto i = 0; i < obsCopy.size(); ++i)
+ for (size_t i = 0; i < obsCopy.size(); ++i)
EXPECT_EQ(obsCopy[i], obs[i]);
// ::SetObservations()
@@ -53,7 +53,7 @@ TEST(KmeansTest, Kmeans)
EXPECT_TRUE(kmeans.Observations(obsCopy));
obsCopy = kmeans.Observations();
- for (auto i = 0; i < obsCopy.size(); ++i)
+ for (size_t i = 0; i < obsCopy.size(); ++i)
EXPECT_EQ(obsCopy[i], obs[i] + math::Vector3d(0.1, 0.2, 0.0));
EXPECT_TRUE(kmeans.Observations(obs));
@@ -133,7 +133,7 @@ TEST(KmeansTest, Append)
std::vector<math::Vector3d> obsCopy;
obsCopy = kmeans.Observations();
- for (auto i = 0; i < obsCopy.size(); ++i)
+ for (size_t i = 0; i < obsCopy.size(); ++i)
EXPECT_EQ(obsTotal[i], obsCopy[i]);
// Append an empty vector.
diff --git a/src/Line2_TEST.cc b/src/Line2_TEST.cc
index 66db6c1..8dcf81d 100644
--- a/src/Line2_TEST.cc
+++ b/src/Line2_TEST.cc
@@ -57,8 +57,11 @@ TEST(Line2Test, Slope)
}
{
+// MSVC reports a warning about division by zero
+#ifndef _MSC_VER
math::Line2d line(0, 0, 0, 10);
EXPECT_TRUE(math::isnan(line.Slope()));
+#endif
}
{
diff --git a/src/Line3_TEST.cc b/src/Line3_TEST.cc
index dc6b599..35347be 100644
--- a/src/Line3_TEST.cc
+++ b/src/Line3_TEST.cc
@@ -164,129 +164,3 @@ TEST(Line3Test, Direction)
lineA.Set(1, 1, 1, 7, 1, 1);
EXPECT_TRUE(lineA.Direction() == math::Vector3d::UnitX);
}
-
-/////////////////////////////////////////////////
-TEST(Line3Test, Within)
-{
- math::Line3d line(0, 0, 0, 1, 1, 1);
- EXPECT_TRUE(line.Within(math::Vector3d(0, 0, 0)));
- EXPECT_TRUE(line.Within(math::Vector3d(1, 1, 1)));
- EXPECT_TRUE(line.Within(math::Vector3d(0.5, 0.5, 0.5)));
-
- EXPECT_FALSE(line.Within(math::Vector3d(-0.5, 0.5, 0.5)));
- EXPECT_FALSE(line.Within(math::Vector3d(0.5, -0.5, 0.5)));
- EXPECT_FALSE(line.Within(math::Vector3d(0.5, 0.5, -0.5)));
-}
-
-/////////////////////////////////////////////////
-TEST(Line3Test, Distance)
-{
- math::Line3d line(0, 0, 0, 0, 1, 0);
- math::Line3d result;
-
- EXPECT_TRUE(line.Distance(math::Line3d(1, 0.5, 0, -1, 0.5, 0), result));
- EXPECT_EQ(result.Length(), 0);
- EXPECT_EQ(result, math::Line3d(0, 0.5, 0, 0, 0.5, 0));
-
- EXPECT_TRUE(line.Distance(math::Line3d(1, 0, 0, -1, 0, 0), result));
- EXPECT_EQ(result.Length(), 0);
- EXPECT_EQ(result, math::Line3d(0, 0, 0, 0, 0, 0));
-
- EXPECT_TRUE(line.Distance(math::Line3d(1, 1.1, 0, -1, 1.1, 0), result));
- EXPECT_NEAR(result.Length(), 0.1, 1e-4);
- EXPECT_EQ(result, math::Line3d(0, 1, 0, 0, 1.1, 0));
-
- EXPECT_TRUE(line.Distance(math::Line3d(1, 0.5, 0.4, -1, 0.5, 0.4), result));
- EXPECT_NEAR(result.Length(), 0.4, 1e-4);
- EXPECT_EQ(result, math::Line3d(0, 0.5, 0, 0, 0.5, 0.4));
-
- EXPECT_TRUE(line.Distance(math::Line3d(0, 0.5, 1, 1, 0.5, 0), result));
- EXPECT_NEAR(result.Length(), sin(IGN_PI / 4), 1e-4);
- EXPECT_EQ(result, math::Line3d(0, 0.5, 0, 0.5, 0.5, 0.5));
-
- // Expect true when lines are parallel
- EXPECT_TRUE(line.Distance(math::Line3d(2, 0, 0, 2, 1, 0), result));
- EXPECT_EQ(result[0], line[0]);
- EXPECT_EQ(result[1], math::Vector3d(2, 0, 0));
-
- EXPECT_TRUE(line.Distance(math::Line3d(2, 1, 0, 2, 0, 0), result));
- EXPECT_EQ(result[0], line[0]);
- EXPECT_EQ(result[1], math::Vector3d(2, 0, 0));
-
- EXPECT_TRUE(line.Distance(math::Line3d(1, 1, 0, 1, 2, 0), result));
- EXPECT_EQ(result[0], line[1]);
- EXPECT_EQ(result[1], math::Vector3d(1, 1, 0));
-
- EXPECT_TRUE(line.Distance(math::Line3d(1, 2, 0, 1, 1, 0), result));
- EXPECT_EQ(result[0], line[1]);
- EXPECT_EQ(result[1], math::Vector3d(1, 1, 0));
-
- // Expect false when the passed in line is a point
- EXPECT_FALSE(line.Distance(math::Line3d(2, 0, 0, 2, 0, 0), result));
-
- // Expect false when the first line is a point.
- line.Set(0, 0, 0, 0, 0, 0);
- EXPECT_FALSE(line.Distance(math::Line3d(2, 0, 0, 2, 1, 0), result));
-}
-
-/////////////////////////////////////////////////
-TEST(Line3Test, Intersect)
-{
- math::Line3d line(0, 0, 0, 0, 1, 0);
- math::Vector3d pt;
-
- EXPECT_TRUE(line.Intersect(math::Line3d(1, 0.5, 0, -1, 0.5, 0)));
- EXPECT_TRUE(line.Intersect(math::Line3d(1, 0.5, 0, -1, 0.5, 0), pt));
- EXPECT_EQ(pt, math::Vector3d(0, 0.5, 0));
-
- EXPECT_TRUE(line.Intersect(math::Line3d(1, 0, 0, -1, 0, 0)));
- EXPECT_TRUE(line.Intersect(math::Line3d(1, 0, 0, -1, 0, 0), pt));
- EXPECT_EQ(pt, math::Vector3d(0, 0, 0));
-
- EXPECT_TRUE(line.Intersect(math::Line3d(1, 1, 0, -1, 1, 0)));
- EXPECT_TRUE(line.Intersect(math::Line3d(1, 1, 0, -1, 1, 0), pt));
- EXPECT_EQ(pt, math::Vector3d(0, 1, 0));
-
- EXPECT_TRUE(line.Intersect(math::Line3d(0, 0.5, -1, 0, 0.5, 1)));
- EXPECT_TRUE(line.Intersect(math::Line3d(0, 0.5, -1, 0, 0.5, 1), pt));
- EXPECT_EQ(pt, math::Vector3d(0, 0.5, 0));
-
- EXPECT_TRUE(line.Intersect(math::Line3d(-1, 0.5, -1, 1, 0.5, 1)));
- EXPECT_TRUE(line.Intersect(math::Line3d(-1, 0.5, -1, 1, 0.5, 1), pt));
- EXPECT_EQ(pt, math::Vector3d(0, 0.5, 0));
-
- EXPECT_FALSE(line.Intersect(math::Line3d(1, 1.1, 0, -1, 1.1, 0)));
- EXPECT_FALSE(line.Intersect(math::Line3d(1, -0.1, 0, -1, -0.1, 0)));
-
- EXPECT_FALSE(line.Intersect(math::Line3d(0.1, 0.1, 0, 0.6, 0.6, 0)));
- EXPECT_FALSE(line.Intersect(math::Line3d(-0.1, 0, 0, -0.1, 1, 0)));
-
- EXPECT_TRUE(line.Intersect(math::Line3d(0, -1, 0, 0, 0.1, 0)));
- EXPECT_TRUE(line.Intersect(math::Line3d(0, 1, 0, 0, 1.1, 0)));
-}
-
-/////////////////////////////////////////////////
-TEST(Line3Test, Parallel)
-{
- math::Line3d line(0, 0, 0, 0, 1, 0);
- EXPECT_TRUE(line.Parallel(math::Line3d(1, 0, 0, 1, 1, 0)));
- EXPECT_TRUE(line.Parallel(math::Line3d(1, 1, 0, 1, 0, 0)));
- EXPECT_TRUE(line.Parallel(math::Line3d(0, 0, 0, 0, 10, 0)));
- EXPECT_TRUE(line.Parallel(math::Line3d(-100, 100, 20, -100, 200, 20)));
-
- EXPECT_FALSE(line.Parallel(math::Line3d(1, 0, 0, 1, 1, 1)));
- EXPECT_FALSE(line.Parallel(math::Line3d(1, 0, 0, 2, 0, 0)));
- EXPECT_FALSE(line.Parallel(math::Line3d(1, 0, 1, 2, 0, 1)));
-}
-
-/////////////////////////////////////////////////
-TEST(Line3Test, Coplanar)
-{
- math::Line3d line(0, 0, 0, 0, 1, 0);
- EXPECT_TRUE(line.Coplanar(math::Line3d(1, 0, 0, 1, 1, 0)));
- EXPECT_TRUE(line.Coplanar(math::Line3d(0, 0, 0, 0, 10, 0)));
- EXPECT_TRUE(line.Coplanar(math::Line3d(-100, 100, 20, -100, 200, 20)));
-
- EXPECT_FALSE(line.Coplanar(math::Line3d(1, 0, 0, 1, 1, 1)));
- EXPECT_FALSE(line.Coplanar(math::Line3d(1, 0, 1, 2, 0, 0)));
-}
diff --git a/src/MassMatrix3_TEST.cc b/src/MassMatrix3_TEST.cc
index 40dd576..db53377 100644
--- a/src/MassMatrix3_TEST.cc
+++ b/src/MassMatrix3_TEST.cc
@@ -15,9 +15,6 @@
*
*/
-#ifndef _USE_MATH_DEFINES
-# define _USE_MATH_DEFINES
-#endif
#include <gtest/gtest.h>
#include <cmath>
@@ -284,7 +281,7 @@ TEST(MassMatrix3dTest, PrincipalMoments)
const math::Vector3d Ixxyyzz(2.0, 2.0, 2.0);
const math::Vector3d Ixyxzyz(-1.0, 0, -1.0);
math::MassMatrix3d m(1.0, Ixxyyzz, Ixyxzyz);
- const math::Vector3d Ieigen(2-M_SQRT2, 2, 2+M_SQRT2);
+ const math::Vector3d Ieigen(2-IGN_SQRT2, 2, 2+IGN_SQRT2);
EXPECT_EQ(m.PrincipalMoments(), Ieigen);
EXPECT_TRUE(m.IsPositive());
EXPECT_FALSE(m.IsValid());
@@ -296,7 +293,7 @@ TEST(MassMatrix3dTest, PrincipalMoments)
const math::Vector3d Ixxyyzz(4.0, 4.0, 4.0);
const math::Vector3d Ixyxzyz(-1.0, 0, -1.0);
math::MassMatrix3d m(1.0, Ixxyyzz, Ixyxzyz);
- const math::Vector3d Ieigen(4-M_SQRT2, 4, 4+M_SQRT2);
+ const math::Vector3d Ieigen(4-IGN_SQRT2, 4, 4+IGN_SQRT2);
EXPECT_EQ(m.PrincipalMoments(), Ieigen);
EXPECT_TRUE(m.IsPositive());
EXPECT_TRUE(m.IsValid());
@@ -327,5 +324,532 @@ TEST(MassMatrix3dTest, PrincipalMoments)
EXPECT_TRUE(m.IsPositive());
EXPECT_FALSE(m.IsValid());
}
+
+ // Another matrix with large condition number
+ // invalid inertia matrix since it doesn't satisfy triangle inequality
+ // 0.98 + 1e8-1e3 < 1e8+1e3
+ // 0.98 < 2e3
+ {
+ const math::Vector3d Ixxyyzz(1e8, 1e8, 1);
+ const math::Vector3d Ixyxzyz(1e3, 1e3, 1e3);
+ math::MassMatrix3d m(1.0, Ixxyyzz, Ixyxzyz);
+ const math::Vector3d Ieigen(0.98, 1e8-1e3, 1e8+1e3);
+ // the accuracy is approximately 2e-2
+ EXPECT_TRUE(m.PrincipalMoments().Equal(Ieigen, 2.5e-2));
+ EXPECT_FALSE(m.PrincipalMoments().Equal(Ieigen, 1.5e-2));
+ // the default tolerance for == is 1e-6
+ // so this should resolve as not equal
+ EXPECT_NE(m.PrincipalMoments(), Ieigen);
+ EXPECT_TRUE(m.IsPositive());
+ EXPECT_FALSE(m.IsValid());
+ }
+}
+
+/////////////////////////////////////////////////
+TEST(MassMatrix3dTest, PrincipalAxesOffsetIdentity)
+{
+ // Identity inertia matrix, expect unit quaternion
+ math::MassMatrix3d m(1.0, math::Vector3d::One, math::Vector3d::Zero);
+ EXPECT_EQ(m.PrincipalAxesOffset(), math::Quaterniond());
+
+ // Scale the diagonal terms
+ EXPECT_TRUE(m.DiagonalMoments(3.5 * math::Vector3d::One));
+ EXPECT_TRUE(m.OffDiagonalMoments(math::Vector3d::Zero));
+ EXPECT_TRUE(m.IsValid());
+ EXPECT_EQ(m.PrincipalAxesOffset(), math::Quaterniond::Identity);
+}
+
+/////////////////////////////////////////////////
+/// \brief Helper function for verifying principal moments
+/// and axes offset by reconstructing the moment of inertia matrix
+/// from the eigenvectors and diagonalized matrix.
+/// \param[in] _m mass matrix to verify
+/// \param[in] _tolerance relative tolerance to use
+void VerifyPrincipalMomentsAndAxes(const math::MassMatrix3d &_m,
+ const double _tolerance = 1e-6)
+{
+ auto q = _m.PrincipalAxesOffset(_tolerance);
+ auto R = math::Matrix3d(q);
+ EXPECT_FALSE(q.W() == 0.0 && q.X() == 0.0 && q.Y() == 0.0 && q.Z() == 0.0);
+ auto moments = _m.PrincipalMoments(_tolerance);
+ math::Matrix3d L(moments[0], 0, 0,
+ 0, moments[1], 0,
+ 0, 0, moments[2]);
+ EXPECT_EQ(_m.MOI(), R * L * R.Transposed());
+}
+
+/////////////////////////////////////////////////
+/// \brief Helper function for testing diagonal inertia matrices.
+/// Expect the following:
+/// * that principal moments match the diagonal values,
+/// * that mass matrix is valid,
+/// * that principal axes have no offset (identity quaternion)
+/// * that reconstructed moment of inertia matrix matches the original
+/// \param[in] _moments Diagonal/principal moments of inertia.
+void VerifyDiagonalMomentsAndAxes(const math::Vector3d &_moments)
+{
+ math::MassMatrix3d m(1.0, math::Vector3d::Zero, math::Vector3d::Zero);
+ EXPECT_TRUE(m.DiagonalMoments(_moments));
+ EXPECT_EQ(m.PrincipalMoments(), m.DiagonalMoments());
+ EXPECT_TRUE(m.IsValid());
+ // Expect unit quaternion
+ EXPECT_EQ(m.PrincipalAxesOffset(), math::Quaterniond::Identity);
+ VerifyPrincipalMomentsAndAxes(m);
+
+ // Try with negative tolerance, expect sorted principal moments
+ math::Vector3d sortedMoments;
+ {
+ double m0 = _moments[0];
+ double m1 = _moments[1];
+ double m2 = _moments[2];
+ math::sort3(m0, m1, m2);
+ sortedMoments.Set(m0, m1, m2);
+ }
+ const double tolerance = -1e-6;
+ EXPECT_EQ(m.PrincipalMoments(tolerance), sortedMoments);
+ VerifyPrincipalMomentsAndAxes(m, tolerance);
+}
+
+/////////////////////////////////////////////////
+TEST(MassMatrix3dTest, PrincipalAxesOffsetDiagonal)
+{
+ // repeated moments [2, 3, 3]
+ VerifyDiagonalMomentsAndAxes(math::Vector3d(2.0, 3.0, 3.0));
+ VerifyDiagonalMomentsAndAxes(math::Vector3d(3.0, 2.0, 3.0));
+ VerifyDiagonalMomentsAndAxes(math::Vector3d(3.0, 3.0, 2.0));
+ // repeated moments [2, 2, 3]
+ VerifyDiagonalMomentsAndAxes(math::Vector3d(3.0, 2.0, 2.0));
+ VerifyDiagonalMomentsAndAxes(math::Vector3d(2.0, 3.0, 2.0));
+ VerifyDiagonalMomentsAndAxes(math::Vector3d(2.0, 2.0, 3.0));
+ // non-repeated moments
+ VerifyDiagonalMomentsAndAxes(math::Vector3d(2.0, 3.0, 4.0));
+ VerifyDiagonalMomentsAndAxes(math::Vector3d(4.0, 2.0, 3.0));
+ VerifyDiagonalMomentsAndAxes(math::Vector3d(3.0, 4.0, 2.0));
+ VerifyDiagonalMomentsAndAxes(math::Vector3d(2.0, 4.0, 3.0));
+ VerifyDiagonalMomentsAndAxes(math::Vector3d(3.0, 2.0, 4.0));
+ VerifyDiagonalMomentsAndAxes(math::Vector3d(4.0, 3.0, 2.0));
+}
+
+/////////////////////////////////////////////////
+/// \brief Helper function for testing non-diagonal inertia matrices.
+/// Expect the following:
+/// * that principal moments match the supplied values,
+/// * that mass matrix is valid,
+/// * that principal axes have an offset (non-identity quaternion)
+/// * that reconstructed moment of inertia matrix matches the original
+/// \param[in] _principalMoments Expected principal moments of inertia
+/// \param[in] _ixxyyzz Diagonal moments of inertia.
+/// \param[in] _ixyxzyz Off-diagonal moments of inertia.
+/// \param[in] _tolerance Absolute tolerance for eigenvalue expectation.
+void VerifyNondiagonalMomentsAndAxes(const math::Vector3d &_principalMoments,
+ const math::Vector3d &_ixxyyzz,
+ const math::Vector3d &_ixyxzyz,
+ const double _tolerance = 1e-6)
+{
+ math::MassMatrix3d m(1.0, _ixxyyzz, _ixyxzyz);
+ // EXPECT_EQ with default tolerance of 1e-6
+ // this outputs more useful error messages
+ EXPECT_EQ(m.PrincipalMoments(_tolerance), _principalMoments);
+ // also check equality with custom tolerance for small moments
+ EXPECT_TRUE(
+ m.PrincipalMoments(_tolerance).Equal(_principalMoments, _tolerance));
+ EXPECT_TRUE(m.IsValid());
+ // Expect non-unit quaternion
+ EXPECT_NE(m.PrincipalAxesOffset(_tolerance), math::Quaterniond());
+ VerifyPrincipalMomentsAndAxes(m, _tolerance);
+
+ // Try also with negated tolerance
+ EXPECT_TRUE(
+ m.PrincipalMoments(-_tolerance).Equal(_principalMoments, _tolerance));
+ VerifyPrincipalMomentsAndAxes(m, -_tolerance);
+}
+
+/////////////////////////////////////////////////
+TEST(MassMatrix3dTest, PrincipalAxesOffsetRepeat)
+{
+ // Principal moments: [3, 3, 5]
+ // Non-zero Ixy
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 3, 5),
+ math::Vector3d(4, 4, 3), math::Vector3d(-1, 0, 0));
+ // Non-zero Ixz
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 3, 5),
+ math::Vector3d(4, 3, 4), math::Vector3d(0, -1, 0));
+ // Non-zero Iyz
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 3, 5),
+ math::Vector3d(3, 4, 4), math::Vector3d(0, 0, -1));
+
+ // Principal moments: [3, 5, 5]
+ // Non-zero Ixy
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 5, 5),
+ math::Vector3d(4, 4, 5), math::Vector3d(-1, 0, 0));
+ // Non-zero Ixz
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 5, 5),
+ math::Vector3d(4, 5, 4), math::Vector3d(0, -1, 0));
+ // Non-zero Iyz
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 5, 5),
+ math::Vector3d(5, 4, 4), math::Vector3d(0, 0, -1));
+
+ // Principal moments: [4, 5, 5]
+ // Rotated by [45, 45, 0] degrees
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(4, 5, 5),
+ math::Vector3d(4.5, 4.75, 4.75),
+ 0.25*math::Vector3d(-IGN_SQRT2, IGN_SQRT2, 1));
+ // Rotated by [-45, 45, 0] degrees
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(4, 5, 5),
+ math::Vector3d(4.5, 4.75, 4.75),
+ 0.25*math::Vector3d(IGN_SQRT2, IGN_SQRT2, -1));
+ // Rotated by [45, -45, 0] degrees
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(4, 5, 5),
+ math::Vector3d(4.5, 4.75, 4.75),
+ 0.25*math::Vector3d(IGN_SQRT2, -IGN_SQRT2, 1));
+ // Rotated by [-45, -45, 0] degrees
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(4, 5, 5),
+ math::Vector3d(4.5, 4.75, 4.75),
+ 0.25*math::Vector3d(-IGN_SQRT2, -IGN_SQRT2, -1));
+
+ // Principal moments: [4, 4, 5]
+ // Rotated by [45, 45, 45] degrees
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(4, 4, 5),
+ math::Vector3d(4.5, 4.25, 4.25),
+ 0.25*math::Vector3d(-IGN_SQRT2, IGN_SQRT2, -1));
+ // different rotation
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(4, 4, 5),
+ math::Vector3d(4.5, 4.25, 4.25),
+ 0.25*math::Vector3d(IGN_SQRT2, IGN_SQRT2, 1));
+ // different rotation
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(4, 4, 5),
+ math::Vector3d(4.5, 4.25, 4.25),
+ 0.25*math::Vector3d(-IGN_SQRT2, -IGN_SQRT2, 1));
+ // different rotation
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(4, 4, 5),
+ math::Vector3d(4.5, 4.25, 4.25),
+ 0.25*math::Vector3d(IGN_SQRT2, -IGN_SQRT2, -1));
+
+ // Principal moments [4e-9, 4e-9, 5e-9]
+ // Rotated by [45, 45, 45] degrees
+ // use tolerance of 1e-15
+ VerifyNondiagonalMomentsAndAxes(1e-9 * math::Vector3d(4, 4, 5),
+ 1e-9 * math::Vector3d(4.5, 4.25, 4.25),
+ 0.25e-9*math::Vector3d(-IGN_SQRT2, IGN_SQRT2, -1), 1e-15);
+ // different rotation
+ VerifyNondiagonalMomentsAndAxes(1e-9 * math::Vector3d(4, 4, 5),
+ 1e-9 * math::Vector3d(4.5, 4.25, 4.25),
+ 0.25e-9*math::Vector3d(IGN_SQRT2, IGN_SQRT2, 1));
+ // different rotation
+ VerifyNondiagonalMomentsAndAxes(1e-9 * math::Vector3d(4, 4, 5),
+ 1e-9 * math::Vector3d(4.5, 4.25, 4.25),
+ 0.25e-9*math::Vector3d(-IGN_SQRT2, -IGN_SQRT2, 1));
+ // different rotation
+ VerifyNondiagonalMomentsAndAxes(1e-9 * math::Vector3d(4, 4, 5),
+ 1e-9 * math::Vector3d(4.5, 4.25, 4.25),
+ 0.25e-9*math::Vector3d(IGN_SQRT2, -IGN_SQRT2, -1), 1e-15);
+
+ // Principal moments [4, 4, 6]
+ // rotate by 30, 60, 0 degrees
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(4, 4, 6),
+ math::Vector3d(5.5, 4.125, 4.375),
+ 0.25*math::Vector3d(-sqrt(3), 3.0, -sqrt(3)/2));
+
+ // different rotation
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(4, 4, 6),
+ math::Vector3d(4.125, 5.5, 4.375),
+ 0.25*math::Vector3d(-sqrt(3), -sqrt(3)/2, 3.0));
+}
+
+/////////////////////////////////////////////////
+TEST(MassMatrix3dTest, PrincipalAxesOffsetNoRepeat)
+{
+ // Non-diagonal inertia matrix with f1 = 0
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 4, 6),
+ math::Vector3d(3.0, 5.0, 5.0),
+ math::Vector3d(0, 0, 1));
+ // Non-diagonal inertia matrix with f1 = 0
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 4, 6),
+ math::Vector3d(3.0, 5.0, 5.0),
+ math::Vector3d(0, 0, -1));
+
+ // Non-diagonal inertia matrix with f2 = 0
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 4, 6),
+ math::Vector3d(5.0, 4.0, 4.0),
+ math::Vector3d(-1, 1, 0));
+ // Non-diagonal inertia matrix with f2 = 0
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 4, 6),
+ math::Vector3d(5.0, 4.0, 4.0),
+ math::Vector3d(1, -1, 0));
+ // Non-diagonal inertia matrix with f2 = 0
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 4, 6),
+ math::Vector3d(5.0, 4.0, 4.0),
+ math::Vector3d(-1, -1, 0));
+ // Non-diagonal inertia matrix with f2 = 0
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 4, 6),
+ math::Vector3d(5.0, 4.0, 4.0),
+ math::Vector3d(1, 1, 0));
+
+ // Similar non-diagonal inertia matrix with f2 != 0
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 4, 6),
+ math::Vector3d(4.0, 4.0, 5.0),
+ math::Vector3d(0, 1, 1));
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 4, 6),
+ math::Vector3d(4.0, 4.0, 5.0),
+ math::Vector3d(0, -1, 1));
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 4, 6),
+ math::Vector3d(4.0, 4.0, 5.0),
+ math::Vector3d(0, 1, -1));
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(3, 4, 6),
+ math::Vector3d(4.0, 4.0, 5.0),
+ math::Vector3d(0, -1, -1));
+
+ // Test case for v = 0
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(2.5, 3.5, 4.0),
+ math::Vector3d(4.0, 3.0, 3.0),
+ math::Vector3d(0.0, 0, -0.5));
+
+ // Tri-diagonal matrix with identical diagonal terms
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(4-IGN_SQRT2, 4, 4+IGN_SQRT2),
+ math::Vector3d(4.0, 4.0, 4.0),
+ math::Vector3d(-1.0, 0, -1.0));
+ // small magnitude, use tolerance of 1e-15
+ VerifyNondiagonalMomentsAndAxes(
+ 1e-9 * math::Vector3d(4-IGN_SQRT2, 4, 4+IGN_SQRT2),
+ 1e-9 * math::Vector3d(4.0, 4.0, 4.0),
+ 1e-9 * math::Vector3d(-1.0, 0, -1.0), 1e-15);
+
+ // Tri-diagonal matrix with unique diagonal terms
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(5-sqrt(3), 5, 5+sqrt(3)),
+ math::Vector3d(4.0, 5.0, 6.0),
+ math::Vector3d(-1.0, 0, -1.0));
+ // small magnitude, use tolerance of 1e-15
+ VerifyNondiagonalMomentsAndAxes(1e-9*math::Vector3d(5-sqrt(3), 5, 5+sqrt(3)),
+ 1e-9 * math::Vector3d(4.0, 5.0, 6.0),
+ 1e-9 * math::Vector3d(-1.0, 0, -1.0), 1e-15);
+
+ // Nonzero values for all off-axis terms
+ VerifyNondiagonalMomentsAndAxes(math::Vector3d(10, 12, 14),
+ math::Vector3d(13, 11.75, 11.25),
+ math::Vector3d(-0.5*sqrt(3), 1.5, 0.25*sqrt(3)));
+
+ // Nonzero values for all off-axis terms
+ VerifyNondiagonalMomentsAndAxes(
+ math::Vector3d(6.6116, 8.2393186767, 13.983881323),
+ math::Vector3d(11.6116, 8.6116, 8.6116),
+ math::Vector3d(2, 2, 2));
+}
+
+/////////////////////////////////////////////////
+TEST(MassMatrix3dTest, EquivalentBox)
+{
+ // Default mass matrix with non-positive inertia
+ {
+ math::MassMatrix3d m;
+ math::Vector3d size;
+ math::Quaterniond rot;
+
+ // size is all zeros, so SetFromBox should fail
+ EXPECT_FALSE(m.SetFromBox(0.0, size, rot));
+ EXPECT_FALSE(m.SetFromBox(size, rot));
+
+ // even if mass is valid, it should not be set if size is invalid
+ EXPECT_FALSE(m.SetFromBox(1.0, size, rot));
+ EXPECT_EQ(m.Mass(), 0.0);
+
+ // equivalent box should not be findable
+ EXPECT_FALSE(m.EquivalentBox(size, rot));
+ }
+
+ // Moment of inertia matrix that doesn't satisfy triangle inequality
+ {
+ const math::Vector3d ixxyyzz(2.0, 2.0, 2.0);
+ const math::Vector3d ixyxzyz(-1.0, 0, -1.0);
+ math::MassMatrix3d m(1.0, ixxyyzz, ixyxzyz);
+ math::Vector3d size;
+ math::Quaterniond rot;
+ EXPECT_FALSE(m.EquivalentBox(size, rot));
+ }
+
+ // Identity inertia matrix
+ // expect cube with side length sqrt(6)
+ {
+ const double mass = 1.0;
+ math::MassMatrix3d m(mass, math::Vector3d::One, math::Vector3d::Zero);
+ math::Vector3d size;
+ math::Vector3d sizeTrue(sqrt(6) * math::Vector3d::One);
+ math::Quaterniond rot;
+ math::Quaterniond rotTrue(math::Quaterniond::Identity);
+ EXPECT_TRUE(m.EquivalentBox(size, rot));
+ EXPECT_EQ(size, sizeTrue);
+ EXPECT_EQ(rot, rotTrue);
+
+ // create new MassMatrix3d
+ // it initially has zero mass, so SetFromBox(size, rot) will fail
+ math::MassMatrix3d m2;
+ EXPECT_FALSE(m2.SetFromBox(sizeTrue, rotTrue));
+ EXPECT_TRUE(m2.SetFromBox(mass, sizeTrue, rotTrue));
+ EXPECT_EQ(m, m2);
+ }
+
+ // unit box with mass 1.0
+ {
+ const double mass = 1.0;
+ const math::Vector3d size(1, 1, 1);
+ double ixx = mass/12 * (std::pow(size.Y(), 2) + std::pow(size.Z(), 2));
+ double iyy = mass/12 * (std::pow(size.Z(), 2) + std::pow(size.X(), 2));
+ double izz = mass/12 * (std::pow(size.X(), 2) + std::pow(size.Y(), 2));
+ math::Vector3d ixxyyzz(ixx, iyy, izz);
+ math::MassMatrix3d m(mass, ixxyyzz, math::Vector3d::Zero);
+ math::Vector3d size2;
+ math::Quaterniond rot;
+ EXPECT_TRUE(m.EquivalentBox(size2, rot));
+ EXPECT_EQ(size, size2);
+ EXPECT_EQ(rot, math::Quaterniond::Identity);
+
+ math::MassMatrix3d m2;
+ EXPECT_TRUE(m2.SetFromBox(mass, size, rot));
+ EXPECT_EQ(m, m2);
+ }
+
+ // box 1x4x9
+ {
+ const double mass = 12.0;
+ const math::Vector3d ixxyyzz(97, 82, 17);
+ math::MassMatrix3d m(mass, ixxyyzz, math::Vector3d::Zero);
+ math::Vector3d size;
+ math::Quaterniond rot;
+ EXPECT_TRUE(m.EquivalentBox(size, rot));
+ EXPECT_EQ(size, math::Vector3d(1, 4, 9));
+ EXPECT_EQ(rot, math::Quaterniond::Identity);
+
+ math::MassMatrix3d m2;
+ EXPECT_TRUE(m2.SetFromBox(mass, size, rot));
+ EXPECT_EQ(m, m2);
+ }
+
+ // box 1x4x9 rotated by 90 degrees around Z
+ {
+ const double mass = 12.0;
+ const math::Vector3d ixxyyzz(82, 17, 97);
+ math::MassMatrix3d m(mass, ixxyyzz, math::Vector3d::Zero);
+ math::Vector3d size;
+ math::Quaterniond rot;
+ EXPECT_TRUE(m.EquivalentBox(size, rot, -1e-6));
+ EXPECT_EQ(size, math::Vector3d(9, 4, 1));
+ EXPECT_EQ(rot, math::Quaterniond(0, 0, IGN_PI/2));
+
+ math::MassMatrix3d m2;
+ EXPECT_TRUE(m2.SetFromBox(mass, size, rot));
+ EXPECT_EQ(m, m2);
+ }
+
+ // box 1x4x9 rotated by 45 degrees around Z
+ {
+ const double mass = 12.0;
+ const math::Vector3d ixxyyzz(49.5, 49.5, 97);
+ const math::Vector3d ixyxzyz(-32.5, 0.0, 0.0);
+ math::MassMatrix3d m(mass, ixxyyzz, ixyxzyz);
+ math::Vector3d size;
+ math::Quaterniond rot;
+ EXPECT_TRUE(m.EquivalentBox(size, rot));
+ EXPECT_EQ(size, math::Vector3d(9, 4, 1));
+ // There are multiple correct rotations due to box symmetry
+ EXPECT_TRUE(rot == math::Quaterniond(0, 0, IGN_PI/4) ||
+ rot == math::Quaterniond(IGN_PI, 0, IGN_PI/4));
+
+ math::MassMatrix3d m2;
+ EXPECT_TRUE(m2.SetFromBox(mass, size, rot));
+ EXPECT_EQ(m, m2);
+ }
+
+ // long slender box
+ {
+ const double mass = 12.0;
+ const math::Vector3d ixxyyzz(1, 1, 2e-6);
+ math::MassMatrix3d m(mass, ixxyyzz, math::Vector3d::Zero);
+ math::Vector3d size;
+ math::Quaterniond rot;
+ EXPECT_TRUE(m.EquivalentBox(size, rot));
+ EXPECT_EQ(size, math::Vector3d(1e-3, 1e-3, 1));
+ EXPECT_EQ(rot, math::Quaterniond::Identity);
+
+ math::MassMatrix3d m2;
+ EXPECT_TRUE(m2.SetFromBox(mass, size, rot));
+ EXPECT_EQ(m, m2);
+ }
+}
+
+/////////////////////////////////////////////////
+TEST(MassMatrix3dTest, SetFromCylinderZ)
+{
+ const math::Quaterniond q0 = math::Quaterniond::Identity;
+
+ // Default mass matrix with non-positive inertia
+ {
+ math::MassMatrix3d m;
+
+ // input is all zeros, so SetFromCylinderZ should fail
+ EXPECT_FALSE(m.SetFromCylinderZ(0, 0, 0, q0));
+ EXPECT_FALSE(m.SetFromCylinderZ(0, 0, q0));
+
+ // even if some parameters are valid, none should be set if they
+ // are not all valid
+ EXPECT_FALSE(m.SetFromCylinderZ(1, 0, 0, q0));
+ EXPECT_FALSE(m.SetFromCylinderZ(1, 1, 0, q0));
+ EXPECT_FALSE(m.SetFromCylinderZ(1, 0, 1, q0));
+ EXPECT_EQ(m.Mass(), 0.0);
+ }
+
+ // unit cylinder with mass 1.0
+ {
+ const double mass = 1.0;
+ const double length = 1.0;
+ const double radius = 0.5;
+ math::MassMatrix3d m;
+ EXPECT_TRUE(m.SetFromCylinderZ(mass, length, radius, q0));
+
+ double ixx = mass / 12.0 * (3*std::pow(radius, 2) + std::pow(length, 2));
+ double iyy = ixx;
+ double izz = mass / 2.0 * std::pow(radius, 2);
+ const math::Vector3d ixxyyzz(ixx, iyy, izz);
+ EXPECT_EQ(m.DiagonalMoments(), ixxyyzz);
+ EXPECT_EQ(m.OffDiagonalMoments(), math::Vector3d::Zero);
+
+ // double the length and radius
+ EXPECT_TRUE(m.SetFromCylinderZ(mass, 2*length, 2*radius, q0));
+ EXPECT_EQ(m.DiagonalMoments(), 4*ixxyyzz);
+ }
+}
+
+/////////////////////////////////////////////////
+TEST(MassMatrix3dTest, SetFromSphere)
+{
+ // Default mass matrix with non-positive inertia
+ {
+ math::MassMatrix3d m;
+
+ // input is all zeros, so SetFromSphere should fail
+ EXPECT_FALSE(m.SetFromSphere(0.0, 0.0));
+ EXPECT_FALSE(m.SetFromSphere(0.0));
+
+ // even if mass is valid, it should not be set if radius is invalid
+ EXPECT_FALSE(m.SetFromSphere(1.0, 0.0));
+ EXPECT_EQ(m.Mass(), 0.0);
+ }
+
+ // unit sphere with mass 1.0
+ {
+ const double mass = 1.0;
+ const double radius = 0.5;
+ math::MassMatrix3d m;
+ EXPECT_TRUE(m.SetFromSphere(mass, radius));
+
+ double ixx = 0.4 * mass * std::pow(radius, 2);
+ double iyy = ixx;
+ double izz = ixx;
+ const math::Vector3d ixxyyzz(ixx, iyy, izz);
+ EXPECT_EQ(m.DiagonalMoments(), ixxyyzz);
+ EXPECT_EQ(m.OffDiagonalMoments(), math::Vector3d::Zero);
+
+ // double the radius
+ EXPECT_TRUE(m.SetFromSphere(mass, 2*radius));
+ EXPECT_EQ(m.DiagonalMoments(), 4*ixxyyzz);
+ }
}
diff --git a/src/Matrix3_TEST.cc b/src/Matrix3_TEST.cc
index 4b651ce..59ca7dd 100644
--- a/src/Matrix3_TEST.cc
+++ b/src/Matrix3_TEST.cc
@@ -15,7 +15,6 @@
*
*/
-#define _USE_MATH_DEFINES
#include <gtest/gtest.h>
#include "ignition/math/Helpers.hh"
@@ -329,3 +328,73 @@ TEST(Matrix3dTest, Transpose)
EXPECT_EQ(m, mT);
}
+/////////////////////////////////////////////////
+TEST(Matrix3dTest, From2Axes)
+{
+ math::Vector3d v1(1.0, 0.0, 0.0);
+ math::Vector3d v2(0.0, 1.0, 0.0);
+
+ math::Matrix3d m1;
+ m1.From2Axes(v1, v2);
+
+ math::Matrix3d m2;
+ m2.From2Axes(v2, v1);
+
+ math::Matrix3d m1Correct(0, -1, 0,
+ 1, 0, 0,
+ 0, 0, 1);
+ math::Matrix3d m2Correct(m1Correct);
+ m2Correct.Transpose();
+
+ EXPECT_NE(m1, m2);
+ EXPECT_EQ(m1Correct, m1);
+ EXPECT_EQ(m2Correct, m2);
+ EXPECT_EQ(math::Matrix3d::Identity, m1 * m2);
+ EXPECT_EQ(v2, m1 * v1);
+ EXPECT_EQ(v1, m2 * v2);
+
+ // rotation about 45 degrees
+ v1.Set(1.0, 0.0, 0.0);
+ v2.Set(1.0, 1.0, 0.0);
+ m2.From2Axes(v1, v2);
+ // m1 is 90 degrees rotation
+ EXPECT_EQ(m1, m2*m2);
+
+ // with non-unit vectors
+ v1.Set(0.5, 0.5, 0);
+ v2.Set(-0.5, 0.5, 0);
+
+ m1.From2Axes(v1, v2);
+ m2.From2Axes(v2, v1);
+
+ EXPECT_NE(m1, m2);
+ EXPECT_EQ(m1Correct, m1);
+ EXPECT_EQ(m2Correct, m2);
+ EXPECT_EQ(math::Matrix3d::Identity, m1 * m2);
+ EXPECT_EQ(v2, m1 * v1);
+ EXPECT_EQ(v1, m2 * v2);
+
+ // For zero-length vectors, a unit matrix is returned
+ v1.Set(0, 0, 0);
+ v2.Set(-0.5, 0.5, 0);
+ m1.From2Axes(v1, v2);
+ EXPECT_EQ(math::Matrix3d::Identity, m1);
+
+ // For zero-length vectors, a unit matrix is returned
+ v1.Set(-0.5, 0.5, 0);
+ v2.Set(0, 0, 0);
+ m1.From2Axes(v1, v2);
+ EXPECT_EQ(math::Matrix3d::Identity, m1);
+
+ // Parallel vectors
+ v1.Set(1, 0, 0);
+ v2.Set(2, 0, 0);
+ m1.From2Axes(v1, v2);
+ EXPECT_EQ(math::Matrix3d::Identity, m1);
+
+ // Opposite vectors
+ v1.Set(1, 0, 0);
+ v2.Set(-2, 0, 0);
+ m1.From2Axes(v1, v2);
+ EXPECT_EQ(math::Matrix3d::Zero - math::Matrix3d::Identity, m1);
+}
diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc
index 27430c2..d5c157e 100644
--- a/src/Plane_TEST.cc
+++ b/src/Plane_TEST.cc
@@ -44,8 +44,11 @@ TEST(PlaneTest, Distance)
EXPECT_NEAR(plane.Distance(Vector3d(0, 0, 0.2),
Vector3d(0, 0, 1)), -0.1, 1e-6);
+// MSVC reports a warning about division by zero
+#ifndef _MSC_VER
EXPECT_NEAR(plane.Distance(Vector3d(0, 0, 0.1),
Vector3d(1, 0, 0)), 0, 1e-6);
+#endif
}
/////////////////////////////////////////////////
diff --git a/src/Pose_TEST.cc b/src/Pose_TEST.cc
index aecdef5..b91db05 100644
--- a/src/Pose_TEST.cc
+++ b/src/Pose_TEST.cc
@@ -15,7 +15,6 @@
*
*/
-#define _USE_MATH_DEFINES
#include <gtest/gtest.h>
#include "ignition/math/Helpers.hh"
diff --git a/src/Quaternion_TEST.cc b/src/Quaternion_TEST.cc
index aca7432..4ae7322 100644
--- a/src/Quaternion_TEST.cc
+++ b/src/Quaternion_TEST.cc
@@ -15,7 +15,6 @@
*
*/
-#define _USE_MATH_DEFINES
#include <gtest/gtest.h>
#include <cmath>
@@ -185,7 +184,7 @@ TEST(QuaternionTest, Integrate)
// expect no change.
{
const math::Quaterniond q(0.5, 0.5, 0.5, 0.5);
- const double fourPi = 4 * M_PI;
+ const double fourPi = 4 * IGN_PI;
math::Quaterniond qX = q.Integrate(math::Vector3d::UnitX, fourPi);
math::Quaterniond qY = q.Integrate(math::Vector3d::UnitY, fourPi);
math::Quaterniond qZ = q.Integrate(math::Vector3d::UnitZ, fourPi);
@@ -346,7 +345,7 @@ TEST(QuaternionTest, Math)
// Test RPY fixed-body-frame convention:
// Rotate each unit vector in roll and pitch
{
- q = math::Quaterniond(M_PI/2.0, M_PI/2.0, 0);
+ q = math::Quaterniond(IGN_PI/2.0, IGN_PI/2.0, 0);
math::Vector3d v1(1, 0, 0);
math::Vector3d r1 = q.RotateVector(v1);
// 90 degrees about X does nothing,
@@ -419,6 +418,33 @@ TEST(QuaternionTest, Math)
0.707544, 0.705561, -0.0395554, 0,
-0.344106, 0.392882, 0.85278, 0,
0, 0, 0, 1));
+
+ math::Matrix3d matFromQuat(q);
+
+ math::Quaterniond quatFromMat(matFromQuat);
+ math::Quaterniond quatFromMat2; quatFromMat2.Matrix(matFromQuat);
+
+ EXPECT_TRUE(q == quatFromMat);
+ EXPECT_TRUE(q == quatFromMat2);
+
+ // test the cases where matrix trace is negative (requires special handling)
+ q = math::Quaterniond(0, 0, 0, 1);
+ EXPECT_TRUE(q == math::Quaterniond(math::Matrix3d(
+ -1, 0, 0,
+ 0, -1, 0,
+ 0, 0, 1)));
+
+ q = math::Quaterniond(0, 0, 1, 0);
+ EXPECT_TRUE(q == math::Quaterniond(math::Matrix3d(
+ -1, 0, 0,
+ 0, 1, 0,
+ 0, 0, -1)));
+
+ q = math::Quaterniond(0, 1, 0, 0);
+ EXPECT_TRUE(q == math::Quaterniond(math::Matrix3d(
+ 1, 0, 0,
+ 0, -1, 0,
+ 0, 0, -1)));
}
}
@@ -440,3 +466,79 @@ TEST(QuaternionTest, Slerp)
math::Quaterniond q3 = math::Quaterniond::Slerp(1.0, q1, q2, true);
EXPECT_EQ(q3, math::Quaterniond(0.554528, -0.717339, 0.32579, 0.267925));
}
+
+/////////////////////////////////////////////////
+TEST(QuaterniondTest, From2Axes)
+{
+ math::Vector3d v1(1.0, 0.0, 0.0);
+ math::Vector3d v2(0.0, 1.0, 0.0);
+
+ math::Quaterniond q1;
+ q1.From2Axes(v1, v2);
+
+ math::Quaterniond q2;
+ q2.From2Axes(v2, v1);
+
+ math::Quaterniond q1Correct(sqrt(2)/2, 0, 0, sqrt(2)/2);
+ math::Quaterniond q2Correct(sqrt(2)/2, 0, 0, -sqrt(2)/2);
+
+ EXPECT_NE(q1, q2);
+ EXPECT_EQ(q1Correct, q1);
+ EXPECT_EQ(q2Correct, q2);
+ EXPECT_EQ(math::Quaterniond::Identity, q1 * q2);
+ EXPECT_EQ(v2, q1 * v1);
+ EXPECT_EQ(v1, q2 * v2);
+
+ // still the same rotation, but with non-unit vectors
+ v1.Set(0.5, 0.5, 0);
+ v2.Set(-0.5, 0.5, 0);
+
+ q1.From2Axes(v1, v2);
+ q2.From2Axes(v2, v1);
+
+ EXPECT_NE(q1, q2);
+ EXPECT_EQ(q1Correct, q1);
+ EXPECT_EQ(q2Correct, q2);
+ EXPECT_EQ(math::Quaterniond::Identity, q1 * q2);
+ EXPECT_EQ(v2, q1 * v1);
+ EXPECT_EQ(v1, q2 * v2);
+
+ // Test various settings of opposite vectors (which need special care)
+
+ v1.Set(1, 0, 0);
+ v2.Set(-1, 0, 0);
+ q1.From2Axes(v1, v2);
+ q2 = q1 * q1;
+ EXPECT_TRUE(math::equal(q2.W(), 1.0) || math::equal(q2.W(), -1.0));
+ EXPECT_TRUE(math::equal(q2.X(), 0.0));
+ EXPECT_TRUE(math::equal(q2.Y(), 0.0));
+ EXPECT_TRUE(math::equal(q2.Z(), 0.0));
+
+ v1.Set(0, 1, 0);
+ v2.Set(0, -1, 0);
+ q1.From2Axes(v1, v2);
+ q2 = q1 * q1;
+ EXPECT_TRUE(math::equal(q2.W(), 1.0) || math::equal(q2.W(), -1.0));
+ EXPECT_TRUE(math::equal(q2.X(), 0.0));
+ EXPECT_TRUE(math::equal(q2.Y(), 0.0));
+ EXPECT_TRUE(math::equal(q2.Z(), 0.0));
+
+ v1.Set(0, 0, 1);
+ v2.Set(0, 0, -1);
+ q1.From2Axes(v1, v2);
+ q2 = q1 * q1;
+ EXPECT_TRUE(math::equal(q2.W(), 1.0) || math::equal(q2.W(), -1.0));
+ EXPECT_TRUE(math::equal(q2.X(), 0.0));
+ EXPECT_TRUE(math::equal(q2.Y(), 0.0));
+ EXPECT_TRUE(math::equal(q2.Z(), 0.0));
+
+ v1.Set(0, 1, 1);
+ v2.Set(0, -1, -1);
+ q1.From2Axes(v1, v2);
+ q2 = q1 * q1;
+ EXPECT_TRUE(math::equal(q2.W(), 1.0) || math::equal(q2.W(), -1.0));
+ EXPECT_TRUE(math::equal(q2.X(), 0.0));
+ EXPECT_TRUE(math::equal(q2.Y(), 0.0));
+ EXPECT_TRUE(math::equal(q2.Z(), 0.0));
+}
+
diff --git a/src/Rand.cc b/src/Rand.cc
index 57dc9f2..bc1f9a5 100644
--- a/src/Rand.cc
+++ b/src/Rand.cc
@@ -35,7 +35,7 @@ using namespace math;
// and gui simultaneously).
uint32_t Rand::seed = std::random_device {}();
-GeneratorType *Rand::randGenerator = new GeneratorType(seed);
+std::unique_ptr<GeneratorType> Rand::randGenerator(new GeneratorType(seed));
//////////////////////////////////////////////////
void Rand::Seed(unsigned int _seed)
diff --git a/src/Rand_TEST.cc b/src/Rand_TEST.cc
index 75cb689..1660a34 100644
--- a/src/Rand_TEST.cc
+++ b/src/Rand_TEST.cc
@@ -25,20 +25,15 @@ using namespace ignition;
//////////////////////////////////////////////////
TEST(RandTest, Rand)
{
- double d;
- int i;
// TODO: implement a proper random number generator test
- d = ignition::math::Rand::DblUniform(1, 2);
- EXPECT_LE(d, 2);
+ double d = math::Rand::DblUniform(1, 2);
EXPECT_GE(d, 1);
+ EXPECT_LE(d, 2);
- d = math::Rand::DblNormal(2, 3);
- i = math::Rand::IntUniform(1, 2);
- EXPECT_LE(i, 2);
+ int i = math::Rand::IntUniform(1, 2);
EXPECT_GE(i, 1);
-
- i = math::Rand::IntNormal(2, 3);
+ EXPECT_LE(i, 2);
#ifndef _MSC_VER
{
@@ -46,11 +41,14 @@ TEST(RandTest, Rand)
math::Rand::Seed(1001);
d = math::Rand::DblNormal(2, 3);
+ i = math::Rand::IntNormal(10, 5);
// \todo OSX seems to produce different results. See issue #14.
#ifdef __APPLE__
+ EXPECT_EQ(i, 8);
EXPECT_NEAR(d, 5.01545, 1e-5);
#else
+ EXPECT_EQ(i, 9);
EXPECT_NEAR(d, 3.00618, 1e-5);
#endif
}
diff --git a/src/SignalStats.cc b/src/SignalStats.cc
index af55e1f..8c97c67 100644
--- a/src/SignalStats.cc
+++ b/src/SignalStats.cc
@@ -341,3 +341,9 @@ void SignalStats::Reset()
}
}
+//////////////////////////////////////////////////
+SignalStats &SignalStats::operator=(const SignalStats &_s)
+{
+ this->dataPtr = _s.dataPtr->Clone();
+ return *this;
+}
diff --git a/src/SignalStats_TEST.cc b/src/SignalStats_TEST.cc
index 20ce48e..ded785f 100644
--- a/src/SignalStats_TEST.cc
+++ b/src/SignalStats_TEST.cc
@@ -475,6 +475,9 @@ TEST(SignalStatsTest, SignalStats)
EXPECT_TRUE(stats.Map().empty());
EXPECT_EQ(stats.Count(), 0u);
+ math::SignalStats stats2 = stats;
+ EXPECT_EQ(stats.Count(), stats2.Count());
+
// Reset
stats.Reset();
EXPECT_TRUE(stats.Map().empty());
@@ -523,6 +526,18 @@ TEST(SignalStatsTest, SignalStats)
EXPECT_EQ(map.count("rms"), 1u);
EXPECT_EQ(map.count("var"), 1u);
EXPECT_EQ(map.count("FakeStatistic"), 0u);
+
+ math::SignalStats stats2 = stats;
+ std::map<std::string, double> map2 = stats2.Map();
+ EXPECT_FALSE(map2.empty());
+ EXPECT_EQ(map.size(), map2.size());
+ EXPECT_EQ(map.count("max"), map2.count("max"));
+ EXPECT_EQ(map.count("maxAbs"), map2.count("maxAbs"));
+ EXPECT_EQ(map.count("mean"), map2.count("mean"));
+ EXPECT_EQ(map.count("min"), map2.count("min"));
+ EXPECT_EQ(map.count("rms"), map2.count("rms"));
+ EXPECT_EQ(map.count("var"), map2.count("var"));
+ EXPECT_EQ(map.count("FakeStatistic"), map2.count("FakeStatistic"));
}
{
diff --git a/src/Temperature.cc b/src/Temperature.cc
new file mode 100644
index 0000000..711d316
--- /dev/null
+++ b/src/Temperature.cc
@@ -0,0 +1,325 @@
+/*
+ * Copyright (C) 2016 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 "ignition/math/Temperature.hh"
+
+/// \brief Private data for the Temperature class.
+class ignition::math::TemperaturePrivate
+{
+ /// \brief Default constructor
+ public: TemperaturePrivate() : kelvin(0.0) {}
+
+ /// \brief Constructor
+ /// \param[in] _temp Temperature in Kelvin
+ public: explicit TemperaturePrivate(const double _temp) : kelvin(_temp) {}
+
+ /// \brief Temperature value in Kelvin.
+ public: double kelvin;
+};
+
+using namespace ignition;
+using namespace math;
+
+/////////////////////////////////////////////////
+Temperature::Temperature()
+: dataPtr(new TemperaturePrivate)
+{
+}
+
+/////////////////////////////////////////////////
+Temperature::Temperature(const double _temp)
+: dataPtr(new TemperaturePrivate(_temp))
+{
+}
+
+/////////////////////////////////////////////////
+Temperature::Temperature(const Temperature &_temp)
+: dataPtr(new TemperaturePrivate(_temp.Kelvin()))
+{
+}
+
+/////////////////////////////////////////////////
+Temperature::~Temperature()
+{
+}
+
+/////////////////////////////////////////////////
+double Temperature::KelvinToCelsius(const double _temp)
+{
+ return _temp - 273.15;
+}
+
+/////////////////////////////////////////////////
+double Temperature::KelvinToFahrenheit(const double _temp)
+{
+ return _temp * 1.8 - 459.67;
+}
+
+/////////////////////////////////////////////////
+double Temperature::CelsiusToFahrenheit(const double _temp)
+{
+ return _temp * 1.8 + 32.0;
+}
+
+/////////////////////////////////////////////////
+double Temperature::CelsiusToKelvin(const double _temp)
+{
+ return _temp + 273.15;
+}
+
+/////////////////////////////////////////////////
+double Temperature::FahrenheitToCelsius(const double _temp)
+{
+ return (_temp - 32.0) / 1.8;
+}
+
+/////////////////////////////////////////////////
+double Temperature::FahrenheitToKelvin(const double _temp)
+{
+ return (_temp + 459.67) / 1.8;
+}
+
+/////////////////////////////////////////////////
+void Temperature::SetKelvin(const double _temp)
+{
+ this->dataPtr->kelvin = _temp;
+}
+
+/////////////////////////////////////////////////
+void Temperature::SetCelsius(const double _temp)
+{
+ this->SetKelvin(CelsiusToKelvin(_temp));
+}
+
+/////////////////////////////////////////////////
+void Temperature::SetFahrenheit(const double _temp)
+{
+ this->SetKelvin(FahrenheitToKelvin(_temp));
+}
+
+/////////////////////////////////////////////////
+double Temperature::Kelvin() const
+{
+ return this->dataPtr->kelvin;
+}
+
+/////////////////////////////////////////////////
+double Temperature::Celsius() const
+{
+ return KelvinToCelsius(this->dataPtr->kelvin);
+}
+
+/////////////////////////////////////////////////
+double Temperature::Fahrenheit() const
+{
+ return KelvinToFahrenheit(this->dataPtr->kelvin);
+}
+
+/////////////////////////////////////////////////
+double Temperature::operator()() const
+{
+ return this->dataPtr->kelvin;
+}
+
+/////////////////////////////////////////////////
+Temperature &Temperature::operator=(const double _temp)
+{
+ this->SetKelvin(_temp);
+ return *this;
+}
+
+/////////////////////////////////////////////////
+Temperature &Temperature::operator=(const Temperature &_temp)
+{
+ this->SetKelvin(_temp.Kelvin());
+ return *this;
+}
+
+/////////////////////////////////////////////////
+Temperature Temperature::operator+(const double _temp)
+{
+ return this->dataPtr->kelvin + _temp;
+}
+
+/////////////////////////////////////////////////
+Temperature Temperature::operator+(const Temperature &_temp)
+{
+ return this->dataPtr->kelvin + _temp;
+}
+
+/////////////////////////////////////////////////
+const Temperature &Temperature::operator+=(const double _temp)
+{
+ this->dataPtr->kelvin += _temp;
+ return *this;
+}
+
+/////////////////////////////////////////////////
+const Temperature &Temperature::operator+=(const Temperature &_temp)
+{
+ this->dataPtr->kelvin += _temp.Kelvin();
+ return *this;
+}
+
+/////////////////////////////////////////////////
+Temperature Temperature::operator-(const double _temp)
+{
+ return this->dataPtr->kelvin - _temp;
+}
+
+/////////////////////////////////////////////////
+Temperature Temperature::operator-(const Temperature &_temp)
+{
+ return this->dataPtr->kelvin - _temp.Kelvin();
+}
+
+/////////////////////////////////////////////////
+const Temperature &Temperature::operator-=(const double _temp)
+{
+ this->dataPtr->kelvin -= _temp;
+ return *this;
+}
+
+/////////////////////////////////////////////////
+const Temperature &Temperature::operator-=(const Temperature &_temp)
+{
+ this->dataPtr->kelvin -= _temp.Kelvin();
+ return *this;
+}
+
+/////////////////////////////////////////////////
+Temperature Temperature::operator*(const double _temp)
+{
+ return Temperature(this->dataPtr->kelvin * _temp);
+}
+
+/////////////////////////////////////////////////
+Temperature Temperature::operator*(const Temperature &_temp)
+{
+ return Temperature(this->dataPtr->kelvin * _temp.Kelvin());
+}
+
+/////////////////////////////////////////////////
+const Temperature &Temperature::operator*=(const double _temp)
+{
+ this->dataPtr->kelvin *= _temp;
+ return *this;
+}
+
+/////////////////////////////////////////////////
+const Temperature &Temperature::operator*=(const Temperature &_temp)
+{
+ this->dataPtr->kelvin *= _temp.Kelvin();
+ return *this;
+}
+
+/////////////////////////////////////////////////
+Temperature Temperature::operator/(const double _temp)
+{
+ return Temperature(this->dataPtr->kelvin / _temp);
+}
+
+/////////////////////////////////////////////////
+Temperature Temperature::operator/(const Temperature &_temp)
+{
+ return Temperature(this->dataPtr->kelvin / _temp.Kelvin());
+}
+
+/////////////////////////////////////////////////
+const Temperature &Temperature::operator/=(const double _temp)
+{
+ this->dataPtr->kelvin /= _temp;
+ return *this;
+}
+
+/////////////////////////////////////////////////
+const Temperature &Temperature::operator/=(const Temperature &_temp)
+{
+ this->dataPtr->kelvin /= _temp.Kelvin();
+ return *this;
+}
+
+/////////////////////////////////////////////////
+bool Temperature::operator==(const Temperature &_temp) const
+{
+ return ignition::math::equal(this->dataPtr->kelvin, _temp.Kelvin());
+}
+
+/////////////////////////////////////////////////
+bool Temperature::operator==(const double _temp) const
+{
+ return ignition::math::equal(this->dataPtr->kelvin, _temp);
+}
+
+/////////////////////////////////////////////////
+bool Temperature::operator!=(const Temperature &_temp) const
+{
+ return !(*this == _temp);
+}
+
+/////////////////////////////////////////////////
+bool Temperature::operator!=(const double _temp) const
+{
+ return !(*this == _temp);
+}
+
+/////////////////////////////////////////////////
+bool Temperature::operator<(const Temperature &_temp) const
+{
+ return this->dataPtr->kelvin < _temp.Kelvin();
+}
+
+/////////////////////////////////////////////////
+bool Temperature::operator<(const double _temp) const
+{
+ return this->dataPtr->kelvin < _temp;
+}
+
+/////////////////////////////////////////////////
+bool Temperature::operator<=(const Temperature &_temp) const
+{
+ return this->dataPtr->kelvin <= _temp.Kelvin();
+}
+
+/////////////////////////////////////////////////
+bool Temperature::operator<=(const double _temp) const
+{
+ return this->dataPtr->kelvin <= _temp;
+}
+
+/////////////////////////////////////////////////
+bool Temperature::operator>(const Temperature &_temp) const
+{
+ return this->dataPtr->kelvin > _temp.Kelvin();
+}
+
+/////////////////////////////////////////////////
+bool Temperature::operator>(const double _temp) const
+{
+ return this->dataPtr->kelvin > _temp;
+}
+
+/////////////////////////////////////////////////
+bool Temperature::operator>=(const Temperature &_temp) const
+{
+ return this->dataPtr->kelvin >= _temp.Kelvin();
+}
+
+/////////////////////////////////////////////////
+bool Temperature::operator>=(const double _temp) const
+{
+ return this->dataPtr->kelvin >= _temp;
+}
diff --git a/src/Temperature_TEST.cc b/src/Temperature_TEST.cc
new file mode 100644
index 0000000..0cb39cc
--- /dev/null
+++ b/src/Temperature_TEST.cc
@@ -0,0 +1,178 @@
+/*
+ * Copyright (C) 2016 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/Temperature.hh"
+
+using namespace ignition;
+using namespace math;
+
+/////////////////////////////////////////////////
+TEST(TemperatureTest, Constructor)
+{
+ Temperature temp;
+ EXPECT_NEAR(temp.Kelvin(), 0.0, 1e-6);
+
+ Temperature temp2(1.1);
+ EXPECT_NEAR(temp2.Kelvin(), 1.1, 1e-6);
+
+ Temperature temp3(temp2);
+ EXPECT_NEAR(temp3.Kelvin(), 1.1, 1e-6);
+ EXPECT_NEAR(temp3.Celsius(), -272.05, 1e-6);
+
+ EXPECT_TRUE(temp2 == temp3);
+ EXPECT_TRUE(temp2 == 1.1);
+ EXPECT_TRUE(temp2 != temp);
+ EXPECT_TRUE(temp2 != 1.2);
+
+ EXPECT_TRUE(temp < temp2);
+ EXPECT_TRUE(temp < 10.0);
+ EXPECT_TRUE(temp <= temp2);
+ EXPECT_TRUE(temp <= 0.0);
+ EXPECT_TRUE(temp <= 0.1);
+
+ EXPECT_FALSE(temp > temp2);
+ EXPECT_FALSE(temp > 80.0);
+ EXPECT_FALSE(temp >= temp2);
+ EXPECT_FALSE(temp >= 0.1);
+ EXPECT_TRUE(temp >= 0.0);
+}
+
+/////////////////////////////////////////////////
+TEST(TemperatureTest, Conversions)
+{
+ EXPECT_NEAR(Temperature::KelvinToCelsius(0), -273.15, 1e-6);
+ EXPECT_NEAR(Temperature::KelvinToFahrenheit(300), 80.33, 1e-6);
+
+ EXPECT_NEAR(Temperature::CelsiusToFahrenheit(20.0), 68.0, 1e-6);
+ EXPECT_NEAR(Temperature::CelsiusToKelvin(10.0), 283.15, 1e-6);
+
+ EXPECT_NEAR(Temperature::FahrenheitToCelsius(-40.0),
+ Temperature::CelsiusToFahrenheit(-40.0), 1e-6);
+ EXPECT_NEAR(Temperature::FahrenheitToKelvin(60.0), 288.7055, 1e-3);
+}
+
+/////////////////////////////////////////////////
+TEST(TemperatureTest, MutatorsAccessors)
+{
+ Temperature temp;
+ EXPECT_NEAR(temp.Kelvin(), 0.0, 1e-6);
+
+ temp.SetKelvin(10);
+ EXPECT_NEAR(temp.Kelvin(), 10.0, 1e-6);
+
+ temp.SetCelsius(20);
+ EXPECT_NEAR(temp(), 293.15, 1e-6);
+
+ temp.SetFahrenheit(30);
+ EXPECT_NEAR(temp.Fahrenheit(), 30.0, 1e-6);
+ EXPECT_NEAR(temp(), 272.0388889, 1e-6);
+}
+
+/////////////////////////////////////////////////
+TEST(TemperatureTest, Operators)
+{
+ Temperature temp(20);
+ EXPECT_NEAR(temp(), 20, 1e-6);
+
+ temp = 30;
+ EXPECT_NEAR(temp(), 30, 1e-6);
+
+ Temperature temp2 = temp;
+ // cppcheck-suppress knownConditionTrueFalse
+ EXPECT_TRUE(temp == temp2);
+
+ EXPECT_NEAR((temp + temp2).Kelvin(), 60, 1e-6);
+ EXPECT_NEAR((temp + 40).Kelvin(), 70, 1e-6);
+
+ EXPECT_NEAR((temp - temp2).Kelvin(), 0, 1e-6);
+ EXPECT_NEAR((temp - 20).Kelvin(), 10.0, 1e-6);
+
+ EXPECT_NEAR((temp * temp2).Kelvin(), 900, 1e-6);
+ EXPECT_NEAR((temp * 2).Kelvin(), 60.0, 1e-6);
+
+ EXPECT_NEAR((temp / temp2).Kelvin(), 1.0, 1e-6);
+ EXPECT_NEAR((temp / 2).Kelvin(), 15.0, 1e-6);
+
+ temp += temp2;
+ EXPECT_NEAR(temp.Kelvin(), 60.0, 1e-6);
+ temp -= temp2;
+ EXPECT_NEAR(temp.Kelvin(), 30.0, 1e-6);
+
+ temp += 5.0;
+ EXPECT_NEAR(temp.Kelvin(), 35.0, 1e-6);
+ temp -= 5.0;
+ EXPECT_NEAR(temp.Kelvin(), 30.0, 1e-6);
+
+ temp *= temp2;
+ EXPECT_NEAR(temp.Kelvin(), 900, 1e-6);
+ temp /= temp2;
+ EXPECT_NEAR(temp.Kelvin(), 30, 1e-6);
+
+ temp *= 4.0;
+ EXPECT_NEAR(temp.Kelvin(), 120, 1e-6);
+ temp /= 4.0;
+ EXPECT_NEAR(temp.Kelvin(), 30, 1e-6);
+
+ Temperature temp3;
+ temp3 = temp;
+ EXPECT_TRUE(temp3 == temp);
+ EXPECT_TRUE(temp3 == temp2);
+}
+
+/////////////////////////////////////////////////
+TEST(TemperatureTest, OperatorStreamOut)
+{
+ Temperature temp(55.45);
+ std::ostringstream stream;
+ stream << temp;
+ EXPECT_EQ(stream.str(), "55.45");
+}
+
+/////////////////////////////////////////////////
+TEST(TemperatureTest, OperatorStreamIn)
+{
+ Temperature temp;
+ std::istringstream stream("23.4");
+ stream >> temp;
+ EXPECT_NEAR(temp.Kelvin(), 23.4, 1e-6);
+}
+
+/////////////////////////////////////////////////
+TEST(TemperatureTest, Negative)
+{
+ Temperature temp(235.0);
+
+ Temperature temp2 = 103.0 - temp;
+ EXPECT_NEAR(temp2.Kelvin(), -132.0, 1e-6);
+
+ Temperature temp3 = 133.0 + temp2;
+ EXPECT_NEAR(temp3.Kelvin(), 1.0, 1e-6);
+
+ Temperature temp4 = 4.0 * temp3;
+ EXPECT_NEAR(temp4.Kelvin(), 4.0, 1e-6);
+
+ Temperature temp5 = 2.0 / temp3;
+ EXPECT_NEAR(temp5.Kelvin(), 2.0, 1e-6);
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/src/Triangle3_TEST.cc b/src/Triangle3_TEST.cc
deleted file mode 100644
index fba27d7..0000000
--- a/src/Triangle3_TEST.cc
+++ /dev/null
@@ -1,258 +0,0 @@
-/*
- * 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/Triangle3.hh"
-#include "ignition/math/Helpers.hh"
-
-using namespace ignition;
-using namespace math;
-
-/////////////////////////////////////////////////
-TEST(Triangle3Test, Constructor)
-{
- {
- // Default constructor
- Triangle3d tri;
- EXPECT_EQ(tri[0], Vector3d(0, 0, 0));
- EXPECT_EQ(tri[1], Vector3d(0, 0, 0));
- EXPECT_EQ(tri[2], Vector3d(0, 0, 0));
- EXPECT_FALSE(tri.Valid());
- }
-
- {
- // Construct from three points
- Triangle3d tri(Vector3d(0, 0, 0), Vector3d(0, 1, 0), Vector3d(1, 0, 0));
-
- EXPECT_TRUE(tri.Valid());
-
- EXPECT_EQ(tri[0], Vector3d(0, 0, 0));
- EXPECT_EQ(tri[1], Vector3d(0, 1, 0));
- EXPECT_EQ(tri[2], Vector3d(1, 0, 0));
- EXPECT_THROW(tri[3], IndexException);
- }
-
- {
- // Construct degenerate from 3 collinear points
- Triangle3d tri(Vector3d(0, 0, 0), Vector3d(0, 1, 0), Vector3d(0, 2, 0));
-
- // Expect not Valid
- EXPECT_FALSE(tri.Valid());
- }
-}
-
-/////////////////////////////////////////////////
-TEST(Triangle3Test, Set)
-{
- Triangle3d tri;
-
- tri.Set(0, Vector3d(3, 4, 1));
- tri.Set(1, Vector3d(5, -6, 2));
- tri.Set(2, Vector3d(7, 8, -3));
- EXPECT_EQ(tri[0], Vector3d(3, 4, 1));
- EXPECT_EQ(tri[1], Vector3d(5, -6, 2));
- EXPECT_EQ(tri[2], Vector3d(7, 8, -3));
-
- tri.Set(Vector3d(0.1, 0.2, -0.3),
- Vector3d(0.3, -0.4, 0.5),
- Vector3d(1.5, -2.6, 3.7));
- EXPECT_EQ(tri[0], Vector3d(0.1, 0.2, -0.3));
- EXPECT_EQ(tri[1], Vector3d(0.3, -0.4, 0.5));
- EXPECT_EQ(tri[2], Vector3d(1.5, -2.6, 3.7));
-
- EXPECT_THROW(tri.Set(3, Vector3d(1.5, 2.6, 3.8)), IndexException);
-}
-
-/////////////////////////////////////////////////
-TEST(Triangle3Test, Side)
-{
- Triangle3d tri(Vector3d(0, 0, 0), Vector3d(0, 1, 1), Vector3d(1, 0, 2));
-
- EXPECT_TRUE(tri.Side(0) == Line3d(0, 0, 0, 0, 1, 1));
- EXPECT_TRUE(tri.Side(1) == Line3d(0, 1, 1, 1, 0, 2));
- EXPECT_TRUE(tri.Side(2) == Line3d(1, 0, 2, 0, 0, 0));
-
- EXPECT_THROW(tri.Side(3), IndexException);
-}
-
-/////////////////////////////////////////////////
-TEST(Triangle3Test, ContainsLine)
-{
- Triangle3d tri(Vector3d(0, 0, 0), Vector3d(0, 1, 0), Vector3d(1, 0, 0));
-
- EXPECT_TRUE(tri.Contains(tri.Side(0)));
- EXPECT_TRUE(tri.Contains(tri.Side(1)));
- EXPECT_TRUE(tri.Contains(tri.Side(2)));
-
- EXPECT_TRUE(tri.Contains(Line3d(0.1, 0.1, 0, 0.5, 0.5, 0)));
-
- EXPECT_FALSE(tri.Contains(Line3d(0.1, 0.1, 0, 0.6, 0.6, 0)));
- EXPECT_FALSE(tri.Contains(Line3d(-0.1, -0.1, 0, 0.5, 0.5, 0)));
- EXPECT_FALSE(tri.Contains(Line3d(0.1, 0.1, 0.1, 0.5, 0.5, 0.1)));
- EXPECT_FALSE(tri.Contains(Line3d(0.1, 0.1, -0.1, 0.1, 0.1, 0.1)));
-}
-
-/////////////////////////////////////////////////
-TEST(Triangle3Test, Intersects)
-{
- Vector3d pt1;
- Triangle3d tri(Vector3d(0, 0, 0),
- Vector3d(0, 1, 0),
- Vector3d(1, 0, 0));
-
- EXPECT_TRUE(tri.Intersects(tri.Side(0), pt1));
- EXPECT_EQ(pt1, Vector3d(0, 0, 0));
-
- EXPECT_TRUE(tri.Intersects(tri.Side(1), pt1));
- EXPECT_EQ(pt1, Vector3d(0, 1, 0));
-
- EXPECT_TRUE(tri.Intersects(tri.Side(2), pt1));
- EXPECT_EQ(pt1, Vector3d(1, 0, 0));
-
- EXPECT_TRUE(tri.Intersects(Line3d(0.1, 0.1, 0, 0.5, 0.5, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0.1, 0.1, 0));
-
- EXPECT_TRUE(tri.Intersects(Line3d(0.1, 0.1, 0, 0.6, 0.6, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0.5, 0.5, 0));
-
- EXPECT_TRUE(tri.Intersects(Line3d(0.6, 0.6, 0, 0.1, 0.1, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0.5, 0.5, 0));
-
- EXPECT_TRUE(tri.Intersects(Line3d(0.1, 0.1, 0, -0.6, 0.1, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0.0, 0.1, 0));
-
- EXPECT_TRUE(tri.Intersects(Line3d(0.1, 0.1, 0, 0.1, -0.1, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0.1, 0, 0));
-
- EXPECT_TRUE(tri.Intersects(Line3d(-0.1, -0.1, 0, 0.5, 0.5, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0.0, 0.0, 0));
-
- EXPECT_TRUE(tri.Intersects(Line3d(-2, -2, 0, 0.2, 0.2, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0.0, 0.0, 0));
-
- EXPECT_TRUE(tri.Intersects(Line3d(0.1, 0.1, -1, 0.1, 0.1, 1), pt1));
- EXPECT_EQ(pt1, Vector3d(0.1, 0.1, 0));
-
- EXPECT_TRUE(tri.Intersects(Line3d(0.1, 0.1, -1, 0.3, 0.3, 1), pt1));
- EXPECT_EQ(pt1, Vector3d(0.2, 0.2, 0));
-
- EXPECT_FALSE(tri.Intersects(Line3d(-0.1, 0, 0, -0.1, 1, 0), pt1));
-
- // A flat triangle raised along the z-axis
- {
- tri.Set(Vector3d(0, 0, 0.5),
- Vector3d(0, 1, 0.5),
- Vector3d(1, 0, 0.5));
- EXPECT_TRUE(tri.Intersects(Line3d(0.5, 0.5, 2, 0.5, 0.5, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0.5, 0.5, 0.5));
- }
-
- // An angled triangle
- {
- tri.Set(Vector3d(0, 0, 1),
- Vector3d(1, 0, 0),
- Vector3d(0, 1, 0));
-
- EXPECT_TRUE(tri.Intersects(Line3d(1, 0, 2, 1, 0, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(1, 0, 0));
-
- EXPECT_FALSE(tri.Intersects(Line3d(1.1, 0, 2, 1.1, 0, 0), pt1));
- EXPECT_FALSE(tri.Intersects(Line3d(0, 1.1, 2, 0, 1.1, 0), pt1));
- EXPECT_FALSE(tri.Intersects(Line3d(-0.1, 0, 2, -0.1, 0, 0), pt1));
- EXPECT_FALSE(tri.Intersects(Line3d(0.51, 0.51, 2, 0.51, 0.51, 0), pt1));
-
- EXPECT_TRUE(tri.Intersects(Line3d(0, 1, 2, 0, 1, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0, 1, 0));
-
- EXPECT_TRUE(tri.Intersects(Line3d(0, 0, 2, 0, 0, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0, 0, 1.0));
-
- EXPECT_TRUE(tri.Intersects(Line3d(0.1, 0.1, 2, 0.1, 0.1, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0.1, 0.1, 0.8));
-
- EXPECT_TRUE(tri.Intersects(Line3d(0.2, 0.2, 2, 0.2, 0.2, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0.2, 0.2, 0.6));
-
- EXPECT_TRUE(tri.Intersects(Line3d(0.3, 0.3, 2, 0.3, 0.3, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0.3, 0.3, 0.4));
-
- EXPECT_TRUE(tri.Intersects(Line3d(0.4, 0.4, 2, 0.4, 0.4, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0.4, 0.4, 0.2));
-
- EXPECT_TRUE(tri.Intersects(Line3d(0.5, 0.5, 2, 0.5, 0.5, 0), pt1));
- EXPECT_EQ(pt1, Vector3d(0.5, 0.5, 0));
- }
-}
-
-/////////////////////////////////////////////////
-TEST(Triangle3Test, ContainsPt)
-{
- Triangle3d tri(Vector3d(0, 0, 0),
- Vector3d(0, 1, 0),
- Vector3d(1, 0, 0));
-
- EXPECT_TRUE(tri.Contains(tri[0]));
- EXPECT_TRUE(tri.Contains(tri[1]));
- EXPECT_TRUE(tri.Contains(tri[2]));
-
- EXPECT_TRUE(tri.Contains(Vector3d(0.1, 0.1, 0)));
- EXPECT_TRUE(tri.Contains(Vector3d(0, 0.5, 0)));
- EXPECT_TRUE(tri.Contains(Vector3d(0.5, 0, 0)));
- EXPECT_TRUE(tri.Contains(Vector3d(0.5, 0.5, 0)));
-
- EXPECT_FALSE(tri.Contains(Vector3d(-0.01, -0.01, 0)));
- EXPECT_FALSE(tri.Contains(Vector3d(1.01, 0, 0)));
- EXPECT_FALSE(tri.Contains(Vector3d(0, 1.01, 0)));
- EXPECT_FALSE(tri.Contains(Vector3d(0.1, 0.1, 0.1)));
- EXPECT_FALSE(tri.Contains(Vector3d(0.1, 0.1, -0.1)));
-}
-
-/////////////////////////////////////////////////
-TEST(Triangle3Test, Perimeter)
-{
- {
- Triangle3d tri(Vector3d(0, 0, 0),
- Vector3d(0, 1, 0),
- Vector3d(1, 0, 0));
-
- EXPECT_DOUBLE_EQ(tri.Perimeter(), 2.0 + sqrt(2.0));
- }
- {
- Triangle3d tri(Vector3d(0, 0, 1),
- Vector3d(0, 1, 0),
- Vector3d(1, 0, 0));
-
- EXPECT_DOUBLE_EQ(tri.Perimeter(), 3*sqrt(2.0));
- }
-}
-
-/////////////////////////////////////////////////
-TEST(Triangle3Test, Area)
-{
- {
- Triangle3d tri(Vector3d(0, 0, 0), Vector3d(0, 1, 0), Vector3d(1, 0, 0));
-
- EXPECT_NEAR(tri.Area(), 0.5, 1e-6);
- }
- {
- Triangle3d tri(Vector3d(0, 0, 1), Vector3d(0, 1, 0), Vector3d(1, 0, 0));
-
- // (base * height) / 2
- EXPECT_NEAR(tri.Area(), (sqrt(2) * sqrt(1.5))*0.5, 1e-6);
- }
-}
diff --git a/src/Vector2_TEST.cc b/src/Vector2_TEST.cc
index 34aec10..79ab69e 100644
--- a/src/Vector2_TEST.cc
+++ b/src/Vector2_TEST.cc
@@ -15,9 +15,6 @@
*
*/
-#ifndef _USE_MATH_DEFINES
-#define _USE_MATH_DEFINES
-#endif
#include <gtest/gtest.h>
#include <cmath>
@@ -294,7 +291,7 @@ TEST(Vector2dTest, Length)
EXPECT_DOUBLE_EQ(math::Vector2d::Zero.SquaredLength(), 0.0);
// One vector
- EXPECT_NEAR(math::Vector2d::One.Length(), M_SQRT2, 1e-10);
+ EXPECT_NEAR(math::Vector2d::One.Length(), IGN_SQRT2, 1e-10);
EXPECT_DOUBLE_EQ(math::Vector2d::One.SquaredLength(), 2.0);
// Arbitrary vector
diff --git a/src/Vector3_TEST.cc b/src/Vector3_TEST.cc
index fcc85e9..fa85f27 100644
--- a/src/Vector3_TEST.cc
+++ b/src/Vector3_TEST.cc
@@ -171,6 +171,10 @@ TEST(Vector3dTest, Normalize)
vec3 = vec2.Normalize();
EXPECT_EQ(vec3, vec2);
EXPECT_EQ(vec2, math::Vector3d(0.267261, 0.534522, 0.801784));
+
+ const math::Vector3d vecConst(1, 2, 3);
+ EXPECT_EQ(vecConst.Normalized(), vec3);
+ EXPECT_EQ(vecConst, math::Vector3d(1, 2, 3));
}
/////////////////////////////////////////////////
diff --git a/tools/code_check.sh b/tools/code_check.sh
index e5f79be..793bc52 100644
--- a/tools/code_check.sh
+++ b/tools/code_check.sh
@@ -65,13 +65,15 @@ SUPPRESS=/tmp/cpp_check.suppress
echo "" >> $SUPPRESS
#cppcheck
-CPPCHECK_BASE="cppcheck -q --suppressions-list=$SUPPRESS"
+CPPCHECK_BASE="cppcheck -q --suppressions-list=$SUPPRESS --inline-suppr"
if [ $CPPCHECK_LT_157 -eq 0 ]; then
# use --language argument if 1.57 or greater (issue #907)
CPPCHECK_BASE="$CPPCHECK_BASE --language=c++"
fi
CPPCHECK_INCLUDES="-I ./include -I $builddir -I test -I ./include/ignition/math"
-CPPCHECK_RULES="-DIGNITION_VISIBLE"
+CPPCHECK_RULES="-DIGNITION_VISIBLE"\
+" --rule-file=./tools/cppcheck_rules/header_guard.rule"\
+" --rule-file=./tools/cppcheck_rules/namespace_AZ.rule"
CPPCHECK_CMD1A="-j 4 --enable=style,performance,portability,information"
CPPCHECK_CMD1B="$CPPCHECK_RULES $CPPCHECK_FILES"
CPPCHECK_CMD1="$CPPCHECK_CMD1A $CPPCHECK_CMD1B"
diff --git a/tools/cppcheck_rules/header_guard.rule b/tools/cppcheck_rules/header_guard.rule
new file mode 100644
index 0000000..6607951
--- /dev/null
+++ b/tools/cppcheck_rules/header_guard.rule
@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<rule version="1">
+ <tokenlist>define</tokenlist>
+ <pattern>#define _[A-Z]</pattern>
+ <message>
+ <id>_AZdefine</id>
+ <severity>warning</severity>
+ <summary>Definitions should not start with underscore and then a capital letter.</summary>
+ </message>
+</rule>
diff --git a/tools/cppcheck_rules/namespace_AZ.rule b/tools/cppcheck_rules/namespace_AZ.rule
new file mode 100644
index 0000000..75250d7
--- /dev/null
+++ b/tools/cppcheck_rules/namespace_AZ.rule
@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<rule version="1">
+ <pattern>:: _[A-Z]</pattern>
+ <message>
+ <id>_AZnaming</id>
+ <severity>warning</severity>
+ <summary>Identifier that starts with underscore and then a capital letter is reserved.</summary>
+ </message>
+</rule>
--
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