p80426359
/
pxmlw6n2f
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add ign-math

This commit is contained in:
zhangshuai 2019-05-28 08:28:29 +08:00
parent bc6b17b55a
commit f27ddfc6ac
181 changed files with 68289 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Gazebo_Distributed_MPI/mpi_run/hostfile
View File

@ -1,4 +1,4 @@
airc02-PowerEdge-T640 slots=1
zhangshuai-ThinkPad-X250 slots=1
anwen slots=1

1
ign-math/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
build

1
ign-math/AUTHORS Normal file
View File

@ -0,0 +1 @@
OSRFoundation

308
ign-math/CMakeLists.txt Normal file
View File

@ -0,0 +1,308 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
set (IGN_PROJECT_NAME "math")
project (ignition-${IGN_PROJECT_NAME})
string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
set (PROJECT_MAJOR_VERSION 2)
set (PROJECT_MINOR_VERSION 9)
set (PROJECT_PATCH_VERSION 0)
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} version ${PROJECT_VERSION_FULL}")
set (project_cmake_dir ${PROJECT_SOURCE_DIR}/cmake
CACHE PATH "Location of CMake scripts")
include (${project_cmake_dir}/Utils.cmake)
########################################
# Package Creation:
include (${project_cmake_dir}/cpack.cmake)
set (CPACK_PACKAGE_VERSION "${PROJECT_VERSION_FULL}")
set (CPACK_PACKAGE_VERSION_MAJOR "${PROJECT_MAJOR_VERSION}")
set (CPACK_PACKAGE_VERSION_MINOR "${PROJECT_MINOR_VERSION}")
set (CPACK_PACKAGE_VERSION_PATCH "${PROJECT_PATCH_VERSION}")
if (CPACK_GENERATOR)
message(STATUS "Found CPack generators: ${CPACK_GENERATOR}")
configure_file("${project_cmake_dir}/cpack_options.cmake.in"
${PROJECT_CPACK_CFG_FILE} @ONLY)
set(CPACK_PROJECT_CONFIG_FILE ${PROJECT_CPACK_CFG_FILE})
include (CPack)
endif()
# If we're configuring only to package source, stop here
if (PACKAGE_SOURCE_ONLY)
message(WARNING "Configuration was done in PACKAGE_SOURCE_ONLY mode."
"You can build a tarball (make package_source), but nothing else.")
return()
endif()
#################################################
# Documentation:
add_subdirectory(doc)
# Configure documentation uploader
configure_file("${project_cmake_dir}/upload_doc.sh.in"
${CMAKE_BINARY_DIR}/upload_doc.sh @ONLY)
# If we're configuring only to build docs, stop here
if (DOC_ONLY)
message(WARNING "Configuration was done in DOC_ONLY mode."
" You can build documentation (make doc), but nothing else.")
return()
endif()
enable_testing()
# Use GNUInstallDirst to get canonical paths
include(GNUInstallDirs)
# with -fPIC
if(UNIX AND NOT WIN32)
set (CMAKE_INSTALL_PREFIX "/usr" CACHE STRING "Install Prefix")
find_program(CMAKE_UNAME uname /bin /usr/bin /usr/local/bin )
if(CMAKE_UNAME)
exec_program(uname ARGS -m OUTPUT_VARIABLE CMAKE_SYSTEM_PROCESSOR)
set(CMAKE_SYSTEM_PROCESSOR ${CMAKE_SYSTEM_PROCESSOR} CACHE INTERNAL
"processor type (i386 and x86_64)")
if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64")
ADD_DEFINITIONS(-fPIC)
endif(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64")
endif(CMAKE_UNAME)
endif()
set (CMAKE_INCLUDE_DIRECTORIES_PROJECT_BEFORE ON)
# developer's option to cache PKG_CONFIG_PATH and
# LD_LIBRARY_PATH for local installs
if(PKG_CONFIG_PATH)
set (ENV{PKG_CONFIG_PATH} ${PKG_CONFIG_PATH}:$ENV{PKG_CONFIG_PATH})
endif()
if(LD_LIBRARY_PATH)
set (ENV{LD_LIBRARY_PATH} ${LD_LIBRARY_PATH}:$ENV{LD_LIBRARY_PATH})
endif()
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_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")
set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${LIB_INSTALL_DIR}")
endif("${isSystemDir}" STREQUAL "-1")
endif()
set (BUILD_IGNITION ON CACHE INTERNAL "Build Ignition Math" FORCE)
set (build_errors "" CACHE INTERNAL "build errors" FORCE)
set (build_warnings "" CACHE INTERNAL "build warnings" FORCE)
include (${project_cmake_dir}/DissectVersion.cmake)
message (STATUS "\n\n====== Finding 3rd Party Packages ======")
include (${project_cmake_dir}/SearchForStuff.cmake)
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
"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
# uppercase old supported and deprecated modes
string(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE_UPPERCASE)
set (BUILD_TYPE_PROFILE FALSE)
set (BUILD_TYPE_RELEASE FALSE)
set (BUILD_TYPE_RELWITHDEBINFO FALSE)
set (BUILD_TYPE_DEBUG FALSE)
if ("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "PROFILE")
set (BUILD_TYPE_PROFILE TRUE)
elseif ("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "RELEASE")
set (BUILD_TYPE_RELEASE TRUE)
elseif ("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "RELWITHDEBINFO")
set (BUILD_TYPE_RELWITHDEBINFO TRUE)
elseif ("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "DEBUG")
set (BUILD_TYPE_DEBUG TRUE)
elseif ("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "COVERAGE")
include (${project_cmake_dir}/CodeCoverage.cmake)
set (BUILD_TYPE_DEBUG TRUE)
SETUP_TARGET_FOR_COVERAGE(coverage ctest coverage)
else()
build_error("CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} unknown. Valid options are: Debug Release RelWithDebInfo Profile Check")
endif()
#####################################
# Handle CFlags
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.
# Recommended to use /W4 instead of /Wall
if (MSVC)
set(WARN_LEVEL "/W4")
else()
set(WARN_LEVEL "-Wall")
endif()
filter_valid_compiler_warnings(${WARN_LEVEL} -Wextra -Wno-long-long
-Wno-unused-value -Wno-unused-value -Wno-unused-value -Wno-unused-value
-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
if (UNIX)
ign_setup_unix()
else (WIN32)
ign_setup_windows()
endif()
if(APPLE)
ign_setup_apple()
endif()
#################################################
# Print warnings and errors
if ( build_warnings )
message(STATUS "BUILD WARNINGS")
foreach (msg ${build_warnings})
message(STATUS ${msg})
endforeach ()
message(STATUS "END BUILD WARNINGS\n")
endif (build_warnings)
########### Add uninstall target ###############
configure_file(
"${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in"
"${CMAKE_CURRENT_BINARY_DIR}/cmake/cmake_uninstall.cmake"
IMMEDIATE @ONLY)
add_custom_target(uninstall
"${CMAKE_COMMAND}" -P
"${CMAKE_CURRENT_BINARY_DIR}/cmake/cmake_uninstall.cmake")
if (build_errors)
message(STATUS "BUILD ERRORS: These must be resolved before compiling.")
foreach (msg ${build_errors})
message(STATUS ${msg})
endforeach ()
message(STATUS "END BUILD ERRORS\n")
message (FATAL_ERROR "Errors encountered in build. "
"Please see the BUILD ERRORS above.")
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)
ign_install_includes(
${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)
message (STATUS "Custom CFlags:${CMAKE_CXX_FLAGS}")
else()
message (STATUS "Use default CFlags")
endif()
message (STATUS "Build Type: ${CMAKE_BUILD_TYPE}")
message (STATUS "Install path: ${CMAKE_INSTALL_PREFIX}")
if (BUILD_IGNITION)
set(TEST_TYPE "UNIT")
add_subdirectory(src)
add_subdirectory(include)
add_subdirectory(test)
endif (BUILD_IGNITION)
########################################
# Make the package config files
set (pkgconfig_files ${PROJECT_NAME_LOWER})
foreach (pkgconfig ${pkgconfig_files})
configure_file(${CMAKE_SOURCE_DIR}/cmake/pkgconfig/${pkgconfig}.in
${CMAKE_CURRENT_BINARY_DIR}/cmake/pkgconfig/${pkgconfig}${PROJECT_MAJOR_VERSION}.pc @ONLY)
install(FILES ${CMAKE_CURRENT_BINARY_DIR}/cmake/pkgconfig/${pkgconfig}${PROJECT_MAJOR_VERSION}.pc
DESTINATION ${LIB_INSTALL_DIR}/pkgconfig COMPONENT pkgconfig)
endforeach()
########################################
# Make the cmake config files
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)
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
include(WriteBasicConfigVersionFile)
write_basic_config_version_file(
${CMAKE_CURRENT_BINARY_DIR}/${cmake_conf_version_file}
VERSION "${PROJECT_VERSION_FULL}"
COMPATIBILITY SameMajorVersion)
install(FILES
${CMAKE_CURRENT_BINARY_DIR}/${cmake_conf_file}
${CMAKE_CURRENT_BINARY_DIR}/${cmake_conf_version_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,
# for example, by the Ubuntu overlay, to
# arrange for installation of Ubuntu-specific application-launching
# configuration.
if (EXISTS ${PROJECT_SOURCE_DIR}/cmake/packager-hooks/CMakeLists.txt)
message(STATUS "Loading packager build hooks from cmake/packager-hooks")
add_subdirectory(cmake/packager-hooks)
endif()
message(STATUS "Configuration successful. Type make to compile ${PROJECT_NAME_LOWER}")
endif(build_errors)

178
ign-math/COPYING Normal file
View File

@ -0,0 +1,178 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS

103
ign-math/Changelog.md Normal file
View File

@ -0,0 +1,103 @@
## Ignition Math 2.x
1. Fixed frustum falsely saying it contained AABB in some cases
* [Pull request 193](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/193)
### Ignition Math 2.8
### Ignition Math 2.8.0
1. Added Color
* [Pull request 150](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/150)
1. Added OrientedBox
* [Pull request 146](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/146)
1. Added an assignment operator to the Frustum class.
* [Pull request 144](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/144)
### Ignition Math 2.7
### Ignition Math 2.7.0
1. Add static const variables as alternative to macros in Helpers.hh
* [Pull request 137](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/137)
1. Add new methods for floating numbers: lessOrEqual and greaterOrEqual
* [Pull request 134](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/134)
### Ignition Math 2.6
### Ignition Math 2.6.0
1. Added copy constructor, equality operators and assignment operators to
SphericalCoordinates class.
* [Pull request 131](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/131)
1. Fix Euler angle conversion of quaternions near singularities
* [Pull request 129](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/129)
1. Backport triangle3, helper functions, equality helper to work with 387 fp unit
(Contribution from Rich Mattes).
* [Pull request 125](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/125)
* [Pull request 58](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/58)
* [Pull request 56](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/56)
1. Added Matrix4<T>::LookAt
* [Pull request 124](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/124)
1. Set Inertial Rotations
* [Pull request 121](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/121)
1. Added SemanticVersion class
* [Pull request 120](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/120)
### Ignition Math 2.5
### Ignition Math 2.5.0
1. Added PID class
* [Pull request 117](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/117)
1. Added SphericalCoordinate class
* [Pull request 108](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/108)
### 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 113](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/113)
### Ignition Math 2.3.0
1. Added simple volumes formulas
* [Pull request 84](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/84)
1. Add Length and SquaredLength for Vector2 with test
* [Pull request 73](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/73)
1. Add Equal function with numerical tolerance argument
* [Pull request 75](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/75)
1. First part of MassMatrix3 class, mostly accessors and modifiers
* [Pull request 77](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/77)
1. Add Transpose methods for Matrix3,4 with test
* [Pull request 74](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/74)
1. Multiplication improvements for Vector/Matrix classes
* [Pull request 69](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/69)
1. Scalar +,- operators for Vector[234]
* [Pull request 71](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/71)
1. Add Determinant method for Matrix[34]
* [Pull request 72](https://bitbucket.org/ignitionrobotics/ign-math/pull-requests/72)
1. Fixes for compiling and running tests on Windows 7/Visual Studio 2013
Contribution from Silvio Traversaro.
* [Pull request 62](https://bitbucket.org/ignitionrobotics/ign-math/pull-request/62)

15
ign-math/LICENSE Normal file
View File

@ -0,0 +1,15 @@
Software License Agreement (Apache License)
Copyright 2012 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.

1
ign-math/NEWS Normal file
View File

@ -0,0 +1 @@
http://ignition_robotics.org

45
ign-math/README.md Normal file
View File

@ -0,0 +1,45 @@
# Ignition Math
**Math classes and functions for robot applications.**
Ignition Math is a component in the Ignition framework, a set of libraries
designed to rapidly develop robot applications. The library defines math
classes and functions used in other Ignition libraries and programs.
[http://ignitionrobotics.org](http://ignitionrobotics.org)
## Continuous integration
This project uses [appveyor](https://ci.appveyor.com/project/scpeters/ign-math/history)
for testing on Windows.
It has the following build status:
![appveyor badge](https://ci.appveyor.com/api/projects/status/bitbucket/ignitionrobotics/ign-math?svg=true)
This project also uses [bitbucket pipelines](https://bitbucket.org/ignitionrobotics/ign-math/addon/pipelines/home#!/)
for testing with Linux.
Test coverage reports are available at Codecov:
[![codecov](https://codecov.io/bb/ignitionrobotics/ign-math/branch/ign-math2/graph/badge.svg)](https://codecov.io/bb/ignitionrobotics/ign-math)
## Optional Dependencies
sudo apt-get install doxygen
## Installation
Standard installation can be performed in UNIX systems using the following
steps:
- mkdir build/
- cd build/
- cmake ..
- sudo make install
## Uninstallation
To uninstall the software installed with the previous steps:
- cd build/
- sudo make uninstall

22
ign-math/appveyor.yml Normal file
View File

@ -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

21
ign-math/bitbucket-pipelines.yml Normal file
View File

@ -0,0 +1,21 @@
# This is a sample build configuration for C++.
# Check our guides at https://confluence.atlassian.com/x/VYk8Lw for more examples.
# Only use spaces to indent your .yml configuration.
# -----
# You can specify a custom docker image from Docker Hub as your build environment.
image: ubuntu:xenial
pipelines:
default:
- step:
script: # Modify the commands below to build your repository.
- apt-get update
- apt-get -y install cmake build-essential lcov curl mercurial
- mkdir build
- cd build
- cmake .. -DCMAKE_BUILD_TYPE=coverage
- make
- make test
- make coverage
- bash <(curl -s https://codecov.io/bash)

134
ign-math/cmake/CodeCoverage.cmake Normal file
View File

@ -0,0 +1,134 @@
#
# 2012-01-31, Lars Bilke
# - Enable Code Coverage
#
# 2013-09-17, Joakim Söderberg
# - Added support for Clang.
# - Some additional usage instructions.
#
# USAGE:
# 1. Copy this file into your cmake modules path.
#
# 2. Add the following line to your CMakeLists.txt:
# INCLUDE(CodeCoverage)
#
# 3. Set compiler flags to turn off optimization and enable coverage:
# SET(CMAKE_CXX_FLAGS "-g -O0 -fprofile-arcs -ftest-coverage")
# SET(CMAKE_C_FLAGS "-g -O0 -fprofile-arcs -ftest-coverage")
#
# 3. Use the function SETUP_TARGET_FOR_COVERAGE to create a custom make target
# which runs your test executable and produces a lcov code coverage report:
# Example:
# SETUP_TARGET_FOR_COVERAGE(
# my_coverage_target # Name for custom target.
# test_driver # Name of the test driver executable that runs the tests.
# # NOTE! This should always have a ZERO as exit code
# # otherwise the coverage generation will not complete.
# coverage # Name of output directory.
# )
#
# 4. Build a Debug build:
# cmake -DCMAKE_BUILD_TYPE=Debug ..
# make
# make my_coverage_target
#
#
# Check prereqs
FIND_PROGRAM( GCOV_PATH gcov )
FIND_PROGRAM( LCOV_PATH lcov )
FIND_PROGRAM( GREP_PATH grep )
FIND_PROGRAM( GENHTML_PATH genhtml )
FIND_PROGRAM( GCOVR_PATH gcovr PATHS ${CMAKE_SOURCE_DIR}/tests)
IF(NOT GCOV_PATH)
MESSAGE(FATAL_ERROR "gcov not found! Aborting...")
ENDIF() # NOT GCOV_PATH
IF(NOT CMAKE_COMPILER_IS_GNUCXX)
# Clang version 3.0.0 and greater now supports gcov as well.
MESSAGE(WARNING "Compiler is not GNU gcc! Clang Version 3.0.0 and greater supports gcov as well, but older versions don't.")
IF(NOT "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
MESSAGE(FATAL_ERROR "Compiler is not GNU gcc! Aborting...")
ENDIF()
ENDIF() # NOT CMAKE_COMPILER_IS_GNUCXX
SET(CMAKE_CXX_FLAGS_COVERAGE
"-g -O0 --coverage -fprofile-arcs -ftest-coverage"
CACHE STRING "Flags used by the C++ compiler during coverage builds."
FORCE )
SET(CMAKE_C_FLAGS_COVERAGE
"-g -O0 --coverage -fprofile-arcs -ftest-coverage"
CACHE STRING "Flags used by the C compiler during coverage builds."
FORCE )
SET(CMAKE_EXE_LINKER_FLAGS_COVERAGE
""
CACHE STRING "Flags used for linking binaries during coverage builds."
FORCE )
SET(CMAKE_SHARED_LINKER_FLAGS_COVERAGE
""
CACHE STRING "Flags used by the shared libraries linker during coverage builds."
FORCE )
MARK_AS_ADVANCED(
CMAKE_CXX_FLAGS_COVERAGE
CMAKE_C_FLAGS_COVERAGE
CMAKE_EXE_LINKER_FLAGS_COVERAGE
CMAKE_SHARED_LINKER_FLAGS_COVERAGE )
IF ( NOT (CMAKE_BUILD_TYPE STREQUAL "Debug" OR CMAKE_BUILD_TYPE STREQUAL "Coverage"))
MESSAGE( WARNING "Code coverage results with an optimized (non-Debug) build may be misleading" )
ENDIF() # NOT CMAKE_BUILD_TYPE STREQUAL "Debug"
# Param _targetname The name of new the custom make target
# Param _testrunner The name of the target which runs the tests.
# MUST return ZERO always, even on errors.
# If not, no coverage report will be created!
# Param _outputname lcov output is generated as _outputname.info
# HTML report is generated in _outputname/index.html
# Optional fourth parameter is passed as arguments to _testrunner
# Pass them in list form, e.g.: "-j;2" for -j 2
FUNCTION(SETUP_TARGET_FOR_COVERAGE _targetname _testrunner _outputname)
IF(NOT LCOV_PATH)
MESSAGE(FATAL_ERROR "lcov not found! Aborting...")
ENDIF() # NOT LCOV_PATH
IF(NOT GREP_PATH)
MESSAGE(FATAL_ERROR "grep not found! Run code coverage on linux or mac.")
ENDIF()
IF(NOT GENHTML_PATH)
MESSAGE(FATAL_ERROR "genhtml not found! Aborting...")
ENDIF() # NOT GENHTML_PATH
# Setup target
ADD_CUSTOM_TARGET(${_targetname}
COMMAND ${CMAKE_COMMAND} -E remove ${_outputname}.info.cleaned
${_outputname}.info
# Capturing lcov counters and generating report
COMMAND ${LCOV_PATH} -q --no-checksum --directory ${PROJECT_BINARY_DIR}
--capture --output-file ${_outputname}.info 2>/dev/null
COMMAND ${LCOV_PATH} -q --remove ${_outputname}.info
'test/*' '/usr/*' '*_TEST*' --output-file ${_outputname}.info.cleaned
COMMAND ${GENHTML_PATH} -q --legend -o ${_outputname}
${_outputname}.info.cleaned
COMMAND ${LCOV_PATH} --summary ${_outputname}.info.cleaned 2>&1 | grep "lines" | cut -d ' ' -f 4 | cut -d '%' -f 1 > coverage/lines.txt
COMMAND ${LCOV_PATH} --summary ${_outputname}.info.cleaned 2>&1 | grep "functions" | cut -d ' ' -f 4 | cut -d '%' -f 1 > coverage/functions.txt
COMMAND ${CMAKE_COMMAND} -E rename ${_outputname}.info.cleaned
${_outputname}.info
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
COMMENT "Resetting code coverage counters to zero.\n"
"Processing code coverage counters and generating report."
)
# Show info where to find the report
ADD_CUSTOM_COMMAND(TARGET ${_targetname} POST_BUILD
COMMAND COMMAND ${LCOV_PATH} -q --zerocounters --directory ${PROJECT_BINARY_DIR};
COMMENT "Open ./${_outputname}/index.html in your browser to view the coverage report."
)
ENDFUNCTION() # SETUP_TARGET_FOR_COVERAGE

42
ign-math/cmake/DefaultCFlags.cmake Normal file
View File

@ -0,0 +1,42 @@
# Build type link flags
set (CMAKE_LINK_FLAGS_RELEASE " " CACHE INTERNAL "Link flags for release" FORCE)
set (CMAKE_LINK_FLAGS_RELWITHDEBINFO " " CACHE INTERNAL "Link flags for release with debug support" FORCE)
set (CMAKE_LINK_FLAGS_DEBUG " " CACHE INTERNAL "Link flags for debug" FORCE)
set (CMAKE_LINK_FLAGS_PROFILE " -pg" CACHE INTERNAL "Link flags for profile" FORCE)
set (CMAKE_LINK_FLAGS_COVERAGE " --coverage" CACHE INTERNAL "Link flags for static code checking" FORCE)
set (CMAKE_C_FLAGS_RELEASE "")
if (NOT APPLE)
# -s doesn't work with default osx compiler clang, alternative:
# http://stackoverflow.com/questions/6085491/gcc-vs-clang-symbol-strippingu
set (CMAKE_C_FLAGS_RELEASE "-s")
endif()
set (CMAKE_C_FLAGS_RELEASE " ${CMAKE_C_FLAGS_RELEASE} -O3 -DNDEBUG ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for release" FORCE)
set (CMAKE_CXX_FLAGS_RELEASE ${CMAKE_C_FLAGS_RELEASE})
set (CMAKE_C_FLAGS_RELWITHDEBINFO " -g -O2 ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for release with debug support" FORCE)
set (CMAKE_CXX_FLAGS_RELWITHDEBINFO ${CMAKE_C_FLAGS_RELWITHDEBINFO})
set (CMAKE_C_FLAGS_DEBUG " -ggdb3 ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for debug" FORCE)
set (CMAKE_CXX_FLAGS_DEBUG ${CMAKE_C_FLAGS_DEBUG})
set (CMAKE_C_FLAGS_PROFILE " -fno-omit-frame-pointer -g -pg ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for profile" FORCE)
set (CMAKE_CXX_FLAGS_PROFILE ${CMAKE_C_FLAGS_PROFILE})
set (CMAKE_C_FLAGS_COVERAGE " -g -O0 -Wformat=2 --coverage -fno-inline ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for static code checking" FORCE)
set (CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_C_FLAGS_COVERAGE} -fno-elide-constructors -fno-default-inline -fno-implicit-inline-templates")
#####################################
# 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}} -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}}")
# Add visibility in UNIX
check_gcc_visibility()
if (GCC_SUPPORTS_VISIBILITY)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fvisibility=hidden")
endif()
endif()

5
ign-math/cmake/DissectVersion.cmake Normal file
View File

@ -0,0 +1,5 @@
# Find version components
STRING (REGEX REPLACE "^([0-9]+).*" "\\1" PROJECT_MAJOR_VERSION "${PROJECT_VERSION_FULL}")
STRING (REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1" PROJECT_MINOR_VERSION "${PROJECT_VERSION_FULL}")
STRING (REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+)" "\\1" PROJECT_REVISION_VERSION ${PROJECT_VERSION_FULL})
STRING (REGEX REPLACE "^[0-9]+\\.[0-9]+\\.[0-9]+(.*)" "\\1" PROJECT_CANDIDATE_VERSION ${PROJECT_VERSION_FULL})

55
ign-math/cmake/FindOS.cmake Normal file
View File

@ -0,0 +1,55 @@
# Check the OS type.
# CMake does not distinguish Linux from other Unices.
STRING (REGEX MATCH "Linux" PLAYER_OS_LINUX ${CMAKE_SYSTEM_NAME})
# Nor *BSD
STRING (REGEX MATCH "BSD" PLAYER_OS_BSD ${CMAKE_SYSTEM_NAME})
# Or Solaris. I'm seeing a trend, here
STRING (REGEX MATCH "SunOS" PLAYER_OS_SOLARIS ${CMAKE_SYSTEM_NAME})
# Windows is easy (for once)
IF (WIN32)
SET (PLAYER_OS_WIN TRUE BOOL INTERNAL)
ENDIF (WIN32)
# Check if it's an Apple OS
IF (APPLE)
# Check if it's OS X or another MacOS (that's got to be pretty unlikely)
STRING (REGEX MATCH "Darwin" PLAYER_OS_OSX ${CMAKE_SYSTEM_NAME})
IF (NOT PLAYER_OS_OSX)
SET (PLAYER_OS_MACOS TRUE BOOL INTERNAL)
ENDIF (NOT PLAYER_OS_OSX)
ENDIF (APPLE)
# QNX
IF (QNXNTO)
SET (PLAYER_OS_QNX TRUE BOOL INTERNAL)
ENDIF (QNXNTO)
IF (PLAYER_OS_LINUX)
MESSAGE (STATUS "Operating system is Linux")
ELSEIF (PLAYER_OS_BSD)
MESSAGE (STATUS "Operating system is BSD")
ELSEIF (PLAYER_OS_WIN)
MESSAGE (STATUS "Operating system is Windows")
ELSEIF (PLAYER_OS_OSX)
MESSAGE (STATUS "Operating system is Apple MacOS X")
ELSEIF (PLAYER_OS_MACOS)
MESSAGE (STATUS "Operating system is Apple MacOS (not OS X)")
ELSEIF (PLAYER_OS_QNX)
MESSAGE (STATUS "Operating system is QNX")
ELSEIF (PLAYER_OS_SOLARIS)
MESSAGE (STATUS "Operating system is Solaris")
ELSE (PLAYER_OS_LINUX)
MESSAGE (STATUS "Operating system is generic Unix")
ENDIF (PLAYER_OS_LINUX)
#################################################
# Check for non-case-sensitive filesystems
execute_process(COMMAND ${CMAKE_CURRENT_SOURCE_DIR}/tools/case_sensitive_filesystem
RESULT_VARIABLE FILESYSTEM_CASE_SENSITIVE_RETURN)
if (${FILESYSTEM_CASE_SENSITIVE_RETURN} EQUAL 0)
set(FILESYSTEM_CASE_SENSITIVE TRUE)
else()
set(FILESYSTEM_CASE_SENSITIVE FALSE)
endif()

113
ign-math/cmake/FindSSE.cmake Normal file
View File

@ -0,0 +1,113 @@
# Check if SSE instructions are available on the machine where
# the project is compiled.
IF (ARCH MATCHES "i386" OR ARCH MATCHES "x86_64")
IF(CMAKE_SYSTEM_NAME MATCHES "Linux")
EXEC_PROGRAM(cat ARGS "/proc/cpuinfo" OUTPUT_VARIABLE CPUINFO)
STRING(REGEX REPLACE "^.*(sse2).*$" "\\1" SSE_THERE ${CPUINFO})
STRING(COMPARE EQUAL "sse2" "${SSE_THERE}" SSE2_TRUE)
IF (SSE2_TRUE)
set(SSE2_FOUND true CACHE BOOL "SSE2 available on host")
ELSE (SSE2_TRUE)
set(SSE2_FOUND false CACHE BOOL "SSE2 available on host")
ENDIF (SSE2_TRUE)
# /proc/cpuinfo apparently omits sse3 :(
STRING(REGEX REPLACE "^.*[^s](sse3).*$" "\\1" SSE_THERE ${CPUINFO})
STRING(COMPARE EQUAL "sse3" "${SSE_THERE}" SSE3_TRUE)
IF (NOT SSE3_TRUE)
STRING(REGEX REPLACE "^.*(T2300).*$" "\\1" SSE_THERE ${CPUINFO})
STRING(COMPARE EQUAL "T2300" "${SSE_THERE}" SSE3_TRUE)
ENDIF (NOT SSE3_TRUE)
STRING(REGEX REPLACE "^.*(ssse3).*$" "\\1" SSE_THERE ${CPUINFO})
STRING(COMPARE EQUAL "ssse3" "${SSE_THERE}" SSSE3_TRUE)
IF (SSE3_TRUE OR SSSE3_TRUE)
set(SSE3_FOUND true CACHE BOOL "SSE3 available on host")
ELSE (SSE3_TRUE OR SSSE3_TRUE)
set(SSE3_FOUND false CACHE BOOL "SSE3 available on host")
ENDIF (SSE3_TRUE OR SSSE3_TRUE)
IF (SSSE3_TRUE)
set(SSSE3_FOUND true CACHE BOOL "SSSE3 available on host")
ELSE (SSSE3_TRUE)
set(SSSE3_FOUND false CACHE BOOL "SSSE3 available on host")
ENDIF (SSSE3_TRUE)
STRING(REGEX REPLACE "^.*(sse4_1).*$" "\\1" SSE_THERE ${CPUINFO})
STRING(COMPARE EQUAL "sse4_1" "${SSE_THERE}" SSE41_TRUE)
IF (SSE41_TRUE)
set(SSE4_1_FOUND true CACHE BOOL "SSE4.1 available on host")
ELSE (SSE41_TRUE)
set(SSE4_1_FOUND false CACHE BOOL "SSE4.1 available on host")
ENDIF (SSE41_TRUE)
STRING(REGEX REPLACE "^.*(sse4_2).*$" "\\1" SSE_THERE ${CPUINFO})
STRING(COMPARE EQUAL "sse4_2" "${SSE_THERE}" SSE42_TRUE)
IF (SSE42_TRUE)
set(SSE4_2_FOUND true CACHE BOOL "SSE4.2 available on host")
ELSE (SSE42_TRUE)
set(SSE4_2_FOUND false CACHE BOOL "SSE4.2 available on host")
ENDIF (SSE42_TRUE)
ELSEIF(CMAKE_SYSTEM_NAME MATCHES "Darwin")
EXEC_PROGRAM("/usr/sbin/sysctl -n machdep.cpu.features" OUTPUT_VARIABLE
CPUINFO)
STRING(REGEX REPLACE "^.*[^S](SSE2).*$" "\\1" SSE_THERE ${CPUINFO})
STRING(COMPARE EQUAL "SSE2" "${SSE_THERE}" SSE2_TRUE)
IF (SSE2_TRUE)
set(SSE2_FOUND true CACHE BOOL "SSE2 available on host")
ELSE (SSE2_TRUE)
set(SSE2_FOUND false CACHE BOOL "SSE2 available on host")
ENDIF (SSE2_TRUE)
STRING(REGEX REPLACE "^.*[^S](SSE3).*$" "\\1" SSE_THERE ${CPUINFO})
STRING(COMPARE EQUAL "SSE3" "${SSE_THERE}" SSE3_TRUE)
IF (SSE3_TRUE)
set(SSE3_FOUND true CACHE BOOL "SSE3 available on host")
ELSE (SSE3_TRUE)
set(SSE3_FOUND false CACHE BOOL "SSE3 available on host")
ENDIF (SSE3_TRUE)
STRING(REGEX REPLACE "^.*(SSSE3).*$" "\\1" SSE_THERE ${CPUINFO})
STRING(COMPARE EQUAL "SSSE3" "${SSE_THERE}" SSSE3_TRUE)
IF (SSSE3_TRUE)
set(SSSE3_FOUND true CACHE BOOL "SSSE3 available on host")
ELSE (SSSE3_TRUE)
set(SSSE3_FOUND false CACHE BOOL "SSSE3 available on host")
ENDIF (SSSE3_TRUE)
STRING(REGEX REPLACE "^.*(SSE4.1).*$" "\\1" SSE_THERE ${CPUINFO})
STRING(COMPARE EQUAL "SSE4.1" "${SSE_THERE}" SSE41_TRUE)
IF (SSE41_TRUE)
set(SSE4_1_FOUND true CACHE BOOL "SSE4.1 available on host")
ELSE (SSE41_TRUE)
set(SSE4_1_FOUND false CACHE BOOL "SSE4.1 available on host")
ENDIF (SSE41_TRUE)
ELSEIF(CMAKE_SYSTEM_NAME MATCHES "Windows")
# TODO
set(SSE2_FOUND true CACHE BOOL "SSE2 available on host")
set(SSE3_FOUND false CACHE BOOL "SSE3 available on host")
set(SSSE3_FOUND false CACHE BOOL "SSSE3 available on host")
set(SSE4_1_FOUND false CACHE BOOL "SSE4.1 available on host")
ELSE(CMAKE_SYSTEM_NAME MATCHES "Linux")
set(SSE2_FOUND true CACHE BOOL "SSE2 available on host")
set(SSE3_FOUND false CACHE BOOL "SSE3 available on host")
set(SSSE3_FOUND false CACHE BOOL "SSSE3 available on host")
set(SSE4_1_FOUND false CACHE BOOL "SSE4.1 available on host")
ENDIF(CMAKE_SYSTEM_NAME MATCHES "Linux")
ENDIF(ARCH MATCHES "i386" OR ARCH MATCHES "x86_64")
if(NOT SSE2_FOUND)
MESSAGE(STATUS "Could not find hardware support for SSE2 on this machine.")
endif(NOT SSE2_FOUND)
if(NOT SSE3_FOUND)
MESSAGE(STATUS "Could not find hardware support for SSE3 on this machine.")
endif(NOT SSE3_FOUND)
if(NOT SSSE3_FOUND)
MESSAGE(STATUS "Could not find hardware support for SSSE3 on this machine.")
endif(NOT SSSE3_FOUND)
if(NOT SSE4_1_FOUND)
MESSAGE(STATUS "Could not find hardware support for SSE4.1 on this machine.")
endif(NOT SSE4_1_FOUND)

27
ign-math/cmake/HostCFlags.cmake Normal file
View File

@ -0,0 +1,27 @@
include (${project_cmake_dir}/FindSSE.cmake)
if (SSE2_FOUND)
set (CMAKE_C_FLAGS_ALL "-msse -msse2 ${CMAKE_C_FLAGS_ALL}")
if (NOT APPLE)
set (CMAKE_C_FLAGS_ALL "-mfpmath=sse ${CMAKE_C_FLAGS_ALL}")
endif()
endif()
if (SSE3_FOUND)
set (CMAKE_C_FLAGS_ALL "-msse3 ${CMAKE_C_FLAGS_ALL}")
endif()
if (SSSE3_FOUND)
set (CMAKE_C_FLAGS_ALL "-mssse3 ${CMAKE_C_FLAGS_ALL}")
endif()
if (SSE4_1_FOUND OR SSE4_2_FOUND)
if (SSE4_1_FOUND)
set (CMAKE_C_FLAGS_ALL "-msse4.1 ${CMAKE_C_FLAGS_ALL}")
endif()
if (SSE4_2_FOUND)
set (CMAKE_C_FLAGS_ALL "-msse4.2 ${CMAKE_C_FLAGS_ALL}")
endif()
else()
message(STATUS "\nSSE4 disabled.\n")
endif()

59
ign-math/cmake/Ronn2Man.cmake Normal file
View File

@ -0,0 +1,59 @@
#
# Based on work of Emmanuel Roullit <emmanuel@netsniff-ng.org>
# Copyright 2009, 2012 Emmanuel Roullit.
# Subject to the GPL, version 2.
#
MACRO(ADD_MANPAGE_TARGET)
# It is not possible add a dependency to target 'install'
# Run hard-coded 'make man' when 'make install' is invoked
INSTALL(CODE "EXECUTE_PROCESS(COMMAND make man)")
ADD_CUSTOM_TARGET(man)
ENDMACRO(ADD_MANPAGE_TARGET)
FIND_PROGRAM(RONN ronn)
FIND_PROGRAM(GZIP gzip)
IF (NOT RONN OR NOT GZIP)
IF (NOT RONN)
BUILD_WARNING ("ronn not found, manpages won't be generated")
ENDIF(NOT RONN)
IF (NOT GZIP)
BUILD_WARNING ("gzip not found, manpages won't be generated")
ENDIF(NOT GZIP)
# empty macro
MACRO(manpage MANFILE)
ENDMACRO(manpage)
SET (MANPAGES_SUPPORT FALSE)
ELSE (NOT RONN OR NOT GZIP)
MESSAGE (STATUS "Looking for ronn to generate manpages - found")
SET (MANPAGES_SUPPORT TRUE)
MACRO(manpage RONNFILE SECTION)
SET(RONNFILE_FULL_PATH ${CMAKE_CURRENT_SOURCE_DIR}/${RONNFILE})
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${RONNFILE}.${SECTION}
DEPENDS ${RONNFILE}
COMMAND ${RONN}
ARGS -r --pipe ${RONNFILE_FULL_PATH}.${SECTION}.ronn
> ${CMAKE_CURRENT_BINARY_DIR}/${RONNFILE}.${SECTION}
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${RONNFILE}.${SECTION}.gz
DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/${RONNFILE}.${SECTION}
COMMAND ${GZIP} -c ${CMAKE_CURRENT_BINARY_DIR}/${RONNFILE}.${SECTION}
> ${CMAKE_CURRENT_BINARY_DIR}/${RONNFILE}.${SECTION}.gz
)
SET(MANPAGE_TARGET "man-${RONNFILE}")
ADD_CUSTOM_TARGET(${MANPAGE_TARGET} DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/${RONNFILE}.${SECTION}.gz)
ADD_DEPENDENCIES(man ${MANPAGE_TARGET})
INSTALL(
FILES ${CMAKE_CURRENT_BINARY_DIR}/${RONNFILE}.${SECTION}.gz
DESTINATION share/man/man${SECTION}
)
ENDMACRO(manpage RONNFILE SECTION)
ENDIF(NOT RONN OR NOT GZIP)

23
ign-math/cmake/SearchForStuff.cmake Normal file
View File

@ -0,0 +1,23 @@
include (${project_cmake_dir}/Utils.cmake)
include (CheckCXXSourceCompiles)
include (${project_cmake_dir}/FindOS.cmake)
include (FindPkgConfig)
# Detect the architecture
include (${project_cmake_dir}/TargetArch.cmake)
target_architecture(ARCH)
message(STATUS "Building for arch: ${ARCH}")
########################################
# Include man pages stuff
include (${project_cmake_dir}/Ronn2Man.cmake)
add_manpage_target()
#################################################
# Macro to check for visibility capability in compiler
# Original idea from: https://gitorious.org/ferric-cmake-stuff/
macro (check_gcc_visibility)
include (CheckCXXCompilerFlag)
check_cxx_compiler_flag(-fvisibility=hidden GCC_SUPPORTS_VISIBILITY)
endmacro()

158
ign-math/cmake/TargetArch.cmake Normal file
View File

@ -0,0 +1,158 @@
# Copyright (c) 2012 Petroules Corporation. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# Redistributions of source code must retain the above copyright notice, this
# list of conditions and the following disclaimer. Redistributions in binary
# form must reproduce the above copyright notice, this list of conditions and
# the following disclaimer in the documentation and/or other materials
# provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
# ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
# Based on the Qt 5 processor detection code, so should be very accurate
# https://qt.gitorious.org/qt/qtbase/blobs/master/src/corelib/global/qprocessordetection.h
# Currently handles arm (v5, v6, v7), x86 (32/64), ia64, and ppc (32/64)
# Regarding POWER/PowerPC, just as is noted in the Qt source,
# "There are many more known variants/revisions that we do not handle/detect."
set(archdetect_c_code "
#if defined(__arm__) || defined(__TARGET_ARCH_ARM)
#if defined(__ARM_ARCH_7__) \\
|| defined(__ARM_ARCH_7A__) \\
|| defined(__ARM_ARCH_7R__) \\
|| defined(__ARM_ARCH_7M__) \\
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 7)
#error cmake_ARCH armv7
#elif defined(__ARM_ARCH_6__) \\
|| defined(__ARM_ARCH_6J__) \\
|| defined(__ARM_ARCH_6T2__) \\
|| defined(__ARM_ARCH_6Z__) \\
|| defined(__ARM_ARCH_6K__) \\
|| defined(__ARM_ARCH_6ZK__) \\
|| defined(__ARM_ARCH_6M__) \\
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 6)
#error cmake_ARCH armv6
#elif defined(__ARM_ARCH_5TEJ__) \\
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 5)
#error cmake_ARCH armv5
#else
#error cmake_ARCH arm
#endif
#elif defined(__i386) || defined(__i386__) || defined(_M_IX86)
#error cmake_ARCH i386
#elif defined(__x86_64) || defined(__x86_64__) || defined(__amd64) || defined(_M_X64)
#error cmake_ARCH x86_64
#elif defined(__ia64) || defined(__ia64__) || defined(_M_IA64)
#error cmake_ARCH ia64
#elif defined(__ppc__) || defined(__ppc) || defined(__powerpc__) \\
|| defined(_ARCH_COM) || defined(_ARCH_PWR) || defined(_ARCH_PPC) \\
|| defined(_M_MPPC) || defined(_M_PPC)
#if defined(__ppc64__) || defined(__powerpc64__) || defined(__64BIT__)
#error cmake_ARCH ppc64
#else
#error cmake_ARCH ppc
#endif
#endif
#error cmake_ARCH unknown
")
# Set ppc_support to TRUE before including this file or ppc and ppc64
# will be treated as invalid architectures since they are no longer supported by Apple
function(target_architecture output_var)
if(APPLE AND CMAKE_OSX_ARCHITECTURES)
# On OS X we use CMAKE_OSX_ARCHITECTURES *if* it was set
# First let's normalize the order of the values
# Note that it's not possible to compile PowerPC applications if you are using
# the OS X SDK version 10.6 or later - you'll need 10.4/10.5 for that, so we
# disable it by default
# See this page for more information:
# http://stackoverflow.com/questions/5333490/how-can-we-restore-ppc-ppc64-as-well-as-full-10-4-10-5-sdk-support-to-xcode-4
# Architecture defaults to i386 or ppc on OS X 10.5 and earlier, depending on the CPU type detected at runtime.
# On OS X 10.6+ the default is x86_64 if the CPU supports it, i386 otherwise.
foreach(osx_arch ${CMAKE_OSX_ARCHITECTURES})
if("${osx_arch}" STREQUAL "ppc" AND ppc_support)
set(osx_arch_ppc TRUE)
elseif("${osx_arch}" STREQUAL "i386")
set(osx_arch_i386 TRUE)
elseif("${osx_arch}" STREQUAL "x86_64")
set(osx_arch_x86_64 TRUE)
elseif("${osx_arch}" STREQUAL "ppc64" AND ppc_support)
set(osx_arch_ppc64 TRUE)
else()
message(FATAL_ERROR "Invalid OS X arch name: ${osx_arch}")
endif()
endforeach()
# Now add all the architectures in our normalized order
if(osx_arch_ppc)
list(APPEND ARCH ppc)
endif()
if(osx_arch_i386)
list(APPEND ARCH i386)
endif()
if(osx_arch_x86_64)
list(APPEND ARCH x86_64)
endif()
if(osx_arch_ppc64)
list(APPEND ARCH ppc64)
endif()
else()
file(WRITE "${CMAKE_BINARY_DIR}/arch.c" "${archdetect_c_code}")
enable_language(C)
# Detect the architecture in a rather creative way...
# This compiles a small C program which is a series of ifdefs that selects a
# particular #error preprocessor directive whose message string contains the
# target architecture. The program will always fail to compile (both because
# file is not a valid C program, and obviously because of the presence of the
# #error preprocessor directives... but by exploiting the preprocessor in this
# way, we can detect the correct target architecture even when cross-compiling,
# since the program itself never needs to be run (only the compiler/preprocessor)
try_run(
run_result_unused
compile_result_unused
"${CMAKE_BINARY_DIR}"
"${CMAKE_BINARY_DIR}/arch.c"
COMPILE_OUTPUT_VARIABLE ARCH
CMAKE_FLAGS CMAKE_OSX_ARCHITECTURES=${CMAKE_OSX_ARCHITECTURES}
)
# Parse the architecture name from the compiler output
string(REGEX MATCH "cmake_ARCH ([a-zA-Z0-9_]+)" ARCH "${ARCH}")
# Get rid of the value marker leaving just the architecture name
string(REPLACE "cmake_ARCH " "" ARCH "${ARCH}")
# If we are compiling with an unknown architecture this variable should
# already be set to "unknown" but in the case that it's empty (i.e. due
# to a typo in the code), then set it to unknown
if (NOT ARCH)
set(ARCH unknown)
endif()
endif()
set(${output_var} "${ARCH}" PARENT_SCOPE)
endfunction()

48
ign-math/cmake/TestUtils.cmake Normal file
View File

@ -0,0 +1,48 @@
#################################################
macro (ign_build_tests)
# Find the Python interpreter for running the
# check_test_ran.py script
find_package(PythonInterp QUIET)
# Build all the tests
foreach(GTEST_SOURCE_file ${ARGN})
string(REGEX REPLACE ".cc" "" BINARY_NAME ${GTEST_SOURCE_file})
set(BINARY_NAME ${TEST_TYPE}_${BINARY_NAME})
if(USE_LOW_MEMORY_TESTS)
add_definitions(-DUSE_LOW_MEMORY_TESTS=1)
endif(USE_LOW_MEMORY_TESTS)
add_executable(${BINARY_NAME} ${GTEST_SOURCE_file})
add_dependencies(${BINARY_NAME}
ignition-math${PROJECT_MAJOR_VERSION}
gtest gtest_main
)
if (UNIX)
target_link_libraries(${BINARY_NAME}
libgtest_main.a
libgtest.a
pthread
ignition-math${PROJECT_MAJOR_VERSION})
elseif(WIN32)
target_link_libraries(${BINARY_NAME}
gtest.lib
gtest_main.lib
ignition-math${PROJECT_MAJOR_VERSION}.lib)
else()
message(FATAL_ERROR "Unsupported platform")
endif()
add_test(${BINARY_NAME} ${CMAKE_CURRENT_BINARY_DIR}/${BINARY_NAME}
--gtest_output=xml:${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
set_tests_properties(${BINARY_NAME} PROPERTIES TIMEOUT 240)
if(PYTHONINTERP_FOUND)
# Check that the test produced a result and create a failure if it didn't.
# Guards against crashed and timed out tests.
add_test(check_${BINARY_NAME} ${PYTHON_EXECUTABLE} ${PROJECT_SOURCE_DIR}/tools/check_test_ran.py
${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
endif()
endforeach()
endmacro()

179
ign-math/cmake/Utils.cmake Normal file
View File

@ -0,0 +1,179 @@
################################################################################
#APPEND_TO_CACHED_STRING(_string _cacheDesc [items...])
# Appends items to a cached list.
MACRO (APPEND_TO_CACHED_STRING _string _cacheDesc)
FOREACH (newItem ${ARGN})
SET (${_string} "${${_string}} ${newItem}" CACHE INTERNAL ${_cacheDesc} FORCE)
ENDFOREACH (newItem ${ARGN})
#STRING(STRIP ${${_string}} ${_string})
ENDMACRO (APPEND_TO_CACHED_STRING)
################################################################################
# APPEND_TO_CACHED_LIST (_list _cacheDesc [items...]
# Appends items to a cached list.
MACRO (APPEND_TO_CACHED_LIST _list _cacheDesc)
SET (tempList ${${_list}})
FOREACH (newItem ${ARGN})
LIST (APPEND tempList ${newItem})
ENDFOREACH (newItem ${newItem})
SET (${_list} ${tempList} CACHE INTERNAL ${_cacheDesc} FORCE)
ENDMACRO(APPEND_TO_CACHED_LIST)
#################################################
# Macro to turn a list into a string (why doesn't CMake have this built-in?)
MACRO (LIST_TO_STRING _string _list)
SET (${_string})
FOREACH (_item ${_list})
SET (${_string} "${${_string}} ${_item}")
ENDFOREACH (_item)
#STRING(STRIP ${${_string}} ${_string})
ENDMACRO (LIST_TO_STRING)
#################################################
# BUILD ERROR macro
macro (BUILD_ERROR)
foreach (str ${ARGN})
SET (msg "\t${str}")
MESSAGE (STATUS ${msg})
APPEND_TO_CACHED_LIST(build_errors "build errors" ${msg})
endforeach ()
endmacro (BUILD_ERROR)
#################################################
# BUILD WARNING macro
macro (BUILD_WARNING)
foreach (str ${ARGN})
SET (msg "\t${str}" )
MESSAGE (STATUS ${msg} )
APPEND_TO_CACHED_LIST(build_warnings "build warning" ${msg})
endforeach (str ${ARGN})
endmacro (BUILD_WARNING)
#################################################
macro (ign_add_library _name)
set(LIBS_DESTINATION ${PROJECT_BINARY_DIR}/src)
set_source_files_properties(${ARGN} PROPERTIES COMPILE_DEFINITIONS "BUILDING_DLL")
add_library(${_name} SHARED ${ARGN})
target_link_libraries (${_name} ${general_libraries})
set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${LIBS_DESTINATION})
if (MSVC)
set_target_properties( ${_name} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${LIBS_DESTINATION})
set_target_properties( ${_name} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY_DEBUG ${LIBS_DESTINATION})
set_target_properties( ${_name} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY_RELEASE ${LIBS_DESTINATION})
endif ( MSVC )
endmacro ()
#################################################
macro (ign_add_static_library _name)
add_library(${_name} STATIC ${ARGN})
target_link_libraries (${_name} ${general_libraries})
endmacro ()
#################################################
macro (ign_add_executable _name)
add_executable(${_name} ${ARGN})
target_link_libraries (${_name} ${general_libraries})
endmacro ()
#################################################
macro (ign_install_includes _subdir)
install(FILES ${ARGN}
DESTINATION ${INCLUDE_INSTALL_DIR}/${_subdir} COMPONENT headers)
endmacro()
#################################################
macro (ign_install_library _name)
set_target_properties(${_name} PROPERTIES SOVERSION ${PROJECT_MAJOR_VERSION} VERSION ${PROJECT_VERSION_FULL})
install (TARGETS ${_name} DESTINATION ${LIB_INSTALL_DIR} COMPONENT shlib)
endmacro ()
#################################################
macro (ign_install_executable _name)
set_target_properties(${_name} PROPERTIES VERSION ${PROJECT_VERSION_FULL})
install (TARGETS ${_name} DESTINATION ${BIN_INSTALL_DIR})
manpage(${_name} 1)
endmacro ()
#################################################
macro (ign_setup_unix)
# USE_HOST_CFLAGS (default TRUE)
# 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)
endif()
# USE_UPSTREAM_CFLAGS (default TRUE)
# Will use predefined ignition developers cflags
if(NOT DEFINED USE_UPSTREAM_CFLAGS OR USE_UPSTREAM_CFLAGS)
message(STATUS "Enable upstream CFlags")
include(${project_cmake_dir}/DefaultCFlags.cmake)
endif()
endmacro()
#################################################
macro (ign_setup_windows)
if(MSVC)
add_definitions("/EHsc")
endif()
endmacro()
#################################################
macro (ign_setup_apple)
# NOTE MacOSX provides different system versions than CMake is parsing.
# The following table lists the most recent OSX versions
# 9.x.x = Mac OSX Leopard (10.5)
# 10.x.x = Mac OSX Snow Leopard (10.6)
# 11.x.x = Mac OSX Lion (10.7)
# 12.x.x = Mac OSX Mountain Lion (10.8)
if (${CMAKE_SYSTEM_VERSION} LESS 10)
add_definitions(-DMAC_OS_X_VERSION=1050)
elseif (${CMAKE_SYSTEM_VERSION} GREATER 10 AND ${CMAKE_SYSTEM_VERSION} LESS 11)
add_definitions(-DMAC_OS_X_VERSION=1060)
elseif (${CMAKE_SYSTEM_VERSION} GREATER 11 AND ${CMAKE_SYSTEM_VERSION} LESS 12)
add_definitions(-DMAC_OS_X_VERSION=1070)
elseif (${CMAKE_SYSTEM_VERSION} GREATER 12 OR ${CMAKE_SYSTEM_VERSION} EQUAL 12)
add_definitions(-DMAC_OS_X_VERSION=1080)
# Use libc++ on Mountain Lion (10.8)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libc++")
else ()
add_definitions(-DMAC_OS_X_VERSION=0)
endif ()
set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,-undefined -Wl,dynamic_lookup")
endmacro()
# This should be migrated to more fine control solution based on set_property APPEND
# directories. It's present on cmake 2.8.8 while precise version is 2.8.7
link_directories(${PROJECT_BINARY_DIR}/test)
include_directories("${PROJECT_SOURCE_DIR}/test/gtest/include")
#################################################
# Enable tests compilation by default
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
# ENABLE_TESTS_COMPILATION is set to true
macro (ign_build_tests)
endmacro()
if (ENABLE_TESTS_COMPILATION)
include (${project_cmake_dir}/TestUtils.cmake)
endif()
#################################################
# Macro to setup supported compiler warnings
# Based on work of Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST.
include(CheckCXXCompilerFlag)
macro(filter_valid_compiler_warnings)
foreach(flag ${ARGN})
CHECK_CXX_COMPILER_FLAG(${flag} R${flag})
if(${R${flag}})
set(WARNING_CXX_FLAGS "${WARNING_CXX_FLAGS} ${flag}")
endif()
endforeach()
endmacro()

21
ign-math/cmake/cmake_uninstall.cmake.in Normal file
View File

@ -0,0 +1,21 @@
if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
message(FATAL_ERROR "Cannot find install manifest: "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt"")
endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files)
string(REGEX REPLACE "\n" ";" files "${files}")
foreach(file ${files})
message(STATUS "Uninstalling "$ENV{DESTDIR}${file}"")
if(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}")
exec_program(
"@CMAKE_COMMAND@" ARGS "-E remove "$ENV{DESTDIR}${file}""
OUTPUT_VARIABLE rm_out
RETURN_VALUE rm_retval
)
if(NOT "${rm_retval}" STREQUAL 0)
message(FATAL_ERROR "Problem when removing "$ENV{DESTDIR}${file}"")
endif(NOT "${rm_retval}" STREQUAL 0)
else(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}")
message(STATUS "File "$ENV{DESTDIR}${file}" does not exist.")
endif(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}")
endforeach(file)

15
ign-math/cmake/config.hh.in Normal file
View File

@ -0,0 +1,15 @@
/* Config.hh. Generated by CMake for @PROJECT_NAME@. */
/* Version number */
#define IGNITION_MATH_MAJOR_VERSION ${PROJECT_MAJOR_VERSION}
#define IGNITION_MATH_MINOR_VERSION ${PROJECT_MINOR_VERSION}
#define IGNITION_MATH_PATCH_VERSION ${PROJECT_PATCH_VERSION}
#define IGNITION_MATH_VERSION "${PROJECT_VERSION}"
#define IGNITION_MATH_VERSION_FULL "${PROJECT_VERSION_FULL}"
#define IGNITION_MATH_VERSION_HEADER "Ignition math, version ${PROJECT_VERSION_FULL}\nCopyright (C) 2014 Open Source Robotics Foundation.\nReleased under the Apache 2.0 License.\n\n"
#cmakedefine BUILD_TYPE_PROFILE 1
#cmakedefine BUILD_TYPE_DEBUG 1
#cmakedefine BUILD_TYPE_RELEASE 1

25
ign-math/cmake/cpack.cmake Normal file
View File

@ -0,0 +1,25 @@
################################################################################
#Find available package generators
# DEB
if ("${CMAKE_SYSTEM}" MATCHES "Linux")
find_program(DPKG_PROGRAM dpkg)
if (EXISTS ${DPKG_PROGRAM})
list (APPEND CPACK_GENERATOR "DEB")
endif(EXISTS ${DPKG_PROGRAM})
find_program(RPMBUILD_PROGRAM rpmbuild)
endif()
list (APPEND CPACK_SOURCE_GENERATOR "TBZ2")
list (APPEND CPACK_SOURCE_GENERATOR "ZIP")
list (APPEND CPACK_SOURCE_IGNORE_FILES "TODO;/.hg/;.swp$;/build/;.hgtags")
include (InstallRequiredSystemLibraries)
#execute_process(COMMAND dpkg --print-architecture _NPROCE)
set (DEBIAN_PACKAGE_DEPENDS "")
set (RPM_PACKAGE_DEPENDS "")
set (PROJECT_CPACK_CFG_FILE "${PROJECT_BINARY_DIR}/cpack_options.cmake")

28
ign-math/cmake/cpack_options.cmake.in Normal file
View File

@ -0,0 +1,28 @@
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_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")
set(CPACK_PACKAGE_MAINTAINER "Nate Koenig <nate@osrfoundation.org>")
set(CPACK_PACKAGE_CONTACT "Nate Koenig <natekoenig@osrfoundation.org>")
set(CPACK_DEBIAN_PACKAGE_ARCHITECTURE "@DPKG_ARCH@")
set(CPACK_DEBIAN_PACKAGE_DEPENDS "@DEBIAN_PACKAGE_DEPENDS@")
set(CPACK_DEBIAN_PACKAGE_SECTION "devel")
set(CPACK_DEBIAN_PACKAGE_PRIORITY "optional")
set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS ON)
set(CPACK_DEBIAN_PACKAGE_DESCRIPTION
"A set of @IGN_PROJECT_NAME@ classes for robot applications.")
set(CPACK_RPM_PACKAGE_ARCHITECTURE "@DPKG_ARCH@")
set(CPACK_RPM_PACKAGE_REQUIRES "@DEBIAN_PACKAGE_DEPENDS@")
set(CPACK_RPM_PACKAGE_DESCRIPTION
"A set of @IGN_PROJECT_NAME@ classes for robot applications.")
set (CPACK_PACKAGE_FILE_NAME
"@PROJECT_NAME_LOWER@@PROJECT_MAJOR_VERSION@-@PROJECT_VERSION_FULL@")
set (CPACK_SOURCE_PACKAGE_FILE_NAME
"@PROJECT_NAME_LOWER@@PROJECT_MAJOR_VERSION@-@PROJECT_VERSION_FULL@")

41
ign-math/cmake/ignition-math-config.cmake.in Normal file
View File

@ -0,0 +1,41 @@
if (@PKG_NAME@_CONFIG_INCLUDED)
return()
endif()
set(@PKG_NAME@_CONFIG_INCLUDED TRUE)
list(APPEND @PKG_NAME@_INCLUDE_DIRS "@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_INCLUDEDIR@/ignition/@IGN_PROJECT_NAME@@PROJECT_MAJOR_VERSION@")
list(APPEND @PKG_NAME@_LIBRARY_DIRS "@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@")
list(APPEND @PKG_NAME@_CXX_FLAGS -std=c++11)
if ("${CMAKE_CXX_COMPILER_ID} " MATCHES "Clang ")
set(@PKG_NAME@_CXX_FLAGS "${@PKG_NAME@_CXX_FLAGS} -stdlib=libc++")
endif ()
# On windows we produce .dll libraries with no prefix
if (WIN32)
set(CMAKE_FIND_LIBRARY_PREFIXES "")
set(CMAKE_FIND_LIBRARY_SUFFIXES ".lib" ".dll")
endif()
foreach(lib @PKG_LIBRARIES@)
set(onelib "${lib}-NOTFOUND")
find_library(onelib ${lib}
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@_LIBRARIES ${onelib})
endforeach()
foreach(dep @PKG_DEPENDS@)
if(NOT ${dep}_FOUND)
find_package(${dep} REQUIRED)
endif()
list(APPEND @PKG_NAME@_INCLUDE_DIRS "${${dep}_INCLUDE_DIRS}")
list(APPEND @PKG_NAME@_LIBRARIES "${${dep_lib}_LIBRARIES}")
endforeach()
list(APPEND @PKG_NAME@_LDFLAGS "-L@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@")

10
ign-math/cmake/pkgconfig/ignition-math.in Normal file
View File

@ -0,0 +1,10 @@
prefix=@CMAKE_INSTALL_PREFIX@
libdir=${prefix}/@CMAKE_INSTALL_LIBDIR@
includedir=${prefix}/@CMAKE_INSTALL_INCLUDEDIR@/ignition/@IGN_PROJECT_NAME@@PROJECT_MAJOR_VERSION@
Name: Ignition Math
Description: A set of math classes for robot applications
Version: @PROJECT_VERSION_FULL@
Requires:
Libs: "-L${libdir}" -lignition-math@PROJECT_MAJOR_VERSION@
CFlags: "-I${includedir}" -std=c++11

29
ign-math/cmake/upload_doc.sh.in Normal file
View File

@ -0,0 +1,29 @@
#!/bin/sh
# Check if the node was configured to use s3cmd
# This is done by running s3cmd --configure
if [ ! -f "${HOME}/.s3cfg" ]; then
echo "No $HOME/.s3cfg file found. Please config the software first in your system"
exit 1
fi
# Make documentation if not build
if [ ! -f "@CMAKE_BINARY_DIR@/doxygen/html/index.html" ]; then
make doc
if [ ! -f "@CMAKE_BINARY_DIR@/doxygen/html/index.html" ]; then
echo "Documentation not present. Install doxygen, and run `make doc` in the build directory"
exit 1
fi
fi
# Dry run
s3cmd sync @CMAKE_BINARY_DIR@/doxygen/html/* s3://osrf-distributions/ign-@IGN_PROJECT_NAME@/api/@PROJECT_VERSION_FULL@/ --dry-run -v
echo -n "Upload (Y/n)? "
read ans
if [ "$ans" = "n" ] || [ "$ans" = "N" ]; then
exit 1
else
s3cmd sync @CMAKE_BINARY_DIR@/doxygen/html/* s3://osrf-distributions/ign-@IGN_PROJECT_NAME@/api/@PROJECT_VERSION_FULL@/ -v
fi

2
ign-math/codecov.yml Normal file
View File

@ -0,0 +1,2 @@
codecov:
branch: default

23
ign-math/configure.bat Normal file
View File

@ -0,0 +1,23 @@
:: NOTE: This script is only meant to be used as part of the ignition developers' CI system
:: Users and developers should build and install this library using cmake and Visual Studio
:: Set configuration variables
@set build_type=Release
@if not "%1"=="" set build_type=%1
@echo Configuring for build type %build_type%
:: Use legacy install location if unset
@if "%WORKSPACE_INSTALL_DIR%"=="" set WORKSPACE_INSTALL_DIR="install\%build_type%"
:: Go to the directory that this configure.bat file exists in
cd /d %~dp0
:: Create a build directory and configure
md build
cd build
cmake .. -G "NMake Makefiles" -DCMAKE_INSTALL_PREFIX="%WORKSPACE_INSTALL_DIR%" -DCMAKE_BUILD_TYPE="%build_type%" -DENABLE_TESTS_COMPILATION:BOOL=True
:: Note: Testing is enabled by default in legacy branches.
:: Take care when merging this forward.
:: If the caller wants to build and/or install, they should do so after calling this script

14
ign-math/doc/CMakeLists.txt Normal file
View File

@ -0,0 +1,14 @@
find_package(Doxygen)
if (DOXYGEN_FOUND)
configure_file(${CMAKE_SOURCE_DIR}/doc/ignition.in
${CMAKE_BINARY_DIR}/ignition.dox @ONLY)
add_custom_target(doc
# Generate the API documentation
${DOXYGEN_EXECUTABLE} ${CMAKE_BINARY_DIR}/ignition.dox
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
COMMAND cp ${CMAKE_SOURCE_DIR}/doc/ignition_logo.svg
${CMAKE_BINARY_DIR}/doxygen/html
COMMENT "Generating API documentation with Doxygen" VERBATIM)
endif()

1
ign-math/doc/footer.html Normal file
View File

@ -0,0 +1 @@
<!--</td></tr></table>-->

67
ign-math/doc/header.html Normal file
View File

@ -0,0 +1,67 @@
<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
<html>
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8">
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="keywords" content="Ingition: Math">
<title>Ignition: $title</title>
<link href="tabs.css" rel="stylesheet" type="text/css">
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css">
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
$(document).ready(function() { searchBox.OnSelectItem(0); });
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css">
<!--<link href="tabs.css" rel="stylesheet" type="text/css">-->
<link href="style.css" rel="stylesheet" type="text/css">
</head>
<body>
<div class="leftbar">
<h2 style="text-align:center;">
<a href="index.html"><img src="ignition_logo.svg" width="180px"/></a>
</h2>
<div class="menu">
<dl>
<dt>API</dt>
<dd><a href="classes.html">Class List</a></dd>
<dd><a href="hierarchy.html">Class Hierarchy</a></dd>
<dd><a href="globals.html">Globals</a></dd>
<dd><a href="namespacemembers.html">Namespace Members</a></dd>
<dd><a href="files.html">Files</a></dd>
</dl>
</div>
<div class="menu">
<dl>
<dt>Links</dt>
<dd><a href="http://ignitionrobotics.org">Ignition Website</a></dd>
<dd><a href="https://bitbucket.org/ignitionrobotics/ign-math/issues/new">Report Documentation Issues</a></dd>
</dl>
</div>
<div id="MSearchBox" class="MSearchBoxInactive">
<span>
<img id="MSearchSelect" src="search/mag_sel.png"
onmouseover="return searchBox.OnSearchSelectShow()"
onmouseout="return searchBox.OnSearchSelectHide()"
alt=""/>
<input type="text" id="MSearchField" value="Search" accesskey="S"
onfocus="searchBox.OnSearchFieldFocus(true)"
onblur="searchBox.OnSearchFieldFocus(false)"
onkeyup="searchBox.OnSearchFieldChange(event)"/>
</span>
</div> <!-- End MSearchBox -->
<div id="MSearchResultsWindow" style="position: static; display: block; border: none; background-color: #ffffff; width: 18em;">
<iframe src="javascript:void(0)" frameborder="0"
name="MSearchResults"
style="height: 500px; width: 18em; display: block; text-wrap: unrestricted">
</iframe>
</div>
</div>
<div id="top">

2306
ign-math/doc/ignition.in Normal file
View File

File diff suppressed because it is too large Load Diff

249
ign-math/doc/ignition_logo.svg Normal file
View File

@ -0,0 +1,249 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg
xmlns:dc="http://purl.org/dc/elements/1.1/"
xmlns:cc="http://creativecommons.org/ns#"
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
xmlns:svg="http://www.w3.org/2000/svg"
xmlns="http://www.w3.org/2000/svg"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
width="473.20953"
height="338.49789"
id="svg3097"
version="1.1"
inkscape:version="0.48.4 r9939"
sodipodi:docname="ignition_logo.svg">
<defs
id="defs3099" />
<sodipodi:namedview
id="base"
pagecolor="#ffffff"
bordercolor="#666666"
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="0.7"
inkscape:cx="-8.01737"
inkscape:cy="108.51396"
inkscape:document-units="px"
inkscape:current-layer="layer1"
showgrid="false"
fit-margin-top="0"
fit-margin-left="0"
fit-margin-right="0"
fit-margin-bottom="0"
inkscape:window-width="1350"
inkscape:window-height="857"
inkscape:window-x="457"
inkscape:window-y="89"
inkscape:window-maximized="0" />
<metadata
id="metadata3102">
<rdf:RDF>
<cc:Work
rdf:about="">
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title />
</cc:Work>
</rdf:RDF>
</metadata>
<g
inkscape:label="Layer 1"
inkscape:groupmode="layer"
id="layer1"
transform="translate(-159.66847,-323.82846)">
<g
id="g44"
transform="matrix(1.25,0,0,-1.25,389.76025,331.23471)">
<path
d="m 0,0 c 0,-3.272 2.927,-5.924 6.534,-5.924 3.61,0 6.535,2.652 6.535,5.924 0,3.272 -2.925,5.925 -6.535,5.925 C 2.927,5.925 0,3.272 0,0"
style="fill:#f1623b;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path46"
inkscape:connector-curvature="0" />
</g>
<g
id="g48"
transform="matrix(1.25,0,0,-1.25,369.82862,394.56733)">
<path
d="m 0,0 c 0,-5.404 -4.833,-9.788 -10.797,-9.788 -5.962,0 -10.798,4.384 -10.798,9.788 0,1.761 0.513,3.412 1.412,4.84 1.86,2.956 5.366,4.949 9.386,4.949 C -4.833,9.789 0,5.406 0,0"
style="fill:#f1623b;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path50"
inkscape:connector-curvature="0" />
</g>
<g
id="g52"
transform="matrix(1.25,0,0,-1.25,337.86512,375.95645)">
<path
d="m 0,0 c 0,-2.816 -2.52,-5.1 -5.626,-5.1 -3.108,0 -5.626,2.284 -5.626,5.1 0,2.816 2.518,5.101 5.626,5.101 C -2.52,5.101 0,2.816 0,0"
style="fill:#f1623b;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path54"
inkscape:connector-curvature="0" />
</g>
<path
d="m 334.872,370.83683 -7.95625,0 0,-15.96875 7.95625,0 0,15.96875 z"
style="fill:#f1623b;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path56"
inkscape:connector-curvature="0" />
<g
id="g58"
transform="matrix(1.25,0,0,-1.25,349.28362,389.69421)">
<path
d="M 0,0 -4.691,-3.898 -15.124,6.425 -10.433,10.321 0,0 z"
style="fill:#f1623b;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path60"
inkscape:connector-curvature="0" />
</g>
<g
id="g62"
transform="matrix(1.25,0,0,-1.25,324.0755,339.67108)">
<path
d="m 0,0 c 0,0 0.454,-6.8 5.455,-6.8 5.001,0 5.228,5.358 5.228,5.358 0,0 7.954,-11.538 -5.228,-11.538 C -7.729,-12.98 0,0 0,0"
style="fill:#f1623b;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path64"
inkscape:connector-curvature="0" />
</g>
<g
id="g66"
transform="matrix(1.25,0,0,-1.25,422.71925,394.56733)">
<path
d="m 0,0 c 0,-5.404 4.833,-9.788 10.794,-9.788 5.965,0 10.799,4.384 10.799,9.788 0,1.761 -0.512,3.412 -1.409,4.84 -1.861,2.956 -5.368,4.949 -9.39,4.949 C 4.833,9.789 0,5.406 0,0"
style="fill:#f1623b;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path68"
inkscape:connector-curvature="0" />
</g>
<g
id="g70"
transform="matrix(1.25,0,0,-1.25,454.68275,375.95645)">
<path
d="m 0,0 c 0,-2.816 2.52,-5.1 5.626,-5.1 3.107,0 5.628,2.284 5.628,5.1 0,2.816 -2.521,5.101 -5.628,5.101 C 2.52,5.101 0,2.816 0,0"
style="fill:#f1623b;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path72"
inkscape:connector-curvature="0" />
</g>
<path
d="m 457.67575,370.83683 7.9575,0 0,-15.96875 -7.9575,0 0,15.96875 z"
style="fill:#f1623b;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path74"
inkscape:connector-curvature="0" />
<g
id="g76"
transform="matrix(1.25,0,0,-1.25,443.26312,389.69421)">
<path
d="M 0,0 4.692,-3.898 15.124,6.425 10.432,10.321 0,0 z"
style="fill:#f1623b;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path78"
inkscape:connector-curvature="0" />
</g>
<g
id="g80"
transform="matrix(1.25,0,0,-1.25,468.47125,339.67108)">
<path
d="m 0,0 c 0,0 -0.452,-6.8 -5.454,-6.8 -4.999,0 -5.23,5.358 -5.23,5.358 0,0 -7.953,-11.538 5.23,-11.538 C 7.728,-12.98 0,0 0,0"
style="fill:#f1623b;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path82"
inkscape:connector-curvature="0" />
</g>
<path
d="m 422.7195,379.30433 -52.28125,0 0,-14.42375 52.28125,0 0,14.42375 z m -21.455,-35.80125 0,-6.76125 c 0,-1.81625 -1.62,-3.285 -3.62125,-3.285 -2.00125,0 -3.6225,1.46875 -3.6225,3.285 l 0,6.585 c -20.4625,1.18125 -36.6525,16.37 -36.6525,34.94875 l 0,46.6225 c 0,10.23875 9.1575,12.105 20.4575,12.105 l 37.505,0 c 11.29875,0 20.4575,-1.86625 20.4575,-12.105 l 0,-46.6225 c 0,-17.93 -15.0825,-32.70125 -34.52375,-34.7725"
style="fill:#f1623b;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path84"
inkscape:connector-curvature="0" />
<g
id="g86"
transform="matrix(1.25,0,0,-1.25,378.879,495.55483)">
<path
d="m 0,0 c 0,0 1.215,6.836 3.784,6.836 0.135,-6.393 8.377,-10.801 6.187,-21.166 19.458,7.497 20.304,31.971 20.304,31.971 0,0 -0.117,-0.883 4.259,-5.733 7.786,20.727 -10.451,29.928 -10.451,29.928 l -1.348,0.003 C 23.916,40.416 28.458,33.285 15.008,19.623 9.585,25.687 0.048,33.539 5.561,41.89 4.985,41.89 4.394,41.893 3.784,41.893 -20.647,23.148 0,0 0,0"
style="fill:#f1623b;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path88"
inkscape:connector-curvature="0" />
</g>
<g
id="g90"
transform="matrix(1.25,0,0,-1.25,207.40347,592.96504)">
<path
d="m 0,0 -36.232,0 c -1.087,0 -1.956,0.869 -1.956,2.028 l 0,3.986 c 0,1.16 0.869,2.029 1.956,2.029 l 14.13,0 0,40.29 -6.086,0 c -1.087,0 -1.957,0.87 -1.957,1.957 l 0,4.058 c 0,1.159 0.87,2.028 1.957,2.028 l 20.145,0 c 1.159,0 2.028,-0.869 2.028,-2.028 l 0,-4.058 c 0,-1.087 -0.869,-1.957 -2.028,-1.957 l -6.015,0 0,-40.29 14.058,0 c 1.159,0 2.029,-0.869 2.029,-2.029 l 0,-3.986 C 2.029,0.869 1.159,0 0,0"
style="fill:#606163;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path92"
inkscape:connector-curvature="0" />
</g>
<g
id="g94"
transform="matrix(1.25,0,0,-1.25,260.3051,552.74779)">
<path
d="m 0,0 -16.087,0 c -4.42,0 -8.043,-3.623 -8.043,-8.044 l 0,-8.044 c 0,-4.42 3.623,-8.043 8.043,-8.043 L 0,-24.131 0,0 z m -8.043,-48.262 -22.174,0 c -1.087,0 -1.957,0.87 -1.957,2.03 l 0,3.986 c 0,1.158 0.87,2.028 1.957,2.028 l 22.174,0 c 4.42,0 8.043,3.623 8.043,8.044 l -16.087,0 c -8.841,0 -16.087,7.246 -16.087,16.086 l 0,8.044 c 0,8.913 7.246,16.16 16.087,16.16 l 22.102,0 c 1.159,0 2.028,-0.942 2.028,-2.03 l 0,-38.26 c 0,-8.842 -7.246,-16.088 -16.086,-16.088"
style="fill:#606163;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path96"
inkscape:connector-curvature="0" />
</g>
<g
id="g98"
transform="matrix(1.25,0,0,-1.25,328.24334,592.96504)">
<path
d="m 0,0 -3.985,0 c -1.161,0 -2.03,0.869 -2.03,2.028 l 0,22.102 c 0,4.421 -3.623,8.044 -8.043,8.044 l -16.087,0 0,-30.146 C -30.145,0.869 -31.087,0 -32.174,0 l -4.058,0 c -1.087,0 -1.956,0.869 -1.956,2.028 l 0,36.232 c 0,1.088 0.869,2.03 1.956,2.03 l 22.174,0 c 8.84,0 16.086,-7.247 16.086,-16.16 l 0,-22.102 C 2.028,0.869 1.159,0 0,0"
style="fill:#606163;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path100"
inkscape:connector-curvature="0" />
</g>
<g
id="g102"
transform="matrix(1.25,0,0,-1.25,368.55463,532.54879)">
<path
d="m 0,0 -3.986,0 c -1.159,0 -2.029,0.87 -2.029,1.957 l 0,4.058 c 0,1.159 0.87,2.028 2.029,2.028 l 3.986,0 c 1.159,0 2.029,-0.869 2.029,-2.028 l 0,-4.058 C 2.029,0.87 1.159,0 0,0 m 16.087,-48.333 -36.232,0 c -1.087,0 -1.957,0.869 -1.957,2.028 l 0,3.986 c 0,1.16 0.87,2.029 1.957,2.029 l 14.13,0 0,24.131 -6.087,0 c -1.086,0 -1.957,0.942 -1.957,2.029 l 0,4.057 c 0,1.088 0.871,2.03 1.957,2.03 l 12.102,0 c 1.159,0 2.029,-0.942 2.029,-2.03 l 0,-30.217 14.058,0 c 1.159,0 2.028,-0.869 2.028,-2.029 l 0,-3.986 c 0,-1.159 -0.869,-2.028 -2.028,-2.028"
style="fill:#606163;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path104"
inkscape:connector-curvature="0" />
</g>
<g
id="g106"
transform="matrix(1.25,0,0,-1.25,449.08313,592.96504)">
<path
d="m 0,0 -6.015,0 c -8.839,0 -16.087,7.246 -16.087,16.086 l 0,16.088 -14.129,0 c -1.088,0 -1.957,0.942 -1.957,2.029 l 0,4.057 c 0,1.088 0.869,2.03 1.957,2.03 l 14.129,0 0,14.058 c 0,1.159 0.871,2.028 2.029,2.028 l 3.986,0 c 1.159,0 2.029,-0.869 2.029,-2.028 l 0,-14.058 14.058,0 c 1.159,0 2.028,-0.942 2.028,-2.03 l 0,-4.057 C 2.028,33.116 1.159,32.174 0,32.174 l -14.058,0 0,-16.088 c 0,-4.42 3.623,-8.043 8.043,-8.043 l 6.015,0 c 1.159,0 2.028,-0.869 2.028,-2.029 l 0,-3.986 C 2.028,0.869 1.159,0 0,0"
style="fill:#606163;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path108"
inkscape:connector-curvature="0" />
</g>
<g
id="g110"
transform="matrix(1.25,0,0,-1.25,489.39443,532.54879)">
<path
d="m 0,0 -3.986,0 c -1.159,0 -2.029,0.87 -2.029,1.957 l 0,4.058 c 0,1.159 0.87,2.028 2.029,2.028 l 3.986,0 c 1.159,0 2.029,-0.869 2.029,-2.028 l 0,-4.058 C 2.029,0.87 1.159,0 0,0 m 16.087,-48.333 -36.232,0 c -1.087,0 -1.957,0.869 -1.957,2.028 l 0,3.986 c 0,1.16 0.87,2.029 1.957,2.029 l 14.13,0 0,24.131 -6.087,0 c -1.086,0 -1.957,0.942 -1.957,2.029 l 0,4.057 c 0,1.088 0.871,2.03 1.957,2.03 l 12.102,0 c 1.159,0 2.029,-0.942 2.029,-2.03 l 0,-30.217 14.058,0 c 1.159,0 2.028,-0.869 2.028,-2.029 l 0,-3.986 c 0,-1.159 -0.869,-2.028 -2.028,-2.028"
style="fill:#606163;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path112"
inkscape:connector-curvature="0" />
</g>
<g
id="g114"
transform="matrix(1.25,0,0,-1.25,562.40473,562.80266)">
<path
d="m 0,0 c 0,4.421 -3.623,8.044 -8.043,8.044 l -8.044,0 c -4.421,0 -8.044,-3.623 -8.044,-8.044 l 0,-8.044 c 0,-4.42 3.623,-8.043 8.044,-8.043 l 8.044,0 c 4.42,0 8.043,3.623 8.043,8.043 L 0,0 z m -8.043,-24.13 -8.044,0 c -8.841,0 -16.087,7.246 -16.087,16.086 l 0,8.044 c 0,8.913 7.246,16.16 16.087,16.16 l 8.044,0 C 0.797,16.16 8.043,8.913 8.043,0 l 0,-8.044 c 0,-8.84 -7.246,-16.086 -16.086,-16.086"
style="fill:#606163;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path116"
inkscape:connector-curvature="0" />
</g>
<g
id="g118"
transform="matrix(1.25,0,0,-1.25,630.34173,592.96504)">
<path
d="m 0,0 -3.984,0 c -1.161,0 -2.03,0.869 -2.03,2.028 l 0,22.102 c 0,4.421 -3.623,8.044 -8.043,8.044 l -16.088,0 0,-30.146 C -30.145,0.869 -31.086,0 -32.174,0 l -4.056,0 c -1.088,0 -1.958,0.869 -1.958,2.028 l 0,36.232 c 0,1.088 0.87,2.03 1.958,2.03 l 22.173,0 c 8.84,0 16.086,-7.247 16.086,-16.16 l 0,-22.102 C 2.029,0.869 1.16,0 0,0"
style="fill:#606163;fill-opacity:1;fill-rule:nonzero;stroke:none"
id="path120"
inkscape:connector-curvature="0" />
</g>
<text
xml:space="preserve"
style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans"
x="516.81549"
y="661.64667"
id="text3104"
sodipodi:linespacing="125%"><tspan
sodipodi:role="line"
id="tspan3106"
x="516.81549"
y="661.64667"
style="font-size:48px;fill:#666666">Math</tspan></text>
</g>
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 13 KiB

17
ign-math/doc/mainpage.html Normal file
View File

@ -0,0 +1,17 @@
/** \mainpage Ignition: Math API Reference
Welcome to the Ingition Math library API. Should you find problems with this documentation - typos, unclear phrases,
or insufficient detail - please create a <a
href="https://bitbucket.org/osrf/ignition-math/issues/new">new bitbucket issue</a>.
Include sufficient detail to quickly locate the problematic documentation.
<dl>
<dt>API</dt>
<dd><a href="classes.html">Class List</a>- Index of all classes in Ignition Math, organized alphabetically</dd><br/>
<dd><a href="hierarchy.html">Class Hierarchy</a> - Heirarchical class structure.</dd><br/>
<dd><a href="globals.html">Globals</a> - Global defines and variables.</dd><br/>
<dd><a href="namespacemembers.html">Namespace Members</a> - Functions and other goodies in the igntion::math namesapce</dd><br/>
<dd><a href="files.html">Files</a> - A list of all the files.</dd><br/>
</dl>
*/

107
ign-math/doc/style.css Normal file
View File

@ -0,0 +1,107 @@
body, table, div, p, dl {
font-family: arial, verdana, sans, sans-serif;
background-color: #FFF;
font-size: 12px;
margin: 0;
padding 0;
}
div#top {
margin: 0 0 0 20em;
}
div.header {
margin-left: 20em;
}
div.contents {
margin-top: 0px;
margin-left: 20em;
margin-right: 10px;
}
.floatright
{
float: right;
margin: 0 0 1em 1em;
}
.timestamp {
text-align:right;
background-color: #DDD;
font-size:75%;
}
div.leftbar
{
text-align:left;
float: left;
border-right: 1px solid #dddddd;
width: 18em;
margin: 0 0 0 0;
padding: 4 4 4 4;
background-color: #ffffff;
position: fixed;
height: 100%;
}
div.menu {
#display:block;
background:#ffffff;
font-size: 90%;
/*border-top: 2px solid #000000;
* border-bottom: 2px solid #000000;
* */
margin: 0 0 10px 0;
}
div.menu dl {
margin-top: 0px;
margin-bottom: 5px;
}
div.menu dt {
font-weight:bold;
padding:0 4px 4px 4px;
font-size: 110%;
text-align: left;
text-decoration:none;
}
div.menu dd {
font-weight: bold;
margin-left: 0px;
padding-left: 20px;
padding-bottom: 2px;
font-size: 100%;
}
div.leftbar img {
border:0;
}
div.submenu dd {
font-size: 70%;
margin-left: 8px;
padding-left: 10px;
padding-bottom: 3px;
}
div.submenu dd .secondline {
margin-left: 12px;
}
#MSearchBox
{
border: 1px solid black;
position: static;
margin: 10px;
display: block;
height: 20px;
}
#MSearchField
{
background:none;
}

10
ign-math/examples/CMakeLists.txt Normal file
View File

@ -0,0 +1,10 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
# Find the Ignition-Math library
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)
add_executable(triangle_example triangle_example.cc)

80
ign-math/examples/triangle_example.cc Normal file
View File

@ -0,0 +1,80 @@
/*
* Copyright (C) 2012-2014 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 <iostream>
#include <ignition/math.hh>
int main(int argc, char **argv)
{
// Create a triangle with the following vertices:
// 1: x = -1, y = 0
// 2: x = 0, y = 1
// 3: x = 1, y = 0
ignition::math::Triangled tri(
ignition::math::Vector2d(-1, 0),
ignition::math::Vector2d(0, 1),
ignition::math::Vector2d(1, 0));
// The individual vertices are accessible through the [] operator
std::cout << "Vertex 1: " << tri[0] << "\n"
<< "Vertex 2: " << tri[1] << "\n"
<< "Vertex 3: " << tri[2] << "\n";
// Each side of the triangle is also accessible via the Side function
std::cout << "Side 1: " << tri.Side(0) << "\n"
<< "Side 2: " << tri.Side(1) << "\n"
<< "Side 3: " << tri.Side(2) << "\n";
// It's also possible to set each vertex individually.
tri.Set(0, ignition::math::Vector2d(-10, 0));
tri.Set(1, ignition::math::Vector2d(0, 20));
tri.Set(2, ignition::math::Vector2d(10, 2));
// Or set all the vertices at once.
tri.Set(ignition::math::Vector2d(-1, 0),
ignition::math::Vector2d(0, 1),
ignition::math::Vector2d(1, 0));
// You can get the perimeter length and area of the triangle
std::cout << "Perimeter=" << tri.Perimeter()
<< " Area=" << tri.Area() << "\n";
// The Contains functions check if a line or point is inside the triangle
if (tri.Contains(ignition::math::Vector2d(0, 0.5)))
std::cout << "Triangle contains the point 0, 0.5\n";
else
std::cout << "Triangle does not contain the point 0, 0.5\n";
// The Intersect function check if a line segment intersects the triangle.
// It also returns the points of intersection
ignition::math::Vector2d pt1, pt2;
if (tri.Intersects(ignition::math::Line2d(-2, 0.5, 2, 0.5), pt1, pt2))
{
std::cout << "A line from (-2, 0.5) to (2, 0.5) intersects "
<< "the triangle at the\nfollowing points:\n"
<< "\t Pt1=" << pt1 << "\n"
<< "\t Pt2=" << pt2 << "\n";
}
else
{
std::cout << "A line from (-2, 0.5) to (2, 0.5) does not intersect "
<< "the triangle\n";
}
// There are more functions in Triangle. Take a look at the API;
// http://ignitionrobotics.org/libraries/ign_mat/api
}

72
ign-math/examples/vector2_example.cc Normal file
View File

@ -0,0 +1,72 @@
/*
* Copyright (C) 2012-2014 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 <iostream>
#include <ignition/math.hh>
int main(int argc, char **argv)
{
// Create a Vector2 called vec2 of doubles using the typedef Vector2d.
// The initial x any y values are zero.\n\n";
ignition::math::Vector2d vec2;
// The x and y component of vec2 can be set at anytime.
vec2.Set(2.0, 4.0);
// The Vector2 class is a template, so you can also create a Vector2 using
// ignition::math::Vector2<double>
ignition::math::Vector2<double> vec2a;
vec2a.Set(1.0, 2.0);
// It's also possible to set initial values. This time we are using
// a Vector2 of floats
ignition::math::Vector2f vec2b(1.2, 3.4);
// We can output the contents of each vector using std::cout
std::cout << "Vec2: " << vec2 << "\n"
<< "Vec2a: " << vec2a << "\n"
<< "Vec2b: " << vec2b << "\n";
// You can also get access to each component in the vector using the
// X(), Y() accessors or the [] operator.
std::cout << "Vec2: x=" << vec2.X() << " y=" << vec2.Y() << "\n";
std::cout << "Vec2a: x=" << vec2a[0] << " y=" << vec2a[1] << "\n";
std::cout << "Vec2b: x=" << vec2b.X() << " y=" << vec2b[1] << "\n";
// An IndexException will be thrown if the [] operator is given a
// value that is too high
try
{
std::cout << vec2[3] << std::endl;
} catch(ignition::math::IndexException &_e) {
std::cerr << _e.what() << std::endl;
}
// The Vector2 class overloads many common operators
std::cout << vec2 * vec2a << "\n"
<< vec2 + vec2a << "\n"
<< vec2 - vec2a << "\n"
<< vec2 / vec2a << "\n";
// There are also many useful function such as finding the distance
// between two vectors
std::cout << vec2.Distance(vec2a) << std::endl;
// There are more functions in Vector2. Take a look at the API;
// http://ignitionrobotics.org/libraries/ign_mat/api
}

1
ign-math/include/CMakeLists.txt Normal file
View File

@ -0,0 +1 @@
add_subdirectory(ignition)

1
ign-math/include/ignition/CMakeLists.txt Normal file
View File

@ -0,0 +1 @@
add_subdirectory(math)

49
ign-math/include/ignition/math/AffineException.hh Normal file
View File

@ -0,0 +1,49 @@
/*
* Copyright (C) 2012-2014 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_AFFINE_EXCEPTION_HH_
#define IGNITION_MATH_AFFINE_EXCEPTION_HH_
#include <stdexcept>
#include <ignition/math/Helpers.hh>
// Ignore warning C4275. It is okay to ignore this according to microsoft:
// https://msdn.microsoft.com/en-us/library/3tdb471s.aspx
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4275)
#endif
namespace ignition
{
namespace math
{
/// \class AffineException AffineException.hh
/// ignition/math/AffineException.hh
/// \brief Exception that is thrown when a matrix is not affine.
class IGNITION_VISIBLE AffineException : public std::runtime_error
{
public: AffineException() : std::runtime_error("Not and affine matrix")
{}
};
}
}
#ifdef _MSC_VER
#pragma warning(pop)
#endif
#endif

203
ign-math/include/ignition/math/Angle.hh Normal file
View File

@ -0,0 +1,203 @@
/*
* Copyright (C) 2012-2014 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_ANGLE_HH_
#define IGNITION_MATH_ANGLE_HH_
#include <iostream>
#include <ignition/math/Helpers.hh>
/// \brief Macro that converts radians to degrees
/// \param[in] radians
/// \return degrees
#define IGN_RTOD(r) ((r) * 180 / IGN_PI)
/// \brief Converts degrees to radians
/// \param[in] degrees
/// \return radians
#define IGN_DTOR(d) ((d) * IGN_PI / 180)
/// \brief Macro that normalizes an angle in the range -Pi to Pi
/// \param[in] angle
/// \return the angle, in range
#define IGN_NORMALIZE(a) (atan2(sin(a), cos(a)))
namespace ignition
{
namespace math
{
/// \class Angle Angle.hh ignition/math/Angle.hh
/// \brief An angle and related functions.
class IGNITION_VISIBLE Angle
{
/// \brief math::Angle(0)
public: static const Angle Zero;
/// \brief math::Angle(IGN_PI)
public: static const Angle Pi;
/// \brief math::Angle(IGN_PI * 0.5)
public: static const Angle HalfPi;
/// \brief math::Angle(IGN_PI * 2)
public: static const Angle TwoPi;
/// \brief Constructor
public: Angle();
/// \brief Copy Constructor
/// \param[in] _radian Radians
// cppcheck-suppress noExplicitConstructor
public: Angle(double _radian);
/// \brief Copy constructor
/// \param[in] _angle Angle to copy
public: Angle(const Angle &_angle);
/// \brief Destructor
public: virtual ~Angle();
/// \brief Set the value from an angle in radians
/// \param[in] _radian Radian value
public: void Radian(double _radian);
/// \brief Set the value from an angle in degrees
/// \param[in] _degree Degree value
public: void Degree(double _degree);
/// \brief Get the angle in radians
/// \return double containing the angle's radian value
public: double Radian() const;
/// \brief Get the angle in degrees
/// \return double containing the angle's degree value
public: double Degree() const;
/// \brief Normalize the angle in the range -Pi to Pi
public: void Normalize();
/// \brief Return the angle's radian value
/// \return double containing the angle's radian value
public: double operator()() const;
/// \brief Dereference operator
/// \return Double containing the angle's radian value
public: inline double operator*() const
{
return value;
}
/// \brief Substraction, result = this - _angle
/// \param[in] _angle Angle for substraction
/// \return the new angle
public: Angle operator-(const Angle &_angle) const;
/// \brief Addition operator, result = this + _angle
/// \param[in] _angle Angle for addition
/// \return the new angle
public: Angle operator+(const Angle &_angle) const;
/// \brief Multiplication operator, result = this * _angle
/// \param[in] _angle Angle for multiplication
/// \return the new angle
public: Angle operator*(const Angle &_angle) const;
/// \brief Division, result = this / _angle
/// \param[in] _angle Angle for division
/// \return the new angle
public: Angle operator/(const Angle &_angle) const;
/// \brief Subtraction set, this = this - _angle
/// \param[in] _angle Angle for subtraction
/// \return angle
public: Angle operator-=(const Angle &_angle);
/// \brief Addition set, this = this + _angle
/// \param[in] _angle Angle for addition
/// \return angle
public: Angle operator+=(const Angle &_angle);
/// \brief Multiplication set, this = this * _angle
/// \param[in] _angle Angle for multiplication
/// \return angle
public: Angle operator*=(const Angle &_angle);
/// \brief Division set, this = this / _angle
/// \param[in] _angle Angle for division
/// \return angle
public: Angle operator/=(const Angle &_angle);
/// \brief Equality operator, result = this == _angle
/// \param[in] _angle Angle to check for equality
/// \return true if this == _angle
public: bool operator==(const Angle &_angle) const;
/// \brief Inequality
/// \param[in] _angle Angle to check for inequality
/// \return true if this != _angle
public: bool operator!=(const Angle &_angle) const;
/// \brief Less than operator
/// \param[in] _angle Angle to check
/// \return true if this < _angle
public: bool operator<(const Angle &_angle) const;
/// \brief Less or equal operator
/// \param[in] _angle Angle to check
/// \return true if this <= _angle
public: bool operator<=(const Angle &_angle) const;
/// \brief Greater than operator
/// \param[in] _angle Angle to check
/// \return true if this > _angle
public: bool operator>(const Angle &_angle) const;
/// \brief Greater or equal operator
/// \param[in] _angle Angle to check
/// \return true if this >= _angle
public: bool operator>=(const Angle &_angle) const;
/// \brief Stream insertion operator. Outputs in degrees
/// \param[in] _out output stream
/// \param[in] _a angle to output
/// \return The output stream
public: friend std::ostream &operator<<(std::ostream &_out,
const ignition::math::Angle &_a)
{
_out << _a.Radian();
return _out;
}
/// \brief Stream extraction operator. Assumes input is in radians
/// \param in input stream
/// \param pt angle to read value into
/// \return The input stream
public: friend std::istream &operator>>(std::istream &_in,
ignition::math::Angle &_a)
{
// Skip white spaces
_in.setf(std::ios_base::skipws);
_in >> _a.value;
return _in;
}
/// The angle in radians
private: double value;
};
}
}
#endif

231
ign-math/include/ignition/math/Box.hh Normal file
View File

@ -0,0 +1,231 @@
/*
* Copyright (C) 2012-2014 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_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
{
namespace math
{
// Forward declaration of private data
class BoxPrivate;
/// \class Box Box.hh ignition/math/Box.hh
/// \brief Mathematical representation of a box and related functions.
class IGNITION_VISIBLE Box
{
/// \brief Default constructor
public: Box();
/// \brief Constructor. This constructor will compute the box's
/// minimum and maximum corners based on the two arguments.
/// \param[in] _vec1 One corner of the box
/// \param[in] _vec2 Another corner of the box
public: Box(const Vector3d &_vec1, const Vector3d &_vec2);
/// \brief Constructor. This constructor will compute the box's
/// minimum and maximum corners based on the arguments.
/// \param[in] _vec1X One corner's X position
/// \param[in] _vec1Y One corner's Y position
/// \param[in] _vec1Z One corner's Z position
/// \param[in] _vec2X Other corner's X position
/// \param[in] _vec2Y Other corner's Y position
/// \param[in] _vec2Z Other corner's Z position
public: Box(double _vec1X, double _vec1Y, double _vec1Z,
double _vec2X, double _vec2Y, double _vec2Z);
/// \brief Copy Constructor
/// \param[in] _b Box to copy
public: Box(const Box &_b);
/// \brief Destructor
public: virtual ~Box();
/// \brief Get the length along the x dimension
/// \return Double value of the length in the x dimension
public: double XLength() const;
/// \brief Get the length along the y dimension
/// \return Double value of the length in the y dimension
public: double YLength() const;
/// \brief Get the length along the z dimension
/// \return Double value of the length in the z dimension
public: double ZLength() const;
/// \brief Get the size of the box
/// \return Size of the box
public: math::Vector3d Size() const;
/// \brief Get the box center
/// \return The center position of the box
public: math::Vector3d Center() const;
/// \brief Merge a box with this box
/// \param[in] _box Box to add to this box
public: void Merge(const Box &_box);
/// \brief Assignment operator. Set this box to the parameter
/// \param[in] _b Box to copy
/// \return The new box.
public: Box &operator=(const Box &_b);
/// \brief Addition operator. result = this + _b
/// \param[in] _b Box to add
/// \return The new box
public: Box operator+(const Box &_b) const;
/// \brief Addition set operator. this = this + _b
/// \param[in] _b Box to add
/// \return This new box
public: const Box &operator+=(const Box &_b);
/// \brief Equality test operator
/// \param[in] _b Box to test
/// \return True if equal
public: bool operator==(const Box &_b) const;
/// \brief Inequality test operator
/// \param[in] _b Box to test
/// \return True if not equal
public: bool operator!=(const Box &_b) const;
/// \brief Subtract a vector from the min and max values
/// \param _v The vector to use during subtraction
/// \return The new box
public: Box operator-(const Vector3d &_v);
/// \brief Output operator
/// \param[in] _out Output stream
/// \param[in] _b Box to output to the stream
/// \return The stream
public: friend std::ostream &operator<<(std::ostream &_out,
const ignition::math::Box &_b)
{
_out << "Min[" << _b.Min() << "] Max[" << _b.Max() << "]";
return _out;
}
/// \brief Get the minimum corner.
/// \return The Vector3d that is the minimum corner of the box.
public: const Vector3d &Min() const;
/// \brief Get the maximum corner.
/// \return The Vector3d that is the maximum corner of the box.
public: const Vector3d &Max() const;
/// \brief Get a mutable version of the minimum corner.
/// \return The Vector3d that is the minimum corner of the box.
public: Vector3d &Min();
/// \brief Get a mutable version of the maximum corner.
/// \return The Vector3d that is the maximum corner of the box.
public: Vector3d &Max();
/// \brief Test box intersection. This test will only work if
/// both box's minimum corner is less than or equal to their
/// maximum corner.
/// \param[in] _box Box to check for intersection with this box.
/// \return True if this box intersects _box.
public: bool Intersects(const Box &_box) const;
/// \brief Check if a point lies inside the box.
/// \param[in] _p Point to check.
/// \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;
};
}
}
#endif

45
ign-math/include/ignition/math/BoxPrivate.hh Normal file
View File

@ -0,0 +1,45 @@
/*
* 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_MATH_BOX_PRIVATE_HH_
#define IGNITION_MATH_BOX_PRIVATE_HH_
#include <ignition/math/Vector3.hh>
namespace ignition
{
namespace math
{
/// \internal
/// \brief Private data for Box class
class BoxPrivate
{
/// \brief Enumeration of extents
public: enum Extent {EXTENT_NULL, EXTENT_FINITE};
/// \brief Minimum corner of the box
public: Vector3d min = Vector3d::Zero;
/// \brief Maximum corner of the box
public: Vector3d max = Vector3d::Zero;
/// \brief When set to EXTENT_NULL (the default value)
/// the min and max are not valid positions
public: Extent extent = EXTENT_NULL;
};
}
}
#endif

49
ign-math/include/ignition/math/CMakeLists.txt Normal file
View File

@ -0,0 +1,49 @@
include (${project_cmake_dir}/Utils.cmake)
set (headers
AffineException.hh
Angle.hh
Box.hh
Color.hh
Filter.hh
Frustum.hh
Helpers.hh
IndexException.hh
Inertial.hh
Kmeans.hh
Line2.hh
Line3.hh
MassMatrix3.hh
Matrix3.hh
Matrix4.hh
OrientedBox.hh
PID.hh
Plane.hh
Pose3.hh
Quaternion.hh
Rand.hh
RotationSpline.hh
SemanticVersion.hh
SignalStats.hh
SphericalCoordinates.hh
Spline.hh
Temperature.hh
Triangle.hh
Triangle3.hh
Vector2.hh
Vector3.hh
Vector3Stats.hh
Vector4.hh
)
set (ign_headers "" CACHE INTERNAL "Ignition math headers" FORCE)
foreach (hdr ${headers})
APPEND_TO_CACHED_STRING(ign_headers
"Ignition math headers" "#include <ignition/math/${hdr}>\n")
endforeach()
configure_file (${CMAKE_CURRENT_SOURCE_DIR}/math.hh.in
${CMAKE_CURRENT_BINARY_DIR}/math.hh)
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})

318
ign-math/include/ignition/math/Color.hh Normal file
View File

@ -0,0 +1,318 @@
/*
* Copyright (C) 2017 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_COLOR_HH_
#define IGNITION_MATH_COLOR_HH_
#include <iostream>
#include <ignition/math/Helpers.hh>
#include <ignition/math/Vector3.hh>
namespace ignition
{
namespace math
{
/// \class Color Color.hh ignition/math/Color.hh
/// \brief Defines a color using a red (R), green (G), blue (B), and alpha
/// (A) component. Each color component is in the range [0..1].
class IGNITION_VISIBLE Color
{
/// \brief (1, 1, 1)
public: static const Color White;
/// \brief (0, 0, 0)
public: static const Color Black;
/// \brief (1, 0, 0)
public: static const Color Red;
/// \brief (0, 1, 0)
public: static const Color Green;
/// \brief (0, 0, 1)
public: static const Color Blue;
/// \brief (1, 1, 0)
public: static const Color Yellow;
/// \brief (1, 0, 1)
public: static const Color Magenta;
/// \brief (0, 1, 1)
public: static const Color Cyan;
/// \def RGBA
/// \brief A RGBA packed value as an unsigned int
public: typedef unsigned int RGBA;
/// \def BGRA
/// \brief A BGRA packed value as an unsigned int
public: typedef unsigned int BGRA;
/// \def ARGB
/// \brief A ARGB packed value as an unsigned int
public: typedef unsigned int ARGB;
/// \def ABGR
/// \brief A ABGR packed value as an unsigned int
public: typedef unsigned int ABGR;
/// \brief Constructor
public: Color();
/// \brief Constructor
/// \param[in] _r Red value (range 0 to 1)
/// \param[in] _g Green value (range 0 to 1
/// \param[in] _b Blue value (range 0 to 1
/// \param[in] _a Alpha value (0=transparent, 1=opaque)
public: Color(const float _r, const float _g, const float _b,
const float _a = 1.0);
/// \brief Copy Constructor
/// \param[in] _clr Color to copy
public: Color(const Color &_clr);
/// \brief Destructor
public: virtual ~Color();
/// \brief Reset the color to default values to red=0, green=0,
/// blue=0, alpha=1.
public: void Reset();
/// \brief Set the contents of the vector
/// \param[in] _r Red value (range 0 to 1)
/// \param[in] _g Green value (range 0 to 1)
/// \param[in] _b Blue value (range 0 to 1)
/// \param[in] _a Alpha value (0=transparent, 1=opaque)
public: void Set(const float _r = 1, const float _g = 1,
const float _b = 1, const float _a = 1);
/// \brief Get the color in HSV colorspace
/// \return HSV values in a Vector3f format. A vector3f containing
/// {NAN_F, NAN_F, NAN_F} is returned on error.
public: Vector3f HSV() const;
/// \brief Set a color based on HSV values
/// \param[in] _h Hue(0..360)
/// \param[in] _s Saturation(0..1)
/// \param[in] _v Value(0..1)
public: void SetFromHSV(const float _h, const float _s, const float _v);
/// \brief Get the color in YUV colorspace
/// \return the YUV color
public: Vector3f YUV() const;
/// \brief Set from yuv
/// \param[in] _y value
/// \param[in] _u value
/// \param[in] _v value
public: void SetFromYUV(const float _y, const float _u, const float _v);
/// \brief Equal operator
/// \param[in] _pt Color to copy
/// \return Reference to this color
public: Color &operator=(const Color &_pt);
/// \brief Array index operator
/// \param[in] _index Color component index(0=red, 1=green, 2=blue)
/// \return r, g, b, or a when _index is 0, 1, 2 or 3. A NAN_F value is
/// returned if the _index is invalid
public: float operator[](const unsigned int _index);
/// \brief Get as uint32 RGBA packed value
/// \return the color
public: RGBA AsRGBA() const;
/// \brief Get as uint32 BGRA packed value
/// \return the color
public: BGRA AsBGRA() const;
/// \brief Get as uint32 ARGB packed value
/// \return the color
public: ARGB AsARGB() const;
/// \brief Get as uint32 ABGR packed value
/// \return the color
public: ABGR AsABGR() const;
/// \brief Set from uint32 RGBA packed value
/// \param[in] _v the new color
public: void SetFromRGBA(const RGBA _v);
/// \brief Set from uint32 BGRA packed value
/// \param[in] _v the new color
public: void SetFromBGRA(const BGRA _v);
/// \brief Set from uint32 ARGB packed value
/// \param[in] _v the new color
public: void SetFromARGB(const ARGB _v);
/// \brief Set from uint32 ABGR packed value
/// \param[in] _v the new color
public: void SetFromABGR(const ABGR _v);
/// \brief Addition operator (this + _pt)
/// \param[in] _pt Color to add
/// \return The resulting color
public: Color operator+(const Color &_pt) const;
/// \brief Add _v to all color components
/// \param[in] _v Value to add to each color component
/// \return The resulting color
public: Color operator+(const float &_v) const;
/// \brief Addition equal operator
/// \param[in] _pt Color to add
/// \return The resulting color
public: const Color &operator+=(const Color &_pt);
/// \brief Subtraction operator
/// \param[in] _pt The color to substract
/// \return The resulting color
public: Color operator-(const Color &_pt) const;
/// \brief Subtract _v from all color components
/// \param[in] _v Value to subtract
/// \return The resulting color
public: Color operator-(const float &_v) const;
/// \brief Subtraction equal operator
/// \param[in] _pt Color to subtract
/// \return The resulting color
public: const Color &operator-=(const Color &_pt);
/// \brief Division operator
/// \param[in] _pt Color to divide by
/// \return The resulting color
public: const Color operator/(const Color &_pt) const;
/// \brief Divide all color component by _v
/// \param[in] _v The value to divide by
/// \return The resulting color
public: const Color operator/(const float &_v) const;
/// \brief Division equal operator
/// \param[in] _pt Color to divide by
/// \return The resulting color
public: const Color &operator/=(const Color &_pt);
/// \brief Multiplication operator
/// \param[in] _pt The color to muliply by
/// \return The resulting color
public: const Color operator*(const Color &_pt) const;
/// \brief Multiply all color components by _v
/// \param[in] _v The value to multiply by
/// \return The resulting color
public: const Color operator*(const float &_v) const;
/// \brief Multiplication equal operator
/// \param[in] _pt The color to muliply by
/// \return The resulting color
public: const Color &operator*=(const Color &_pt);
/// \brief Equality operator
/// \param[in] _pt The color to check for equality
/// \return True if the this color equals _pt
public: bool operator==(const Color &_pt) const;
/// \brief Inequality operator
/// \param[in] _pt The color to check for inequality
/// \return True if the this color does not equal _pt
public: bool operator!=(const Color &_pt) const;
/// \brief Clamp the color values to valid ranges
private: void Clamp();
/// \brief Stream insertion operator
/// \param[in] _out the output stream
/// \param[in] _pt the color
/// \return the output stream
public: friend std::ostream &operator<<(std::ostream &_out,
const Color &_pt)
{
_out << _pt.r << " " << _pt.g << " " << _pt.b << " " << _pt.a;
return _out;
}
/// \brief Stream insertion operator
/// \param[in] _in the input stream
/// \param[in] _pt
public: friend std::istream &operator>> (std::istream &_in, Color &_pt)
{
// Skip white spaces
_in.setf(std::ios_base::skipws);
_in >> _pt.r >> _pt.g >> _pt.b >> _pt.a;
return _in;
}
/// \brief Get the red value
/// \return The red value
public: float R() const;
/// \brief Get the green value
/// \return The green value
public: float G() const;
/// \brief Get the blue value
/// \return The blue value
public: float B() const;
/// \brief Get the alpha value
/// \return The alpha value
public: float A() const;
/// \brief Get a mutable reference to the red value
/// \return The red value
public: float &R();
/// \brief Get a mutable reference to the green value
/// \return The green value
public: float &G();
/// \brief Get a mutable reference to the blue value
/// \return The blue value
public: float &B();
/// \brief Get a mutable reference to the alpha value
/// \return The alpha value
public: float &A();
/// \brief Set the red value
/// \param _r New red value
public: void R(const float _r);
/// \brief Set the green value
/// \param _r New green value
public: void G(const float _g);
/// \brief Set the blue value
/// \param _r New blue value
public: void B(const float _b);
/// \brief Set the alpha value
/// \param _r New alpha value
public: void A(const float _a);
/// \brief Red value
private: float r = 0;
/// \brief Green value
private: float g = 0;
/// \brief Blue value
private: float b = 0;
/// \brief Alpha value
private: float a = 1;
};
}
}
#endif

248
ign-math/include/ignition/math/Filter.hh Normal file
View File

@ -0,0 +1,248 @@
/*
* Copyright (C) 2014 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_FILTER_HH_
#define IGNITION_MATH_FILTER_HH_
#include <ignition/math/Helpers.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Quaternion.hh>
namespace ignition
{
namespace math
{
/// \class Filter Filter.hh ignition/math/Filter.hh
/// \brief Filter base class
template <class T>
class Filter
{
/// \brief Destructor.
public: virtual ~Filter() {}
/// \brief Set the output of the filter.
/// \param[in] _val New value.
public: virtual void Set(const T &_val)
{
y0 = _val;
}
/// \brief Set the cutoff frequency and sample rate.
/// \param[in] _fc Cutoff frequency.
/// \param[in] _fs Sample rate.
public: virtual void Fc(double _fc, double _fs) = 0;
/// \brief Get the output of the filter.
/// \return Filter's output.
public: virtual const T &Value() const
{
return y0;
}
/// \brief Output.
protected: T y0{};
};
/// \class OnePole Filter.hh ignition/math/Filter.hh
/// \brief A one-pole DSP filter.
/// \sa http://www.earlevel.com/main/2012/12/15/a-one-pole-filter/
template <class T>
class OnePole : public Filter<T>
{
/// \brief Constructor.
public: OnePole() = default;
/// \brief Constructor.
/// \param[in] _fc Cutoff frequency.
/// \param[in] _fs Sample rate.
public: OnePole(double _fc, double _fs)
{
this->Fc(_fc, _fs);
}
// Documentation Inherited.
public: virtual void Fc(double _fc, double _fs)
{
b1 = exp(-2.0 * IGN_PI * _fc / _fs);
a0 = 1.0 - b1;
}
/// \brief Update the filter's output.
/// \paran[in] _x Input value.
/// \return The filter's current output.
public: const T& Process(const T &_x)
{
this->y0 = a0 * _x + b1 * this->y0;
return this->y0;
}
/// \brief Input gain control.
protected: double a0 = 0;
/// \brief Gain of the feedback.
protected: double b1 = 0;
};
/// \class OnePoleQuaternion Filter.hh ignition/math/Filter.hh
/// \brief One-pole quaternion filter.
class OnePoleQuaternion : public OnePole<math::Quaterniond>
{
/// \brief Constructor.
public: OnePoleQuaternion()
{
this->Set(math::Quaterniond(1, 0, 0, 0));
}
/// \brief Constructor.
/// \param[in] _fc Cutoff frequency.
/// \param[in] _fs Sample rate.
public: OnePoleQuaternion(double _fc, double _fs)
: OnePole<math::Quaterniond>(_fc, _fs)
{
this->Set(math::Quaterniond(1, 0, 0, 0));
}
/// \brief Update the filter's output.
/// \paran[in] _x Input value.
/// \return The filter's current output.
public: const math::Quaterniond& Process(
const math::Quaterniond &_x)
{
y0 = math::Quaterniond::Slerp(a0, y0, _x);
return y0;
}
};
/// \class OnePoleVector3 Filter.hh ignition/math/Filter.hh
/// \brief One-pole vector3 filter.
class OnePoleVector3 : public OnePole<math::Vector3d>
{
/// \brief Constructor.
public: OnePoleVector3()
{
this->Set(math::Vector3d(0, 0, 0));
}
/// \brief Constructor.
/// \param[in] _fc Cutoff frequency.
/// \param[in] _fs Sample rate.
public: OnePoleVector3(double _fc, double _fs)
: OnePole<math::Vector3d>(_fc, _fs)
{
this->Set(math::Vector3d(0, 0, 0));
}
};
/// \class BiQuad Filter.hh ignition/math/Filter.hh
/// \brief Bi-quad filter base class.
/// \sa http://www.earlevel.com/main/2003/03/02/the-bilinear-z-transform/
template <class T>
class BiQuad : public Filter<T>
{
/// \brief Constructor.
public: BiQuad() = default;
/// \brief Constructor.
/// \param[in] _fc Cutoff frequency.
/// \param[in] _fs Sample rate.
public: BiQuad(double _fc, double _fs)
{
this->Fc(_fc, _fs);
}
// Documentation Inherited.
public: void Fc(double _fc, double _fs)
{
this->Fc(_fc, _fs, 0.5);
}
/// \brief Set the cutoff frequency, sample rate and Q coefficient.
/// \param[in] _fc Cutoff frequency.
/// \param[in] _fs Sample rate.
/// \param[in] _q Q coefficient.
public: void Fc(double _fc, double _fs, double _q)
{
double k = tan(IGN_PI * _fc / _fs);
double kQuadDenom = k * k + k / _q + 1.0;
this->a0 = k * k/ kQuadDenom;
this->a1 = 2 * this->a0;
this->a2 = this->a0;
this->b0 = 1.0;
this->b1 = 2 * (k * k - 1.0) / kQuadDenom;
this->b2 = (k * k - k / _q + 1.0) / kQuadDenom;
}
/// \brief Set the current filter's output.
/// \param[in] _val New filter's output.
public: virtual void Set(const T &_val)
{
this->y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val;
}
/// \brief Update the filter's output.
/// \param[in] _x Input value.
/// \return The filter's current output.
public: virtual const T& Process(const T &_x)
{
this->y0 = this->a0 * _x +
this->a1 * this->x1 +
this->a2 * this->x2 -
this->b1 * this->y1 -
this->b2 * this->y2;
this->x2 = this->x1;
this->x1 = _x;
this->y2 = this->y1;
this->y1 = this->y0;
return this->y0;
}
/// \brief Input gain control coefficients.
protected: double a0 = 0,
a1 = 0,
a2 = 0,
b0 = 0,
b1 = 0,
b2 = 0;
/// \brief Gain of the feedback coefficients.
protected: T x1{}, x2{}, y1{}, y2{};
};
/// \class BiQuadVector3 Filter.hh ignition/math/Filter.hh
/// \brief BiQuad vector3 filter
class BiQuadVector3 : public BiQuad<math::Vector3d>
{
/// \brief Constructor.
public: BiQuadVector3()
{
this->Set(math::Vector3d(0, 0, 0));
}
/// \brief Constructor.
/// \param[in] _fc Cutoff frequency.
/// \param[in] _fs Sample rate.
public: BiQuadVector3(double _fc, double _fs)
: BiQuad<math::Vector3d>(_fc, _fs)
{
this->Set(math::Vector3d(0, 0, 0));
}
};
}
}
#endif

181
ign-math/include/ignition/math/Frustum.hh Normal file
View File

@ -0,0 +1,181 @@
/*
* 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_MATH_FRUSTUM_HH_
#define IGNITION_MATH_FRUSTUM_HH_
#include <ignition/math/Plane.hh>
#include <ignition/math/Angle.hh>
#include <ignition/math/Pose3.hh>
namespace ignition
{
namespace math
{
// Forward declaration of private data
class FrustumPrivate;
/// \brief Mathematical representation of a frustum and related functions.
/// This is also known as a view frustum.
class IGNITION_VISIBLE Frustum
{
/// \brief Planes that define the boundaries of the frustum.
public: enum FrustumPlane
{
/// \brief Near plane
FRUSTUM_PLANE_NEAR = 0,
/// \brief Far plane
FRUSTUM_PLANE_FAR = 1,
/// \brief Left plane
FRUSTUM_PLANE_LEFT = 2,
/// \brief Right plane
FRUSTUM_PLANE_RIGHT = 3,
/// \brief Top plane
FRUSTUM_PLANE_TOP = 4,
/// \brief Bottom plane
FRUSTUM_PLANE_BOTTOM = 5
};
/// \brief Default constructor. With the following default values:
///
/// * near: 0.0
/// * far: 1.0
/// * fov: 0.78539 radians (45 degrees)
/// * aspect ratio: 1.0
/// * pose: Pose3d::Zero
public: Frustum();
/// \brief Constructor
/// \param[in] _near Near plane distance. This is the distance from
/// the frustum's vertex to the closest plane
/// \param[in] _far Far plane distance. This is the distance from the
/// frustum's vertex to the farthest plane.
/// \param[in] _fov Field of view. The field of view is the
/// angle between the frustum's vertex and the edges of the near or far
/// plane. This value represents the horizontal angle.
/// \param[in] _aspectRatio The aspect ratio, which is the width divided
/// by height of the near or far planes.
/// \param[in] _pose Pose of the frustum, which is the vertex (top of
/// the pyramid).
public: Frustum(const double _near,
const double _far,
const math::Angle &_fov,
const double _aspectRatio,
const math::Pose3d &_pose = math::Pose3d::Zero);
/// \brief Copy Constructor
/// \param[in] _p Frustum to copy.
public: Frustum(const Frustum &_p);
/// \brief Destructor
public: virtual ~Frustum();
/// \brief Get the near distance. This is the distance from the
/// frustum's vertex to the closest plane.
/// \return Near distance.
/// \sa SetNear
public: double Near() const;
/// \brief Set the near distance. This is the distance from the
/// frustum's vertex to the closest plane.
/// \param[in] _near Near distance.
/// \sa Near
public: void SetNear(const double _near);
/// \brief Get the far distance. This is the distance from the
/// frustum's vertex to the farthest plane.
/// \return Far distance.
/// \sa SetFar
public: double Far() const;
/// \brief Set the far distance. This is the distance from the
/// frustum's vertex to the farthest plane.
/// \param[in] _far Far distance.
/// \sa Far
public: void SetFar(const double _far);
/// \brief Get the horizontal field of view. The field of view is the
/// angle between the frustum's vertex and the edges of the near or far
/// plane. This value represents the horizontal angle.
/// \return The field of view.
/// \sa SetFOV
public: math::Angle FOV() const;
/// \brief Set the horizontal field of view. The field of view is the
/// angle between the frustum's vertex and the edges of the near or far
/// plane. This value represents the horizontal angle.
/// \param[in] _fov The field of view.
/// \sa FOV
public: void SetFOV(const math::Angle &_fov);
/// \brief Get the aspect ratio, which is the width divided by height
/// of the near or far planes.
/// \return The frustum's aspect ratio.
/// \sa SetAspectRatio
public: double AspectRatio() const;
/// \brief Set the aspect ratio, which is the width divided by height
/// of the near or far planes.
/// \param[in] _aspectRatio The frustum's aspect ratio.
/// \sa AspectRatio
public: void SetAspectRatio(const double _aspectRatio);
/// \brief Get a plane of the frustum.
/// \param[in] _plane The plane to return.
/// \return Plane of the frustum.
public: Planed Plane(const FrustumPlane _plane) const;
/// \brief Check if a box lies inside the pyramid frustum.
/// \param[in] _b Box to check.
/// \return True if the box is inside the pyramid frustum.
public: bool Contains(const Box &_b) const;
/// \brief Check if a point lies inside the pyramid frustum.
/// \param[in] _p Point to check.
/// \return True if the point is inside the pyramid frustum.
public: bool Contains(const Vector3d &_p) const;
/// \brief Get the pose of the frustum
/// \return Pose of the frustum
/// \sa SetPose
public: Pose3d Pose() const;
/// \brief Set the pose of the frustum
/// \param[in] _pose Pose of the frustum, top vertex.
/// \sa Pose
public: void SetPose(const Pose3d &_pose);
/// \brief Assignment operator. Set this frustum to the parameter.
/// \param[in] _b Frustum to copy
/// \return The new frustum.
public: Frustum &operator=(const Frustum &_f);
/// \brief Compute the planes of the frustum. This is called whenever
/// a property of the frustum is changed.
private: void ComputePlanes();
/// \internal
/// \brief Private data pointer
private: FrustumPrivate *dataPtr;
};
}
}
#endif

84
ign-math/include/ignition/math/FrustumPrivate.hh Normal file
View File

@ -0,0 +1,84 @@
/*
* 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_MATH_FRUSTUM_PRIVATE_HH_
#define IGNITION_MATH_FRUSTUM_PRIVATE_HH_
#include <array>
#include <utility>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Angle.hh>
#include <ignition/math/Plane.hh>
namespace ignition
{
namespace math
{
/// \internal
/// \brief Private data for the Frustum class
class FrustumPrivate
{
/// \brief Constructor
/// \param[in] _near Near distance. This is the distance from
/// the frustum's vertex to the closest plane
/// \param[in] _far Far distance. This is the distance from the
/// frustum's vertex to the farthest plane.
/// \param[in] _fov Field of view. The field of view is the
/// angle between the frustum's vertex and the edges of the near or far
/// plane. This value represents the horizontal angle.
/// \param[in] _aspectRatio The aspect ratio, which is the width divided
/// by height of the near or far planes.
/// \param[in] _pose Pose of the frustum, which is the vertex (top of
/// the pyramid).
public: FrustumPrivate(const double _near,
const double _far,
const math::Angle &_fov,
const double _aspectRatio,
const Pose3d &_pose)
: near(_near), far(_far), fov(_fov),
aspectRatio(_aspectRatio), pose(_pose)
{
}
/// \brief Near distance
public: double near;
/// \brief Far distance
public: double far;
/// \brief Field of view
public: math::Angle fov;
/// \brief Aspect ratio of the near and far planes. This is the
// width divided by the height.
public: double aspectRatio;
/// \brief Pose of the frustum
public: math::Pose3d pose;
/// \brief Each plane of the frustum.
/// \sa Frustum::FrustumPlane
public: std::array<Planed, 6> planes;
/// \brief Each corner of the frustum.
public: std::array<Vector3d, 8> points;
/// \brief each edge of the frustum.
public: std::array<std::pair<Vector3d, Vector3d>, 12> edges;
};
}
}
#endif

680
ign-math/include/ignition/math/Helpers.hh Normal file
View File

@ -0,0 +1,680 @@
/*
* Copyright (C) 2012-2014 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_FUNCTIONS_HH_
#define IGNITION_MATH_FUNCTIONS_HH_
#include <cmath>
#include <algorithm>
#include <limits>
#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 ignition::math::MAX_D
/// \brief Double min value. This value will be similar to 2.22507e-308
#define IGN_DBL_MIN ignition::math::MIN_D
/// \brief Double low value, equivalent to -IGN_DBL_MAX
#define IGN_DBL_LOW ignition::math::LOW_D
/// \brief Double positive infinite value
#define IGN_DBL_INF ignition::math::INF_D
/// \brief Float maximum value. This value will be similar to 3.40282e+38
#define IGN_FLT_MAX ignition::math::MAX_F
/// \brief Float minimum value. This value will be similar to 1.17549e-38
#define IGN_FLT_MIN ignition::math::MIN_F
/// \brief Float lowest value, equivalent to -IGN_FLT_MAX
#define IGN_FLT_LOW ignition::math::LOW_F
/// \brief Float positive infinite value
#define IGN_FLT_INF ignition::math::INF_F
/// \brief 16bit unsigned integer maximum value
#define IGN_UINT16_MAX ignition::math::MAX_UI16
/// \brief 16bit unsigned integer minimum value
#define IGN_UINT16_MIN ignition::math::MIN_UI16
/// \brief 16bit unsigned integer lowest value. This is equivalent to
/// IGN_UINT16_MIN, and is defined here for completeness.
#define IGN_UINT16_LOW ignition::math::LOW_UI16
/// \brief 16-bit unsigned integer positive infinite value
#define IGN_UINT16_INF ignition::math::INF_UI16
/// \brief 16bit integer maximum value
#define IGN_INT16_MAX ignition::math::MAX_I16
/// \brief 16bit integer minimum value
#define IGN_INT16_MIN ignition::math::MIN_I16
/// \brief 16bit integer lowest value. This is equivalent to IGN_INT16_MIN,
/// and is defined here for completeness.
#define IGN_INT16_LOW ignition::math::LOW_I16
/// \brief 16-bit integer positive infinite value
#define IGN_INT16_INF ignition::math::INF_I16
/// \brief 32bit unsigned integer maximum value
#define IGN_UINT32_MAX ignition::math::MAX_UI32
/// \brief 32bit unsigned integer minimum value
#define IGN_UINT32_MIN ignition::math::MIN_UI32
/// \brief 32bit unsigned integer lowest value. This is equivalent to
/// IGN_UINT32_MIN, and is defined here for completeness.
#define IGN_UINT32_LOW ignition::math::LOW_UI32
/// \brief 32-bit unsigned integer positive infinite value
#define IGN_UINT32_INF ignition::math::INF_UI32
/// \brief 32bit integer maximum value
#define IGN_INT32_MAX ignition::math::MAX_I32
/// \brief 32bit integer minimum value
#define IGN_INT32_MIN ignition::math::MIN_I32
/// \brief 32bit integer minimum value. This is equivalent to IGN_INT32_MIN,
/// and is defined here for completeness.
#define IGN_INT32_LOW ignition::math::LOW_I32
/// \brief 32-bit integer positive infinite value
#define IGN_INT32_INF ignition::math::INF_I32
/// \brief 64bit unsigned integer maximum value
#define IGN_UINT64_MAX ignition::math::MAX_UI64
/// \brief 64bit unsigned integer minimum value
#define IGN_UINT64_MIN ignition::math::MIN_UI64
/// \brief 64bit unsigned integer lowest value. This is equivalent to
/// IGN_UINT64_MIN, and is defined here for completeness.
#define IGN_UINT64_LOW ignition::math::LOW_UI64
/// \brief 64-bit unsigned integer positive infinite value
#define IGN_UINT64_INF ignition::math::INF_UI64
/// \brief 64bit integer maximum value
#define IGN_INT64_MAX ignition::math::MAX_I64
/// \brief 64bit integer minimum value
#define IGN_INT64_MIN ignition::math::MIN_I64
/// \brief 64bit integer lowest value. This is equivalent to IGN_INT64_MIN,
/// and is defined here for completeness.
#define IGN_INT64_LOW ignition::math::LOW_I64
/// \brief 64-bit integer positive infinite value
#define IGN_INT64_INF ignition::math::INF_I64
/// \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
#define IGN_SQRT2 1.41421356237309504880
#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
#endif
/// \brief Compute sphere volume
/// \param[in] _radius Sphere radius
#define IGN_SPHERE_VOLUME(_radius) (4.0*IGN_PI*std::pow(_radius, 3)/3.0)
/// \brief Compute cylinder volume
/// \param[in] _r Cylinder base radius
/// \param[in] _l Cylinder length
#define IGN_CYLINDER_VOLUME(_r, _l) (_l * IGN_PI * std::pow(_r, 2))
/// \brief Compute box volume
/// \param[in] _x X length
/// \param[in] _y Y length
/// \param[in] _z Z length
#define IGN_BOX_VOLUME(_x, _y, _z) (_x *_y * _z)
/// \brief Compute box volume from a vector
/// \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.
namespace math
{
/// \brief Double maximum value. This value will be similar to 1.79769e+308
static const double MAX_D = std::numeric_limits<double>::max();
/// \brief Double min value. This value will be similar to 2.22507e-308
static const double MIN_D = std::numeric_limits<double>::min();
/// \brief Double low value, equivalent to -MAX_D
static const double LOW_D = std::numeric_limits<double>::lowest();
/// \brief Double positive infinite value
static const double INF_D = std::numeric_limits<double>::infinity();
/// \brief Returns the representation of a quiet not a number (NAN)
static const double NAN_D = std::numeric_limits<double>::quiet_NaN();
/// \brief Float maximum value. This value will be similar to 3.40282e+38
static const float MAX_F = std::numeric_limits<float>::max();
/// \brief Float minimum value. This value will be similar to 1.17549e-38
static const float MIN_F = std::numeric_limits<float>::min();
/// \brief Float low value, equivalent to -MAX_F
static const float LOW_F = std::numeric_limits<float>::lowest();
/// \brief float positive infinite value
static const float INF_F = std::numeric_limits<float>::infinity();
/// \brief Returns the representation of a quiet not a number (NAN)
static const float NAN_F = std::numeric_limits<float>::quiet_NaN();
/// \brief 16bit unsigned integer maximum value
static const uint16_t MAX_UI16 = std::numeric_limits<uint16_t>::max();
/// \brief 16bit unsigned integer minimum value
static const uint16_t MIN_UI16 = 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.
static const uint16_t LOW_UI16 = std::numeric_limits<uint16_t>::lowest();
/// \brief 16-bit unsigned integer positive infinite value
static const uint16_t INF_UI16 = std::numeric_limits<uint16_t>::infinity();
/// \brief 16bit unsigned integer maximum value
static const int16_t MAX_I16 = std::numeric_limits<int16_t>::max();
/// \brief 16bit unsigned integer minimum value
static const int16_t MIN_I16 = std::numeric_limits<int16_t>::min();
/// \brief 16bit unsigned integer lowest value. This is equivalent to
/// IGN_INT16_MIN, and is defined here for completeness.
static const int16_t LOW_I16 = std::numeric_limits<int16_t>::lowest();
/// \brief 16-bit unsigned integer positive infinite value
static const int16_t INF_I16 = std::numeric_limits<int16_t>::infinity();
/// \brief 32bit unsigned integer maximum value
static const uint32_t MAX_UI32 = std::numeric_limits<uint32_t>::max();
/// \brief 32bit unsigned integer minimum value
static const uint32_t MIN_UI32 = std::numeric_limits<uint32_t>::min();
/// \brief 32bit unsigned integer lowest value. This is equivalent to
/// IGN_UINT32_MIN, and is defined here for completeness.
static const uint32_t LOW_UI32 = std::numeric_limits<uint32_t>::lowest();
/// \brief 32-bit unsigned integer positive infinite value
static const uint32_t INF_UI32 = std::numeric_limits<uint32_t>::infinity();
/// \brief 32bit unsigned integer maximum value
static const int32_t MAX_I32 = std::numeric_limits<int32_t>::max();
/// \brief 32bit unsigned integer minimum value
static const int32_t MIN_I32 = std::numeric_limits<int32_t>::min();
/// \brief 32bit unsigned integer lowest value. This is equivalent to
/// IGN_INT32_MIN, and is defined here for completeness.
static const int32_t LOW_I32 = std::numeric_limits<int32_t>::lowest();
/// \brief 32-bit unsigned integer positive infinite value
static const int32_t INF_I32 = std::numeric_limits<int32_t>::infinity();
/// \brief 64bit unsigned integer maximum value
static const uint64_t MAX_UI64 = std::numeric_limits<uint64_t>::max();
/// \brief 64bit unsigned integer minimum value
static const uint64_t MIN_UI64 = 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.
static const uint64_t LOW_UI64 = std::numeric_limits<uint64_t>::lowest();
/// \brief 64-bit unsigned integer positive infinite value
static const uint64_t INF_UI64 = std::numeric_limits<uint64_t>::infinity();
/// \brief 64bit unsigned integer maximum value
static const int64_t MAX_I64 = std::numeric_limits<int64_t>::max();
/// \brief 64bit unsigned integer minimum value
static const int64_t MIN_I64 = std::numeric_limits<int64_t>::min();
/// \brief 64bit unsigned integer lowest value. This is equivalent to
/// IGN_INT64_MIN, and is defined here for completeness.
static const int64_t LOW_I64 = std::numeric_limits<int64_t>::lowest();
/// \brief 64-bit unsigned integer positive infinite value
static const int64_t INF_I64 = std::numeric_limits<int64_t>::infinity();
/// \brief Returns the representation of a quiet not a number (NAN)
static const int NAN_I = std::numeric_limits<int>::quiet_NaN();
/// \brief Simple clamping function
/// \param[in] _v value
/// \param[in] _min minimum
/// \param[in] _max maximum
template<typename T>
inline T clamp(T _v, T _min, T _max)
{
return std::max(std::min(_v, _max), _min);
}
/// \brief check if a float is NaN
/// \param[in] _v the value
/// \return true if _v is not a number, false otherwise
inline bool isnan(float _v)
{
return (std::isnan)(_v);
}
/// \brief check if a double is NaN
/// \param[in] _v the value
/// \return true if _v is not a number, false otherwise
inline bool isnan(double _v)
{
return (std::isnan)(_v);
}
/// \brief Fix a nan value.
/// \param[in] _v Value to correct.
/// \return 0 if _v is NaN, _v otherwise.
inline float fixnan(float _v)
{
return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
}
/// \brief Fix a nan value.
/// \param[in] _v Value to correct.
/// \return 0 if _v is NaN, _v otherwise.
inline double fixnan(double _v)
{
return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
}
/// \brief Check if parameter is even.
/// \param[in] _v Value to check.
/// \return True if _v is even.
inline bool isEven(const int _v)
{
return !(_v % 2);
}
/// \brief Check if parameter is even.
/// \param[in] _v Value to check.
/// \return True if _v is even.
inline bool isEven(const unsigned int _v)
{
return !(_v % 2);
}
/// \brief Check if parameter is odd.
/// \param[in] _v Value to check.
/// \return True if _v is odd.
inline bool isOdd(const int _v)
{
return (_v % 2) != 0;
}
/// \brief Check if parameter is odd.
/// \param[in] _v Value to check.
/// \return True if _v is odd.
inline bool isOdd(const unsigned int _v)
{
return (_v % 2) != 0;
}
/// \brief get mean of vector of values
/// \param[in] _values the vector of values
/// \return the mean
template<typename T>
inline T mean(const std::vector<T> &_values)
{
T sum = 0;
for (unsigned int i = 0; i < _values.size(); ++i)
sum += _values[i];
return sum / _values.size();
}
/// \brief get variance of vector of values
/// \param[in] _values the vector of values
/// \return the squared deviation
template<typename T>
inline T variance(const std::vector<T> &_values)
{
T avg = mean<T>(_values);
T sum = 0;
for (unsigned int i = 0; i < _values.size(); ++i)
sum += (_values[i] - avg) * (_values[i] - avg);
return sum / _values.size();
}
/// \brief get the maximum value of vector of values
/// \param[in] _values the vector of values
/// \return maximum
template<typename T>
inline T max(const std::vector<T> &_values)
{
T max = std::numeric_limits<T>::min();
for (unsigned int i = 0; i < _values.size(); ++i)
if (_values[i] > max)
max = _values[i];
return max;
}
/// \brief get the minimum value of vector of values
/// \param[in] _values the vector of values
/// \return minimum
template<typename T>
inline T min(const std::vector<T> &_values)
{
T min = std::numeric_limits<T>::max();
for (unsigned int i = 0; i < _values.size(); ++i)
if (_values[i] < min)
min = _values[i];
return min;
}
/// \brief check if two values are equal, within a tolerance
/// \param[in] _a the first value
/// \param[in] _b the second value
/// \param[in] _epsilon the tolerance
template<typename T>
inline bool equal(const T &_a, const T &_b,
const T &_epsilon = T(1e-6))
{
IGN_FP_VOLATILE T diff = std::abs(_a - _b);
return diff <= _epsilon;
}
/// \brief inequality test, within a tolerance
/// \param[in] _a the first value
/// \param[in] _b the second value
/// \param[in] _epsilon the tolerance
template<typename T>
inline bool lessOrNearEqual(const T &_a, const T &_b,
const T &_epsilon = 1e-6)
{
return _a < _b + _epsilon;
}
/// \brief inequality test, within a tolerance
/// \param[in] _a the first value
/// \param[in] _b the second value
/// \param[in] _epsilon the tolerance
template<typename T>
inline bool greaterOrNearEqual(const T &_a, const T &_b,
const T &_epsilon = 1e-6)
{
return _a > _b - _epsilon;
}
/// \brief get value at a specified precision
/// \param[in] _a the number
/// \param[in] _precision the precision
/// \return the value for the specified precision
template<typename T>
inline T precision(const T &_a, const unsigned int &_precision)
{
auto p = std::pow(10, _precision);
return static_cast<T>(std::round(_a * p) / p);
}
/// \brief Sort two numbers, such that _a <= _b
/// \param[out] _a the first number
/// \param[out] _b the second number
template<typename T>
inline void sort2(T &_a, T &_b)
{
using std::swap;
if (_b < _a)
swap(_a, _b);
}
/// \brief Sort three numbers, such that _a <= _b <= _c
/// \param[out] _a the first number
/// \param[out] _b the second number
/// \param[out] _c the third number
template<typename T>
inline void sort3(T &_a, T &_b, T &_c)
{
// _a <= _b
sort2(_a, _b);
// _a <= _c, _b <= _c
sort2(_b, _c);
// _a <= _b <= _c
sort2(_a, _b);
}
/// \brief Is this a power of 2?
/// \param[in] _x the number
/// \return true if _x is a power of 2, false otherwise
inline bool isPowerOfTwo(unsigned int _x)
{
return ((_x != 0) && ((_x & (~_x + 1)) == _x));
}
/// \brief Get the smallest power of two that is greater or equal to
/// a given value
/// \param[in] _x the number
/// \return the same value if _x is already a power of two. Otherwise,
/// it returns the smallest power of two that is greater than _x
inline unsigned int roundUpPowerOfTwo(unsigned int _x)
{
if (_x == 0)
return 1;
if (isPowerOfTwo(_x))
return _x;
while (_x & (_x - 1))
_x = _x & (_x - 1);
_x = _x << 1;
return _x;
}
/// \brief parse string into an integer
/// \param[in] _input the string
/// \return an integer, 0 or 0 and a message in the error stream
inline int parseInt(const std::string &_input)
{
const char *p = _input.c_str();
if (!*p || *p == '?')
return NAN_I;
int s = 1;
while (*p == ' ')
p++;
if (*p == '-')
{
s = -1;
p++;
}
int acc = 0;
while (*p >= '0' && *p <= '9')
acc = acc * 10 + *p++ - '0';
if (*p)
{
std::cerr << "Invalid int numeric format[" << _input << "]\n";
return NAN_I;
}
return s * acc;
}
/// \brief parse string into float
/// \param _input the string
/// \return a floating point number (can be NaN) or 0 with a message in the
/// error stream
inline double parseFloat(const std::string &_input)
{
const char *p = _input.c_str();
if (!*p || *p == '?')
return NAN_D;
int s = 1;
while (*p == ' ')
p++;
if (*p == '-')
{
s = -1;
p++;
}
double acc = 0;
while (*p >= '0' && *p <= '9')
acc = acc * 10 + *p++ - '0';
if (*p == '.')
{
double k = 0.1;
p++;
while (*p >= '0' && *p <= '9')
{
acc += (*p++ - '0') * k;
k *= 0.1;
}
}
if (*p == 'e')
{
int es = 1;
int f = 0;
p++;
if (*p == '-')
{
es = -1;
p++;
}
else if (*p == '+')
{
es = 1;
p++;
}
while (*p >= '0' && *p <= '9')
f = f * 10 + *p++ - '0';
acc *= pow(10, f*es);
}
if (*p)
{
std::cerr << "Invalid double numeric format[" << _input << "]\n";
return NAN_D;
}
return s * acc;
}
// 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
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

48
ign-math/include/ignition/math/IndexException.hh Normal file
View File

@ -0,0 +1,48 @@
/*
* Copyright (C) 2012-2014 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_INDEX_EXCEPTION_HH_
#define IGNITION_MATH_INDEX_EXCEPTION_HH_
#include <stdexcept>
#include <ignition/math/Helpers.hh>
// Ignore warning C4275. It is okay to ignore this according to microsoft:
// https://msdn.microsoft.com/en-us/library/3tdb471s.aspx
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4275)
#endif
namespace ignition
{
namespace math
{
/// \class IndexException IndexException.hh ignition/math/IndexException.hh
/// \brief Exception that is thrown when an out-of-bounds index is
/// encountered.
class IGNITION_VISIBLE IndexException : public std::runtime_error
{
public: IndexException();
};
}
}
#ifdef _MSC_VER
#pragma warning(pop)
#endif
#endif

244
ign-math/include/ignition/math/Inertial.hh Normal file
View File

@ -0,0 +1,244 @@
/*
* 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 Set the inertial pose rotation without affecting the
/// MOI in the base coordinate frame.
/// \param[in] _q New rotation for inertial pose.
/// \return True if the MassMatrix3 is valid.
public: bool SetInertialRotation(const Quaternion<T> &_q)
{
auto moi = this->MOI();
this->pose.Rot() = _q;
auto R = Matrix3<T>(_q);
return this->massMatrix.MOI(R.Transposed() * moi * R);
}
/// \brief Set the MassMatrix rotation (eigenvectors of inertia matrix)
/// without affecting the MOI in the base coordinate frame.
/// Note that symmetries in inertia matrix may prevent the output of
/// MassMatrix3::PrincipalAxesOffset to match this function's input _q,
/// but it is guaranteed that the MOI in the base frame will not change.
/// A negative value of _tol (such as -1e-6) can be passed to ensure
/// that diagonal values are always sorted.
/// \param[in] _q New rotation.
/// \param[in] _tol Relative tolerance given by absolute value
/// of _tol. This is passed to the MassMatrix3
/// PrincipalMoments and PrincipalAxesOffset functions.
/// \return True if the MassMatrix3 is valid.
public: bool SetMassMatrixRotation(const Quaternion<T> &_q,
const T _tol = 1e-6)
{
this->pose.Rot() *= this->MassMatrix().PrincipalAxesOffset(_tol) *
_q.Inverse();
const auto moments = this->MassMatrix().PrincipalMoments(_tol);
const auto diag = Matrix3<T>(
moments[0], 0, 0,
0, moments[1], 0,
0, 0, moments[2]);
const auto R = Matrix3<T>(_q);
return this->massMatrix.MOI(R * diag * 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

86
ign-math/include/ignition/math/Kmeans.hh Normal file
View File

@ -0,0 +1,86 @@
/*
* Copyright (C) 2014-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_MATH_KMEANS_HH_
#define IGNITION_MATH_KMEANS_HH_
#include <vector>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Helpers.hh>
namespace ignition
{
namespace math
{
// Forward declare private data
class KmeansPrivate;
/// \class Kmeans Kmeans.hh math/gzmath.hh
/// \brief K-Means clustering algorithm. Given a set of observations,
/// k-means partitions the observations into k sets so as to minimize the
/// within-cluster sum of squares.
/// Description based on http://en.wikipedia.org/wiki/K-means_clustering.
class IGNITION_VISIBLE Kmeans
{
/// \brief constructor
/// \param[in] _obs Set of observations to cluster.
// cppcheck-suppress noExplicitConstructor
public: Kmeans(const std::vector<Vector3d> &_obs);
/// \brief Destructor.
public: virtual ~Kmeans();
/// \brief Get the observations to cluster.
/// \return The vector of observations.
public: std::vector<Vector3d> Observations() const;
/// \brief Set the observations to cluster.
/// \param[in] _obs The new vector of observations.
/// \return True if the vector is not empty or false otherwise.
public: bool Observations(const std::vector<Vector3d> &_obs);
/// \brief Add observations to the cluster.
/// \param[in] _obs Vector of observations.
/// \return True if the _obs vector is not empty or false otherwise.
public: bool AppendObservations(const std::vector<Vector3d> &_obs);
/// \brief Executes the k-means algorithm.
/// \param[in] _k Number of partitions to cluster.
/// \param[out] _centroids Vector of centroids. Each element contains the
/// centroid of one cluster.
/// \param[out] _labels Vector of labels. The size of this vector is
/// equals to the number of observations. Each element represents the
/// cluster to which observation belongs.
/// \return True when the operation succeed or false otherwise. The
/// operation will fail if the number of observations is not positive,
/// if the number of clusters is non positive, or if the number of
/// clusters if greater than the number of observations.
public: bool Cluster(int _k,
std::vector<Vector3d> &_centroids,
std::vector<unsigned int> &_labels);
/// \brief Given an observation, it returns the closest centroid to it.
/// \param[in] _p Point to check.
/// \return The index of the closest centroid to the point _p.
private: unsigned int ClosestCentroid(const Vector3d &_p) const;
/// \brief Private data pointer
private: KmeansPrivate *dataPtr;
};
}
}
#endif

51
ign-math/include/ignition/math/KmeansPrivate.hh Normal file
View File

@ -0,0 +1,51 @@
/*
* 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_MATH_KMEANS_PRIVATE_HH_
#define IGNITION_MATH_KMEANS_PRIVATE_HH_
#include <vector>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Helpers.hh>
namespace ignition
{
namespace math
{
/// \internal
/// \brief Private data for Kmeans class
class KmeansPrivate
{
/// \brief Observations.
public: std::vector<Vector3d> obs;
/// \brief Centroids.
public: std::vector<Vector3d> centroids;
/// \brief Each element stores the cluster to which observation i belongs.
public: std::vector<unsigned int> labels;
/// \brief Used to calculate the centroid of each partition.
public: std::vector<Vector3d> sums;
/// \brief Counts the number of observations contained in each partition.
public: std::vector<unsigned int> counters;
};
}
}
#endif

319
ign-math/include/ignition/math/Line2.hh Normal file
View File

@ -0,0 +1,319 @@
/*
* Copyright (C) 2014 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_LINE2_HH_
#define IGNITION_MATH_LINE2_HH_
#include <algorithm>
#include <ignition/math/Vector2.hh>
#include <ignition/math/IndexException.hh>
namespace ignition
{
namespace math
{
/// \class Line2 Line2.hh ignition/math/Line2.hh
/// \brief A two dimensional line segment. The line is defined by a
/// start and end point.
template<typename T>
class Line2
{
/// \brief Constructor.
/// \param[in] _ptA Start point of the line segment
/// \param[in] _ptB End point of the line segment
public: Line2(const math::Vector2<T> &_ptA, const math::Vector2<T> &_ptB)
{
this->Set(_ptA, _ptB);
}
/// \brief Constructor.
/// \param[in] _x1 X coordinate of the start point.
/// \param[in] _y1 Y coordinate of the start point.
/// \param[in] _x2 X coordinate of the end point.
/// \param[in] _y2 Y coordinate of the end point.
public: Line2(double _x1, double _y1, double _x2, double _y2)
{
this->Set(_x1, _y1, _x2, _y2);
}
/// \brief Set the start and end point of the line segment
/// \param[in] _ptA Start point of the line segment
/// \param[in] _ptB End point of the line segment
public: void Set(const math::Vector2<T> &_ptA,
const math::Vector2<T> &_ptB)
{
this->pts[0] = _ptA;
this->pts[1] = _ptB;
}
/// \brief Set the start and end point of the line segment
/// \param[in] _x1 X coordinate of the start point.
/// \param[in] _y1 Y coordinate of the start point.
/// \param[in] _x2 X coordinate of the end point.
/// \param[in] _y2 Y coordinate of the end point.
public: void Set(double _x1, double _y1, double _x2, double _y2)
{
this->pts[0].Set(_x1, _y1);
this->pts[1].Set(_x2, _y2);
}
/// \brief Return the cross product of this line and the given line.
/// Give 'a' as this line and 'b' as given line, the equation is:
/// (a.start.x - a.end.x) * (b.start.y - b.end.y) -
/// (a.start.y - a.end.y) * (b.start.x - b.end.x)
/// \param[in] _line Line for the cross product computation.
/// \return Return the cross product of this line and the given line.
public: double CrossProduct(const Line2<T> &_line) const
{
return (this->pts[0].X() - this->pts[1].X()) *
(_line[0].Y() -_line[1].Y()) -
(this->pts[0].Y() - this->pts[1].Y()) *
(_line[0].X() - _line[1].X());
}
/// \brief Return the cross product of this line and the given point.
/// Given 'a' and 'b' as the start and end points, the equation is:
// (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y)
/// \param[in] _pt Point for the cross product computation.
/// \return Return the cross product of this line and the given point.
public: double CrossProduct(const Vector2<T> &_pt) const
{
return (_pt.Y() - this->pts[0].Y()) *
(this->pts[1].X() - this->pts[0].X()) -
(_pt.X() - this->pts[0].X()) *
(this->pts[1].Y() - this->pts[0].Y());
}
/// \brief Check if the given point is collinear with this line.
/// \param[in] _pt The point to check.
/// \param[in] _epsilon The error bounds within which the collinear
/// check will return true.
/// \return Return true if the point is collinear with this line, false
/// otherwise.
public: bool Collinear(const math::Vector2<T> &_pt,
double _epsilon = 1e-6) const
{
return math::equal(this->CrossProduct(_pt),
static_cast<T>(0), _epsilon);
}
/// \brief Check if the given line is parallel with this line.
/// \param[in] _line The line to check.
/// \param[in] _epsilon The error bounds within which the parallel
/// check will return true.
/// \return Return true if the line is parallel with this line, false
/// otherwise. Return true if either line is a point (line with zero
/// length).
public: bool Parallel(const math::Line2<T> &_line,
double _epsilon = 1e-6) const
{
return math::equal(this->CrossProduct(_line),
static_cast<T>(0), _epsilon);
}
/// \brief Check if the given line is collinear with this line. This
/// is the AND of Parallel and Intersect.
/// \param[in] _line The line to check.
/// \param[in] _epsilon The error bounds within which the collinear
/// check will return true.
/// \return Return true if the line is collinear with this line, false
/// otherwise.
public: bool Collinear(const math::Line2<T> &_line,
double _epsilon = 1e-6) const
{
return this->Parallel(_line, _epsilon) &&
this->Intersect(_line, _epsilon);
}
/// \brief Return whether the given point is on this line segment.
/// \param[in] _pt Point to check.
/// \param[in] _epsilon The error bounds within which the OnSegment
/// check will return true.
/// \return True if the point is on the segement.
public: bool OnSegment(const math::Vector2<T> &_pt,
double _epsilon = 1e-6) const
{
return this->Collinear(_pt, _epsilon) && this->Within(_pt, _epsilon);
}
/// \brief Check if the given point is between the start and end
/// points of the line segment. This does not imply that the point is
/// on the 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::Vector2<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;
}
/// \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 Line2<T> &_line,
double _epsilon = 1e-6) const
{
static math::Vector2<T> ignore;
return this->Intersect(_line, ignore, _epsilon);
}
/// \brief Check if this line intersects the given line segment. The
/// point of intersection is returned in the _result 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 Line2<T> &_line, math::Vector2<T> &_pt,
double _epsilon = 1e-6) const
{
double d = this->CrossProduct(_line);
// d is zero if the two line are collinear. Must check special
// cases.
if (math::equal(d, 0.0, _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;
}
// Other wise return false.
else
return false;
}
_pt.X((_line[0].X() - _line[1].X()) *
(this->pts[0].X() * this->pts[1].Y() -
this->pts[0].Y() * this->pts[1].X()) -
(this->pts[0].X() - this->pts[1].X()) *
(_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
_pt.Y((_line[0].Y() - _line[1].Y()) *
(this->pts[0].X() * this->pts[1].Y() -
this->pts[0].Y() * this->pts[1].X()) -
(this->pts[0].Y() - this->pts[1].Y()) *
(_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
_pt /= d;
if (_pt.X() < std::min(this->pts[0].X(), this->pts[1].X()) ||
_pt.X() > std::max(this->pts[0].X(), this->pts[1].X()) ||
_pt.X() < std::min(_line[0].X(), _line[1].X()) ||
_pt.X() > std::max(_line[0].X(), _line[1].X()))
{
return false;
}
if (_pt.Y() < std::min(this->pts[0].Y(), this->pts[1].Y()) ||
_pt.Y() > std::max(this->pts[0].Y(), this->pts[1].Y()) ||
_pt.Y() < std::min(_line[0].Y(), _line[1].Y()) ||
_pt.Y() > std::max(_line[0].Y(), _line[1].Y()))
{
return false;
}
return true;
}
/// \brief Get the length of the line
/// \return The length of the line.
public: T Length() const
{
return sqrt((this->pts[0].X() - this->pts[1].X()) *
(this->pts[0].X() - this->pts[1].X()) +
(this->pts[0].Y() - this->pts[1].Y()) *
(this->pts[0].Y() - this->pts[1].Y()));
}
/// \brief Get the slope of the line
/// \return The slope of the line, NAN_D if the line is vertical.
public: double Slope() const
{
if (math::equal(this->pts[1].X(), this->pts[0].X()))
return NAN_D;
return (this->pts[1].Y() - this->pts[0].Y()) /
static_cast<double>(this->pts[1].X() - this->pts[0].X());
}
/// \brief Equality operator.
/// \param[in] _line Line to compare for equality.
/// \return True if the given line is equal to this line
public: bool operator==(const Line2<T> &_line) const
{
return this->pts[0] == _line[0] && this->pts[1] == _line[1];
}
/// \brief Inequality operator.
/// \param[in] _line Line to compare for inequality.
/// \return True if the given line is not to this line
public: bool operator!=(const Line2<T> &_line) const
{
return !(*this == _line);
}
/// \brief Get the start or end point.
/// \param[in] _index 0 = start point, 1 = end point.
/// \throws IndexException if _index is > 1.
public: math::Vector2<T> operator[](size_t _index) const
{
if (_index > 1)
throw IndexException();
return this->pts[_index];
}
/// \brief Stream extraction operator
/// \param[in] _out output stream
/// \param[in] _pt Line2 to output
/// \return The stream
/// \throws N/A.
public: friend std::ostream &operator<<(
std::ostream &_out, const Line2<T> &_line)
{
_out << _line[0] << " " << _line[1];
return _out;
}
private: math::Vector2<T> pts[2];
};
typedef Line2<int> Line2i;
typedef Line2<double> Line2d;
typedef Line2<float> Line2f;
}
}
#endif

391
ign-math/include/ignition/math/Line3.hh Normal file
View File

@ -0,0 +1,391 @@
/*
* 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_MATH_LINE3_HH_
#define IGNITION_MATH_LINE3_HH_
#include <algorithm>
#include <ignition/math/Vector3.hh>
#include <ignition/math/IndexException.hh>
namespace ignition
{
namespace math
{
/// \class Line3 Line3.hh ignition/math/Line3.hh
/// \brief A three dimensional line segment. The line is defined by a
/// start and end point.
template<typename T>
class Line3
{
/// \brief Line Constructor
public: Line3() = default;
/// \brief Copy constructor
/// \param[in] _line a line object
public: Line3(const Line3<T> &_line)
{
this->pts[0] = _line[0];
this->pts[1] = _line[1];
}
/// \brief Constructor.
/// \param[in] _ptA Start point of the line segment
/// \param[in] _ptB End point of the line segment
public: Line3(const math::Vector3<T> &_ptA, const math::Vector3<T> &_ptB)
{
this->Set(_ptA, _ptB);
}
/// \brief 2D Constructor where Z coordinates are 0
/// \param[in] _x1 X coordinate of the start point.
/// \param[in] _y1 Y coordinate of the start point.
/// \param[in] _x2 X coordinate of the end point.
/// \param[in] _y2 Y coordinate of the end point.
public: Line3(const double _x1, const double _y1,
const double _x2, const double _y2)
{
this->Set(_x1, _y1, _x2, _y2);
}
/// \brief Constructor.
/// \param[in] _x1 X coordinate of the start point.
/// \param[in] _y1 Y coordinate of the start point.
/// \param[in] _z1 Z coordinate of the start point.
/// \param[in] _x2 X coordinate of the end point.
/// \param[in] _y2 Y coordinate of the end point.
/// \param[in] _z2 Z coordinate of the end point.
public: Line3(const double _x1, const double _y1,
const double _z1, const double _x2,
const double _y2, const double _z2)
{
this->Set(_x1, _y1, _z1, _x2, _y2, _z2);
}
/// \brief Set the start and end point of the line segment
/// \param[in] _ptA Start point of the line segment
/// \param[in] _ptB End point of the line segment
public: void Set(const math::Vector3<T> &_ptA,
const math::Vector3<T> &_ptB)
{
this->pts[0] = _ptA;
this->pts[1] = _ptB;
}
/// \brief Set the start point of the line segment
/// \param[in] _ptA Start point of the line segment
public: void SetA(const math::Vector3<T> &_ptA)
{
this->pts[0] = _ptA;
}
/// \brief Set the end point of the line segment
/// \param[in] _ptB End point of the line segment
public: void SetB(const math::Vector3<T> &_ptB)
{
this->pts[1] = _ptB;
}
/// \brief Set the start and end point of the line segment, assuming that
/// both points have the same height.
/// \param[in] _x1 X coordinate of the start point.
/// \param[in] _y1 Y coordinate of the start point.
/// \param[in] _x2 X coordinate of the end point.
/// \param[in] _y2 Y coordinate of the end point.
/// \param[in] _z Z coordinate of both points,
/// by default _z is set to 0.
public: void Set(const double _x1, const double _y1,
const double _x2, const double _y2,
const double _z = 0)
{
this->pts[0].Set(_x1, _y1, _z);
this->pts[1].Set(_x2, _y2, _z);
}
/// \brief Set the start and end point of the line segment
/// \param[in] _x1 X coordinate of the start point.
/// \param[in] _y1 Y coordinate of the start point.
/// \param[in] _z1 Z coordinate of the start point.
/// \param[in] _x2 X coordinate of the end point.
/// \param[in] _y2 Y coordinate of the end point.
/// \param[in] _z2 Z coordinate of the end point.
public: void Set(const double _x1, const double _y1,
const double _z1, const double _x2,
const double _y2, const double _z2)
{
this->pts[0].Set(_x1, _y1, _z1);
this->pts[1].Set(_x2, _y2, _z2);
}
/// \brief Get the direction of the line
/// \return The direction vector
public: math::Vector3<T> Direction() const
{
return (this->pts[1] - this->pts[0]).Normalize();
}
/// \brief Get the length of the line
/// \return The length of the line.
public: T Length() const
{
return this->pts[0].Distance(this->pts[1]);
}
/// \brief 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
public: bool operator==(const Line3<T> &_line) const
{
return this->pts[0] == _line[0] && this->pts[1] == _line[1];
}
/// \brief Inequality operator.
/// \param[in] _line Line to compare for inequality.
/// \return True if the given line is not to this line
public: bool operator!=(const Line3<T> &_line) const
{
return !(*this == _line);
}
/// \brief Get the start or end point.
/// \param[in] _index 0 = start point, 1 = end point.
/// \throws IndexException if _index is > 1.
public: math::Vector3<T> operator[](const size_t _index) const
{
if (_index > 1)
throw IndexException();
return this->pts[_index];
}
/// \brief Stream extraction operator
/// \param[in] _out output stream
/// \param[in] _line Line3 to output
/// \return The stream
public: friend std::ostream &operator<<(
std::ostream &_out, const Line3<T> &_line)
{
_out << _line[0] << " " << _line[1];
return _out;
}
/// \brief Assignment operator
/// \param[in] _line a new value
/// \return this
public: Line3 &operator=(const Line3<T> &_line)
{
this->pts[0] = _line[0];
this->pts[1] = _line[1];
return *this;
}
/// \brief Vector for storing the start and end points of the line
private: math::Vector3<T> pts[2];
};
typedef Line3<int> Line3i;
typedef Line3<double> Line3d;
typedef Line3<float> Line3f;
}
}
#endif

904
ign-math/include/ignition/math/MassMatrix3.hh Normal file
View File

@ -0,0 +1,904 @@
/*
* 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.
* 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_MASSMATRIX3_HH_
#define IGNITION_MATH_MASSMATRIX3_HH_
#include <algorithm>
#include <string>
#include <vector>
#include "ignition/math/Helpers.hh"
#include "ignition/math/Quaternion.hh"
#include "ignition/math/Vector2.hh"
#include "ignition/math/Vector3.hh"
#include "ignition/math/Matrix3.hh"
namespace ignition
{
namespace math
{
/// \class MassMatrix3 MassMatrix3.hh ignition/math/MassMatrix3.hh
/// \brief A class for inertial information about a rigid body
/// consisting of the scalar mass and a 3x3 symmetric moment
/// of inertia matrix stored as two Vector3's.
template<typename T>
class MassMatrix3
{
/// \brief Default Constructor
public: MassMatrix3() : mass(0)
{}
/// \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
public: MassMatrix3(const T &_mass,
const Vector3<T> &_ixxyyzz,
const Vector3<T> &_ixyxzyz)
: mass(_mass), Ixxyyzz(_ixxyyzz), Ixyxzyz(_ixyxzyz)
{}
/// \brief Copy constructor.
/// \param[in] _massMatrix MassMatrix3 element to copy
public: MassMatrix3(const MassMatrix3<T> &_m)
: mass(_m.Mass()), Ixxyyzz(_m.DiagonalMoments()),
Ixyxzyz(_m.OffDiagonalMoments())
{}
/// \brief Destructor.
public: virtual ~MassMatrix3() {}
/// \brief Set the mass.
/// \param[in] _m New mass value.
/// \return True if the MassMatrix3 is valid.
public: bool Mass(const T &_m)
{
this->mass = _m;
return this->IsValid();
}
/// \brief Get the mass
/// \return The mass value
public: T Mass() const
{
return this->mass;
}
/// \brief Set the moment of inertia matrix.
/// \param[in] _ixx X second moment of inertia (MOI) about x axis.
/// \param[in] _iyy Y second moment of inertia about y axis.
/// \param[in] _izz Z second moment of inertia about z axis.
/// \param[in] _ixy XY inertia.
/// \param[in] _ixz XZ inertia.
/// \param[in] _iyz YZ inertia.
/// \return True if the MassMatrix3 is valid.
public: bool InertiaMatrix(const T &_ixx, const T &_iyy, const T &_izz,
const T &_ixy, const T &_ixz, const T &_iyz)
{
this->Ixxyyzz.Set(_ixx, _iyy, _izz);
this->Ixyxzyz.Set(_ixy, _ixz, _iyz);
return this->IsValid();
}
/// \brief Get the diagonal moments of inertia (Ixx, Iyy, Izz).
/// \return The diagonal moments.
public: Vector3<T> DiagonalMoments() const
{
return this->Ixxyyzz;
}
/// \brief Get the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
/// \return The off-diagonal moments of inertia.
public: Vector3<T> OffDiagonalMoments() const
{
return this->Ixyxzyz;
}
/// \brief Set the diagonal moments of inertia (Ixx, Iyy, Izz).
/// \param[in] _ixxyyzz diagonal moments of inertia
/// \return True if the MassMatrix3 is valid.
public: bool DiagonalMoments(const Vector3<T> &_ixxyyzz)
{
this->Ixxyyzz = _ixxyyzz;
return this->IsValid();
}
/// \brief Set the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
/// \param[in] _ixyxzyz off-diagonal moments of inertia
/// \return True if the MassMatrix3 is valid.
public: bool OffDiagonalMoments(const Vector3<T> &_ixyxzyz)
{
this->Ixyxzyz = _ixyxzyz;
return this->IsValid();
}
/// \brief Get IXX
/// \return IXX value
public: T IXX() const
{
return this->Ixxyyzz[0];
}
/// \brief Get IYY
/// \return IYY value
public: T IYY() const
{
return this->Ixxyyzz[1];
}
/// \brief Get IZZ
/// \return IZZ value
public: T IZZ() const
{
return this->Ixxyyzz[2];
}
/// \brief Get IXY
/// \return IXY value
public: T IXY() const
{
return this->Ixyxzyz[0];
}
/// \brief Get IXZ
/// \return IXZ value
public: T IXZ() const
{
return this->Ixyxzyz[1];
}
/// \brief Get IYZ
/// \return IYZ value
public: T IYZ() const
{
return this->Ixyxzyz[2];
}
/// \brief Set IXX
/// \param[in] _v IXX value
/// \return True if the MassMatrix3 is valid.
public: bool IXX(const T &_v)
{
this->Ixxyyzz.X(_v);
return this->IsValid();
}
/// \brief Set IYY
/// \param[in] _v IYY value
/// \return True if the MassMatrix3 is valid.
public: bool IYY(const T &_v)
{
this->Ixxyyzz.Y(_v);
return this->IsValid();
}
/// \brief Set IZZ
/// \param[in] _v IZZ value
/// \return True if the MassMatrix3 is valid.
public: bool IZZ(const T &_v)
{
this->Ixxyyzz.Z(_v);
return this->IsValid();
}
/// \brief Set IXY
/// \param[in] _v IXY value
/// \return True if the MassMatrix3 is valid.
public: bool IXY(const T &_v)
{
this->Ixyxzyz.X(_v);
return this->IsValid();
}
/// \brief Set IXZ
/// \param[in] _v IXZ value
/// \return True if the MassMatrix3 is valid.
public: bool IXZ(const T &_v)
{
this->Ixyxzyz.Y(_v);
return this->IsValid();
}
/// \brief Set IYZ
/// \param[in] _v IYZ value
/// \return True if the MassMatrix3 is valid.
public: bool IYZ(const T &_v)
{
this->Ixyxzyz.Z(_v);
return this->IsValid();
}
/// \brief returns Moments of Inertia as a Matrix3
/// \return Moments of Inertia as a Matrix3
public: Matrix3<T> MOI() const
{
return Matrix3<T>(
this->Ixxyyzz[0], this->Ixyxzyz[0], this->Ixyxzyz[1],
this->Ixyxzyz[0], this->Ixxyyzz[1], this->Ixyxzyz[2],
this->Ixyxzyz[1], this->Ixyxzyz[2], this->Ixxyyzz[2]);
}
/// \brief Sets Moments of Inertia (MOI) from a Matrix3.
/// Symmetric component of input matrix is used by averaging
/// off-axis terms.
/// \param[in] Moments of Inertia as a Matrix3
/// \return True if the MassMatrix3 is valid.
public: bool MOI(const Matrix3<T> &_moi)
{
this->Ixxyyzz.Set(_moi(0, 0), _moi(1, 1), _moi(2, 2));
this->Ixyxzyz.Set(
0.5*(_moi(0, 1) + _moi(1, 0)),
0.5*(_moi(0, 2) + _moi(2, 0)),
0.5*(_moi(1, 2) + _moi(2, 1)));
return this->IsValid();
}
/// \brief Equal operator.
/// \param[in] _massMatrix MassMatrix3 to copy.
/// \return Reference to this object.
public: MassMatrix3 &operator=(const MassMatrix3<T> &_massMatrix)
{
this->mass = _massMatrix.Mass();
this->Ixxyyzz = _massMatrix.DiagonalMoments();
this->Ixyxzyz = _massMatrix.OffDiagonalMoments();
return *this;
}
/// \brief Equality comparison operator.
/// \param[in] _m MassMatrix3 to copy.
/// \return true if each component is equal within a default tolerance,
/// false otherwise
public: bool operator==(const MassMatrix3<T> &_m) const
{
return equal<T>(this->mass, _m.Mass()) &&
(this->Ixxyyzz == _m.DiagonalMoments()) &&
(this->Ixyxzyz == _m.OffDiagonalMoments());
}
/// \brief Inequality test operator
/// \param[in] _m MassMatrix3<T> to test
/// \return True if not equal (using the default tolerance of 1e-6)
public: bool operator!=(const MassMatrix3<T> &_m) const
{
return !(*this == _m);
}
/// \brief Verify that inertia values are positive definite
/// \return True if mass is positive and moment of inertia matrix
/// is positive definite.
public: bool IsPositive() const
{
// Check if mass and determinants of all upper left submatrices
// of moment of inertia matrix are positive
return (this->mass > 0) &&
(this->IXX() > 0) &&
(this->IXX()*this->IYY() - std::pow(this->IXY(), 2) > 0) &&
(this->MOI().Determinant() > 0);
}
/// \brief Verify that inertia values are positive definite
/// and satisfy the triangle inequality.
/// \return True if IsPositive and moment of inertia satisfies
/// the triangle inequality.
public: bool IsValid() const
{
return this->IsPositive() && ValidMoments(this->PrincipalMoments());
}
/// \brief Verify that principal moments are positive
/// and satisfy the triangle inequality.
/// \param[in] _moments Principal moments of inertia.
/// \return True if moments of inertia are positive
/// and satisfy the triangle inequality.
public: static bool ValidMoments(const Vector3<T> &_moments)
{
return _moments[0] > 0 &&
_moments[1] > 0 &&
_moments[2] > 0 &&
_moments[0] + _moments[1] > _moments[2] &&
_moments[1] + _moments[2] > _moments[0] &&
_moments[2] + _moments[0] > _moments[1];
}
/// \brief Compute principal moments of inertia,
/// which are the eigenvalues of the moment of inertia matrix.
/// \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
{
// Compute tolerance relative to maximum value of inertia diagonal
T tol = _tol * this->Ixxyyzz.Max();
if (this->Ixyxzyz.Equal(Vector3<T>::Zero, tol))
{
// Matrix is already diagonalized, return diagonal moments
return this->Ixxyyzz;
}
// 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
Vector3<T> Id(this->Ixxyyzz);
Vector3<T> Ip(this->Ixyxzyz);
// b = Ixx + Iyy + Izz
T b = Id.Sum();
// c = Ixx*Iyy - Ixy^2 + Ixx*Izz - Ixz^2 + Iyy*Izz - Iyz^2
T c = Id[0]*Id[1] - std::pow(Ip[0], 2)
+ Id[0]*Id[2] - std::pow(Ip[1], 2)
+ Id[1]*Id[2] - std::pow(Ip[2], 2);
// d = Ixx*Iyz^2 + Iyy*Ixz^2 + Izz*Ixy^2 - Ixx*Iyy*Izz - 2*Ixy*Ixz*Iyz
T d = Id[0]*std::pow(Ip[2], 2)
+ Id[1]*std::pow(Ip[1], 2)
+ Id[2]*std::pow(Ip[0], 2)
- Id[0]*Id[1]*Id[2]
- 2*Ip[0]*Ip[1]*Ip[2];
// p = b^2 - 3c
T p = std::pow(b, 2) - 3*c;
// At this point, it is important to check that p is not close
// to zero, since its inverse is used to compute delta.
// In equation 4.7, p is expressed as a sum of squares
// that is only zero if the matrix is diagonal
// with identical principal moments.
// This check has no test coverage, since this function returns
// immediately if a diagonal matrix is detected.
if (p < std::pow(tol, 2))
return b / 3.0 * Vector3<T>::One;
// q = 2b^3 - 9bc - 27d
T q = 2*std::pow(b, 3) - 9*b*c - 27*d;
// delta = acos(q / (2 * p^(1.5)))
// additionally clamp the argument to [-1,1]
T delta = acos(clamp<T>(0.5 * q / std::pow(p, 1.5), -1, 1));
// 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*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 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) ||
(math::equal<T>(moments[0], moments[1], std::abs(tol)) &&
math::equal<T>(moments[0], moments[2], std::abs(tol))))
{
// matrix is already aligned with principal axes
// or 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)
/// These Moments of Inertia are specified in the local frame.
/// Where Ixxyyzz.x is Ixx, Ixxyyzz.y is Iyy and Ixxyyzz.z is Izz.
private: Vector3<T> Ixxyyzz;
/// \brief Product moments of inertia. Default is (0.0 0.0 0.0)
/// These MOI off-diagonals are specified in the local frame.
/// Where Ixyxzyz.x is Ixy, Ixyxzyz.y is Ixz and Ixyxzyz.z is Iyz.
private: Vector3<T> Ixyxzyz;
};
typedef MassMatrix3<double> MassMatrix3d;
typedef MassMatrix3<float> MassMatrix3f;
}
}
#endif

134
ign-math/include/ignition/math/MassMatrix3.ipynb Normal file
View File

@ -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": {}
}
]
}

528
ign-math/include/ignition/math/Matrix3.hh Normal file
View File

@ -0,0 +1,528 @@
/*
* Copyright (C) 2012-2014 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_MATRIX3_HH_
#define IGNITION_MATH_MATRIX3_HH_
#include <algorithm>
#include <cstring>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Quaternion.hh>
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>
class Matrix3
{
/// \brief Identity matrix
public: static const Matrix3<T> Identity;
/// \brief Zero matrix
public: static const Matrix3<T> Zero;
/// \brief Constructor
public: Matrix3()
{
std::memset(this->data, 0, sizeof(this->data[0][0])*9);
}
/// \brief Copy constructor
/// \param _m Matrix to copy
public: Matrix3(const Matrix3<T> &_m)
{
std::memcpy(this->data, _m.data, sizeof(this->data[0][0])*9);
}
/// \brief Constructor
/// \param[in] _v00 Row 0, Col 0 value
/// \param[in] _v01 Row 0, Col 1 value
/// \param[in] _v02 Row 0, Col 2 value
/// \param[in] _v10 Row 1, Col 0 value
/// \param[in] _v11 Row 1, Col 1 value
/// \param[in] _v12 Row 1, Col 2 value
/// \param[in] _v20 Row 2, Col 0 value
/// \param[in] _v21 Row 2, Col 1 value
/// \param[in] _v22 Row 2, Col 2 value
public: Matrix3(T _v00, T _v01, T _v02,
T _v10, T _v11, T _v12,
T _v20, T _v21, T _v22)
{
this->data[0][0] = _v00;
this->data[0][1] = _v01;
this->data[0][2] = _v02;
this->data[1][0] = _v10;
this->data[1][1] = _v11;
this->data[1][2] = _v12;
this->data[2][0] = _v20;
this->data[2][1] = _v21;
this->data[2][2] = _v22;
}
/// \brief Construct Matrix3 from a quaternion.
/// \param[in] _q Quaternion.
// cppcheck-suppress noExplicitConstructor
public: Matrix3(const Quaternion<T> &_q)
{
Quaternion<T> qt = _q;
qt.Normalize();
this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y());
}
/// \brief Desctructor
public: virtual ~Matrix3() {}
/// \brief Set values
/// \param[in] _v00 Row 0, Col 0 value
/// \param[in] _v01 Row 0, Col 1 value
/// \param[in] _v02 Row 0, Col 2 value
/// \param[in] _v10 Row 1, Col 0 value
/// \param[in] _v11 Row 1, Col 1 value
/// \param[in] _v12 Row 1, Col 2 value
/// \param[in] _v20 Row 2, Col 0 value
/// \param[in] _v21 Row 2, Col 1 value
/// \param[in] _v22 Row 2, Col 2 value
public: void Set(T _v00, T _v01, T _v02,
T _v10, T _v11, T _v12,
T _v20, T _v21, T _v22)
{
this->data[0][0] = _v00;
this->data[0][1] = _v01;
this->data[0][2] = _v02;
this->data[1][0] = _v10;
this->data[1][1] = _v11;
this->data[1][2] = _v12;
this->data[2][0] = _v20;
this->data[2][1] = _v21;
this->data[2][2] = _v22;
}
/// \brief Set the matrix from three axis (1 per column)
/// \param[in] _xAxis The x axis
/// \param[in] _yAxis The y axis
/// \param[in] _zAxis The z axis
public: void Axes(const Vector3<T> &_xAxis,
const Vector3<T> &_yAxis,
const Vector3<T> &_zAxis)
{
this->Col(0, _xAxis);
this->Col(1, _yAxis);
this->Col(2, _zAxis);
}
/// \brief Set the matrix from an axis and angle
/// \param[in] _axis the axis
/// \param[in] _angle ccw rotation around the axis in radians
public: void Axis(const Vector3<T> &_axis, T _angle)
{
T c = cos(_angle);
T s = sin(_angle);
T C = 1-c;
this->data[0][0] = _axis.X()*_axis.X()*C + c;
this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
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
public: void Col(unsigned int _c, const Vector3<T> &_v)
{
if (_c >= 3)
throw IndexException();
this->data[0][_c] = _v.X();
this->data[1][_c] = _v.Y();
this->data[2][_c] = _v.Z();
}
/// \brief returns the element wise difference of two matrices
public: Matrix3<T> operator-(const Matrix3<T> &_m) const
{
return Matrix3<T>(
this->data[0][0] - _m(0, 0),
this->data[0][1] - _m(0, 1),
this->data[0][2] - _m(0, 2),
this->data[1][0] - _m(1, 0),
this->data[1][1] - _m(1, 1),
this->data[1][2] - _m(1, 2),
this->data[2][0] - _m(2, 0),
this->data[2][1] - _m(2, 1),
this->data[2][2] - _m(2, 2));
}
/// \brief returns the element wise sum of two matrices
public: Matrix3<T> operator+(const Matrix3<T> &_m) const
{
return Matrix3<T>(
this->data[0][0]+_m(0, 0),
this->data[0][1]+_m(0, 1),
this->data[0][2]+_m(0, 2),
this->data[1][0]+_m(1, 0),
this->data[1][1]+_m(1, 1),
this->data[1][2]+_m(1, 2),
this->data[2][0]+_m(2, 0),
this->data[2][1]+_m(2, 1),
this->data[2][2]+_m(2, 2));
}
/// \brief returns the element wise scalar multiplication
public: Matrix3<T> operator*(const T &_s) const
{
return Matrix3<T>(
_s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2],
_s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2],
_s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]);
}
/// \brief Matrix multiplication operator
/// \param[in] _m Matrix3<T> to multiply
/// \return product of this * _m
public: Matrix3<T> operator*(const Matrix3<T> &_m) const
{
return Matrix3<T>(
// first row
this->data[0][0]*_m(0, 0)+
this->data[0][1]*_m(1, 0)+
this->data[0][2]*_m(2, 0),
this->data[0][0]*_m(0, 1)+
this->data[0][1]*_m(1, 1)+
this->data[0][2]*_m(2, 1),
this->data[0][0]*_m(0, 2)+
this->data[0][1]*_m(1, 2)+
this->data[0][2]*_m(2, 2),
// second row
this->data[1][0]*_m(0, 0)+
this->data[1][1]*_m(1, 0)+
this->data[1][2]*_m(2, 0),
this->data[1][0]*_m(0, 1)+
this->data[1][1]*_m(1, 1)+
this->data[1][2]*_m(2, 1),
this->data[1][0]*_m(0, 2)+
this->data[1][1]*_m(1, 2)+
this->data[1][2]*_m(2, 2),
// third row
this->data[2][0]*_m(0, 0)+
this->data[2][1]*_m(1, 0)+
this->data[2][2]*_m(2, 0),
this->data[2][0]*_m(0, 1)+
this->data[2][1]*_m(1, 1)+
this->data[2][2]*_m(2, 1),
this->data[2][0]*_m(0, 2)+
this->data[2][1]*_m(1, 2)+
this->data[2][2]*_m(2, 2));
}
/// \brief Multiplication operator with Vector3 on the right
/// treated like a column vector.
/// \param _vec Vector3
/// \return Resulting vector from multiplication
public: Vector3<T> operator*(const Vector3<T> &_vec) const
{
return Vector3<T>(
this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
this->data[0][2]*_vec.Z(),
this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
this->data[1][2]*_vec.Z(),
this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
this->data[2][2]*_vec.Z());
}
/// \brief Matrix multiplication operator for scaling.
/// \param[in] _s Scaling factor.
/// \param[in] _m Input matrix.
/// \return A scaled matrix.
public: friend inline Matrix3<T> operator*(T _s, const Matrix3<T> &_m)
{
return _m * _s;
}
/// \brief Matrix left multiplication operator for Vector3.
/// Treats the Vector3 like a row vector multiplying the matrix
/// from the left.
/// \param[in] _v Input vector.
/// \param[in] _m Input matrix.
/// \return The product vector.
public: friend inline Vector3<T> operator*(const Vector3<T> &_v,
const Matrix3<T> &_m)
{
return Vector3<T>(
_m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(),
_m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(),
_m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z());
}
/// \brief Equality test with tolerance.
/// \param[in] _m the matrix to compare to
/// \param[in] _tol equality tolerance.
/// \return true if the elements of the matrices are equal within
/// the tolerence specified by _tol.
public: bool Equal(const Matrix3 &_m, const T &_tol) const
{
return equal<T>(this->data[0][0], _m(0, 0), _tol)
&& equal<T>(this->data[0][1], _m(0, 1), _tol)
&& equal<T>(this->data[0][2], _m(0, 2), _tol)
&& equal<T>(this->data[1][0], _m(1, 0), _tol)
&& equal<T>(this->data[1][1], _m(1, 1), _tol)
&& equal<T>(this->data[1][2], _m(1, 2), _tol)
&& equal<T>(this->data[2][0], _m(2, 0), _tol)
&& equal<T>(this->data[2][1], _m(2, 1), _tol)
&& equal<T>(this->data[2][2], _m(2, 2), _tol);
}
/// \brief Equality test operator
/// \param[in] _m Matrix3<T> to test
/// \return True if equal (using the default tolerance of 1e-6)
public: bool operator==(const Matrix3<T> &_m) const
{
return this->Equal(_m, static_cast<T>(1e-6));
}
/// \brief Inequality test operator
/// \param[in] _m Matrix3<T> to test
/// \return True if not equal (using the default tolerance of 1e-6)
public: bool operator!=(const Matrix3<T> &_m) const
{
return !(*this == _m);
}
/// \brief Array subscript operator
/// \param[in] _row row index
/// \return a pointer to the row
public: inline const T &operator()(size_t _row, size_t _col) const
{
if (_row >= 3 || _col >= 3)
throw IndexException();
return this->data[_row][_col];
}
/// \brief Array subscript operator
/// \param[in] _row row index
/// \return a pointer to the row
public: inline T &operator()(size_t _row, size_t _col)
{
if (_row >= 3 || _col >=3)
throw IndexException();
return this->data[_row][_col];
}
/// \brief Return the determinant of the matrix
/// \return Determinant of this matrix.
public: T Determinant() const
{
T t0 = this->data[2][2]*this->data[1][1]
- this->data[2][1]*this->data[1][2];
T t1 = -(this->data[2][2]*this->data[1][0]
-this->data[2][0]*this->data[1][2]);
T t2 = this->data[2][1]*this->data[1][0]
- this->data[2][0]*this->data[1][1];
return t0 * this->data[0][0]
+ t1 * this->data[0][1]
+ t2 * this->data[0][2];
}
/// \brief Return the inverse matrix
/// \return Inverse of this matrix.
public: Matrix3<T> Inverse() const
{
T t0 = this->data[2][2]*this->data[1][1] -
this->data[2][1]*this->data[1][2];
T t1 = -(this->data[2][2]*this->data[1][0] -
this->data[2][0]*this->data[1][2]);
T t2 = this->data[2][1]*this->data[1][0] -
this->data[2][0]*this->data[1][1];
T invDet = 1.0 / (t0 * this->data[0][0] +
t1 * this->data[0][1] +
t2 * this->data[0][2]);
return invDet * Matrix3<T>(
t0,
- (this->data[2][2] * this->data[0][1] -
this->data[2][1] * this->data[0][2]),
+ (this->data[1][2] * this->data[0][1] -
this->data[1][1] * this->data[0][2]),
t1,
+ (this->data[2][2] * this->data[0][0] -
this->data[2][0] * this->data[0][2]),
- (this->data[1][2] * this->data[0][0] -
this->data[1][0] * this->data[0][2]),
t2,
- (this->data[2][1] * this->data[0][0] -
this->data[2][0] * this->data[0][1]),
+ (this->data[1][1] * this->data[0][0] -
this->data[1][0] * this->data[0][1]));
}
/// \brief Transpose this matrix.
public: void Transpose()
{
std::swap(this->data[0][1], this->data[1][0]);
std::swap(this->data[0][2], this->data[2][0]);
std::swap(this->data[1][2], this->data[2][1]);
}
/// \brief Return the transpose of this matrix
/// \return Transpose of this matrix.
public: Matrix3<T> Transposed() const
{
return Matrix3<T>(
this->data[0][0], this->data[1][0], this->data[2][0],
this->data[0][1], this->data[1][1], this->data[2][1],
this->data[0][2], this->data[1][2], this->data[2][2]);
}
/// \brief Stream insertion operator
/// \param[in] _out Output stream
/// \param[in] _m Matrix to output
/// \return the stream
public: friend std::ostream &operator<<(
std::ostream &_out, const ignition::math::Matrix3<T> &_m)
{
_out << precision(_m(0, 0), 6) << " "
<< precision(_m(0, 1), 6) << " "
<< precision(_m(0, 2), 6) << " "
<< precision(_m(1, 0), 6) << " "
<< precision(_m(1, 1), 6) << " "
<< precision(_m(1, 2), 6) << " "
<< precision(_m(2, 0), 6) << " "
<< precision(_m(2, 1), 6) << " "
<< precision(_m(2, 2), 6);
return _out;
}
/// \brief Stream extraction operator
/// \param _in input stream
/// \param _pt Matrix3 to read values into
/// \return the stream
public: friend std::istream &operator>>(
std::istream &_in, ignition::math::Matrix3<T> &_m)
{
// Skip white spaces
_in.setf(std::ios_base::skipws);
T d[9];
_in >> d[0] >> d[1] >> d[2]
>> d[3] >> d[4] >> d[5]
>> d[6] >> d[7] >> d[8];
_m.Set(d[0], d[1], d[2],
d[3], d[4], d[5],
d[6], d[7], d[8]);
return _in;
}
/// \brief the 3x3 matrix
private: T data[3][3];
};
template<typename T>
const Matrix3<T> Matrix3<T>::Identity(
1, 0, 0,
0, 1, 0,
0, 0, 1);
template<typename T>
const Matrix3<T> Matrix3<T>::Zero(
0, 0, 0,
0, 0, 0,
0, 0, 0);
typedef Matrix3<int> Matrix3i;
typedef Matrix3<double> Matrix3d;
typedef Matrix3<float> Matrix3f;
}
}
#endif

860
ign-math/include/ignition/math/Matrix4.hh Normal file
View File

@ -0,0 +1,860 @@
/*
* Copyright (C) 2012-2014 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_MATRIX4_HH_
#define IGNITION_MATH_MATRIX4_HH_
#include <algorithm>
#include <ignition/math/AffineException.hh>
#include <ignition/math/Matrix3.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Pose3.hh>
namespace ignition
{
namespace math
{
/// \class Matrix4 Matrix4.hh ignition/math/Matrix4.hh
/// \brief A 4x4 matrix class
template<typename T>
class Matrix4
{
/// \brief Identity matrix
public: static const Matrix4<T> Identity;
/// \brief Zero matrix
public: static const Matrix4<T> Zero;
/// \brief Constructor
public: Matrix4()
{
memset(this->data, 0, sizeof(this->data[0][0])*16);
}
/// \brief Copy constructor
/// \param _m Matrix to copy
public: Matrix4(const Matrix4<T> &_m)
{
memcpy(this->data, _m.data, sizeof(this->data[0][0])*16);
}
/// \brief Constructor
/// \param[in] _v00 Row 0, Col 0 value
/// \param[in] _v01 Row 0, Col 1 value
/// \param[in] _v02 Row 0, Col 2 value
/// \param[in] _v03 Row 0, Col 3 value
/// \param[in] _v10 Row 1, Col 0 value
/// \param[in] _v11 Row 1, Col 1 value
/// \param[in] _v12 Row 1, Col 2 value
/// \param[in] _v13 Row 1, Col 3 value
/// \param[in] _v20 Row 2, Col 0 value
/// \param[in] _v21 Row 2, Col 1 value
/// \param[in] _v22 Row 2, Col 2 value
/// \param[in] _v23 Row 2, Col 3 value
/// \param[in] _v30 Row 3, Col 0 value
/// \param[in] _v31 Row 3, Col 1 value
/// \param[in] _v32 Row 3, Col 2 value
/// \param[in] _v33 Row 3, Col 3 value
public: Matrix4(T _v00, T _v01, T _v02, T _v03,
T _v10, T _v11, T _v12, T _v13,
T _v20, T _v21, T _v22, T _v23,
T _v30, T _v31, T _v32, T _v33)
{
this->Set(_v00, _v01, _v02, _v03,
_v10, _v11, _v12, _v13,
_v20, _v21, _v22, _v23,
_v30, _v31, _v32, _v33);
}
/// \brief Construct Matrix4 from a quaternion.
/// \param[in] _q Quaternion.
// cppcheck-suppress noExplicitConstructor
public: Matrix4(const Quaternion<T> &_q)
{
Quaternion<T> qt = _q;
qt.Normalize();
this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
0,
2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
0,
2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y(),
0,
0, 0, 0, 1);
}
/// \brief Construct Matrix4 from a math::Pose3
/// \param[in] _pose Pose.
// cppcheck-suppress noExplicitConstructor
public: Matrix4(const Pose3<T> &_pose) : Matrix4(_pose.Rot())
{
this->Translate(_pose.Pos());
}
/// \brief Destructor
public: virtual ~Matrix4() {}
/// \brief Change the values
/// \param[in] _v00 Row 0, Col 0 value
/// \param[in] _v01 Row 0, Col 1 value
/// \param[in] _v02 Row 0, Col 2 value
/// \param[in] _v03 Row 0, Col 3 value
/// \param[in] _v10 Row 1, Col 0 value
/// \param[in] _v11 Row 1, Col 1 value
/// \param[in] _v12 Row 1, Col 2 value
/// \param[in] _v13 Row 1, Col 3 value
/// \param[in] _v20 Row 2, Col 0 value
/// \param[in] _v21 Row 2, Col 1 value
/// \param[in] _v22 Row 2, Col 2 value
/// \param[in] _v23 Row 2, Col 3 value
/// \param[in] _v30 Row 3, Col 0 value
/// \param[in] _v31 Row 3, Col 1 value
/// \param[in] _v32 Row 3, Col 2 value
/// \param[in] _v33 Row 3, Col 3 value
public: void Set(
T _v00, T _v01, T _v02, T _v03,
T _v10, T _v11, T _v12, T _v13,
T _v20, T _v21, T _v22, T _v23,
T _v30, T _v31, T _v32, T _v33)
{
this->data[0][0] = _v00;
this->data[0][1] = _v01;
this->data[0][2] = _v02;
this->data[0][3] = _v03;
this->data[1][0] = _v10;
this->data[1][1] = _v11;
this->data[1][2] = _v12;
this->data[1][3] = _v13;
this->data[2][0] = _v20;
this->data[2][1] = _v21;
this->data[2][2] = _v22;
this->data[2][3] = _v23;
this->data[3][0] = _v30;
this->data[3][1] = _v31;
this->data[3][2] = _v32;
this->data[3][3] = _v33;
}
/// \brief Set the upper-left 3x3 matrix from an axis and angle
/// \param[in] _axis the axis
/// \param[in] _angle ccw rotation around the axis in radians
public: void Axis(const Vector3<T> &_axis, T _angle)
{
T c = cos(_angle);
T s = sin(_angle);
T C = 1-c;
this->data[0][0] = _axis.X()*_axis.X()*C + c;
this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
}
/// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ]
/// \param[in] _t Values to set
public: void Translate(const Vector3<T> &_t)
{
this->data[0][3] = _t.X();
this->data[1][3] = _t.Y();
this->data[2][3] = _t.Z();
}
/// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ]
/// \param[in] _x X translation value.
/// \param[in] _y Y translation value.
/// \param[in] _z Z translation value.
public: void Translate(T _x, T _y, T _z)
{
this->data[0][3] = _x;
this->data[1][3] = _y;
this->data[2][3] = _z;
}
/// \brief Get the translational values as a Vector3
/// \return x,y,z translation values
public: Vector3<T> Translation() const
{
return Vector3<T>(this->data[0][3], this->data[1][3], this->data[2][3]);
}
/// \brief Get the scale values as a Vector3<T>
/// \return x,y,z scale values
public: Vector3<T> Scale() const
{
return Vector3<T>(this->data[0][0], this->data[1][1], this->data[2][2]);
}
/// \brief Get the rotation as a quaternion
/// \return the rotation
public: Quaternion<T> Rotation() const
{
Quaternion<T> q;
/// algorithm from Ogre::Quaternion<T> source, which in turn is based on
/// Ken Shoemake's article "Quaternion<T> Calculus and Fast Animation".
T trace = this->data[0][0] + this->data[1][1] + this->data[2][2];
T root;
if (trace > 0)
{
root = sqrt(trace + 1.0);
q.W(root / 2.0);
root = 1.0 / (2.0 * root);
q.X((this->data[2][1] - this->data[1][2]) * root);
q.Y((this->data[0][2] - this->data[2][0]) * root);
q.Z((this->data[1][0] - this->data[0][1]) * root);
}
else
{
static unsigned int s_iNext[3] = {1, 2, 0};
unsigned int i = 0;
if (this->data[1][1] > this->data[0][0])
i = 1;
if (this->data[2][2] > this->data[i][i])
i = 2;
unsigned int j = s_iNext[i];
unsigned int k = s_iNext[j];
root = sqrt(this->data[i][i] - this->data[j][j] -
this->data[k][k] + 1.0);
T a, b, c;
a = root / 2.0;
root = 1.0 / (2.0 * root);
b = (this->data[j][i] + this->data[i][j]) * root;
c = (this->data[k][i] + this->data[i][k]) * root;
switch (i)
{
default:
case 0: q.X(a); break;
case 1: q.Y(a); break;
case 2: q.Z(a); break;
};
switch (j)
{
default:
case 0: q.X(b); break;
case 1: q.Y(b); break;
case 2: q.Z(b); break;
};
switch (k)
{
default:
case 0: q.X(c); break;
case 1: q.Y(c); break;
case 2: q.Z(c); break;
};
q.W((this->data[k][j] - this->data[j][k]) * root);
}
return q;
}
/// \brief Get the rotation as a Euler angles
/// \param[in] _firstSolution True to get the first Euler solution,
/// false to get the second.
/// \return the rotation
public: Vector3<T> EulerRotation(bool _firstSolution) const
{
Vector3<T> euler;
Vector3<T> euler2;
T m31 = this->data[2][0];
T m11 = this->data[0][0];
T m12 = this->data[0][1];
T m13 = this->data[0][2];
T m32 = this->data[2][1];
T m33 = this->data[2][2];
T m21 = this->data[1][0];
if (std::abs(m31) >= 1.0)
{
euler.Z(0.0);
euler2.Z(0.0);
if (m31 < 0.0)
{
euler.Y(IGN_PI / 2.0);
euler2.Y(IGN_PI / 2.0);
euler.X(atan2(m12, m13));
euler2.X(atan2(m12, m13));
}
else
{
euler.Y(-IGN_PI / 2.0);
euler2.Y(-IGN_PI / 2.0);
euler.X(atan2(-m12, -m13));
euler2.X(atan2(-m12, -m13));
}
}
else
{
euler.Y(-asin(m31));
euler2.Y(IGN_PI - euler.Y());
euler.X(atan2(m32 / cos(euler.Y()), m33 / cos(euler.Y())));
euler2.X(atan2(m32 / cos(euler2.Y()), m33 / cos(euler2.Y())));
euler.Z(atan2(m21 / cos(euler.Y()), m11 / cos(euler.Y())));
euler2.Z(atan2(m21 / cos(euler2.Y()), m11 / cos(euler2.Y())));
}
if (_firstSolution)
return euler;
else
return euler2;
}
/// \brief Get the transformation as math::Pose
/// \return the pose
public: Pose3<T> Pose() const
{
return Pose3<T>(this->Translation(), this->Rotation());
}
/// \brief Set the scale
/// \param[in] _s scale
public: void Scale(const Vector3<T> &_s)
{
this->data[0][0] = _s.X();
this->data[1][1] = _s.Y();
this->data[2][2] = _s.Z();
this->data[3][3] = 1.0;
}
/// \brief Set the scale
/// \param[in] _x X scale value.
/// \param[in] _y Y scale value.
/// \param[in] _z Z scale value.
public: void Scale(T _x, T _y, T _z)
{
this->data[0][0] = _x;
this->data[1][1] = _y;
this->data[2][2] = _z;
this->data[3][3] = 1.0;
}
/// \brief Return true if the matrix is affine
/// \return true if the matrix is affine, false otherwise
public: bool IsAffine() const
{
return equal(this->data[3][0], static_cast<T>(0)) &&
equal(this->data[3][1], static_cast<T>(0)) &&
equal(this->data[3][2], static_cast<T>(0)) &&
equal(this->data[3][3], static_cast<T>(1));
}
/// \brief Perform an affine transformation
/// \param _v Vector3 value for the transformation
/// \return The result of the transformation
/// \throws AffineException when matrix is not affine.
public: Vector3<T> TransformAffine(const Vector3<T> &_v) const
{
if (!this->IsAffine())
throw AffineException();
return Vector3<T>(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
this->data[0][2]*_v.Z() + this->data[0][3],
this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
this->data[1][2]*_v.Z() + this->data[1][3],
this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
this->data[2][2]*_v.Z() + this->data[2][3]);
}
/// \brief Return the determinant of the matrix
/// \return Determinant of this matrix.
public: T Determinant() const
{
T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
v0 = this->data[2][0]*this->data[3][1]
- this->data[2][1]*this->data[3][0];
v1 = this->data[2][0]*this->data[3][2]
- this->data[2][2]*this->data[3][0];
v2 = this->data[2][0]*this->data[3][3]
- this->data[2][3]*this->data[3][0];
v3 = this->data[2][1]*this->data[3][2]
- this->data[2][2]*this->data[3][1];
v4 = this->data[2][1]*this->data[3][3]
- this->data[2][3]*this->data[3][1];
v5 = this->data[2][2]*this->data[3][3]
- this->data[2][3]*this->data[3][2];
t00 = v5*this->data[1][1] - v4*this->data[1][2] + v3*this->data[1][3];
t10 = -v5*this->data[1][0] + v2*this->data[1][2] - v1*this->data[1][3];
t20 = v4*this->data[1][0] - v2*this->data[1][1] + v0*this->data[1][3];
t30 = -v3*this->data[1][0] + v1*this->data[1][1] - v0*this->data[1][2];
return t00 * this->data[0][0]
+ t10 * this->data[0][1]
+ t20 * this->data[0][2]
+ t30 * this->data[0][3];
}
/// \brief Return the inverse matrix.
/// This is a non-destructive operation.
/// \return Inverse of this matrix.
public: Matrix4<T> Inverse() const
{
T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
Matrix4<T> r;
v0 = this->data[2][0]*this->data[3][1] -
this->data[2][1]*this->data[3][0];
v1 = this->data[2][0]*this->data[3][2] -
this->data[2][2]*this->data[3][0];
v2 = this->data[2][0]*this->data[3][3] -
this->data[2][3]*this->data[3][0];
v3 = this->data[2][1]*this->data[3][2] -
this->data[2][2]*this->data[3][1];
v4 = this->data[2][1]*this->data[3][3] -
this->data[2][3]*this->data[3][1];
v5 = this->data[2][2]*this->data[3][3] -
this->data[2][3]*this->data[3][2];
t00 = +(v5*this->data[1][1] -
v4*this->data[1][2] + v3*this->data[1][3]);
t10 = -(v5*this->data[1][0] -
v2*this->data[1][2] + v1*this->data[1][3]);
t20 = +(v4*this->data[1][0] -
v2*this->data[1][1] + v0*this->data[1][3]);
t30 = -(v3*this->data[1][0] -
v1*this->data[1][1] + v0*this->data[1][2]);
T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] +
t20 * this->data[0][2] + t30 * this->data[0][3]);
r(0, 0) = t00 * invDet;
r(1, 0) = t10 * invDet;
r(2, 0) = t20 * invDet;
r(3, 0) = t30 * invDet;
r(0, 1) = -(v5*this->data[0][1] -
v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
r(1, 1) = +(v5*this->data[0][0] -
v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
r(2, 1) = -(v4*this->data[0][0] -
v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
r(3, 1) = +(v3*this->data[0][0] -
v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
v0 = this->data[1][0]*this->data[3][1] -
this->data[1][1]*this->data[3][0];
v1 = this->data[1][0]*this->data[3][2] -
this->data[1][2]*this->data[3][0];
v2 = this->data[1][0]*this->data[3][3] -
this->data[1][3]*this->data[3][0];
v3 = this->data[1][1]*this->data[3][2] -
this->data[1][2]*this->data[3][1];
v4 = this->data[1][1]*this->data[3][3] -
this->data[1][3]*this->data[3][1];
v5 = this->data[1][2]*this->data[3][3] -
this->data[1][3]*this->data[3][2];
r(0, 2) = +(v5*this->data[0][1] -
v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
r(1, 2) = -(v5*this->data[0][0] -
v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
r(2, 2) = +(v4*this->data[0][0] -
v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
r(3, 2) = -(v3*this->data[0][0] -
v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
v0 = this->data[2][1]*this->data[1][0] -
this->data[2][0]*this->data[1][1];
v1 = this->data[2][2]*this->data[1][0] -
this->data[2][0]*this->data[1][2];
v2 = this->data[2][3]*this->data[1][0] -
this->data[2][0]*this->data[1][3];
v3 = this->data[2][2]*this->data[1][1] -
this->data[2][1]*this->data[1][2];
v4 = this->data[2][3]*this->data[1][1] -
this->data[2][1]*this->data[1][3];
v5 = this->data[2][3]*this->data[1][2] -
this->data[2][2]*this->data[1][3];
r(0, 3) = -(v5*this->data[0][1] -
v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
r(1, 3) = +(v5*this->data[0][0] -
v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
r(2, 3) = -(v4*this->data[0][0] -
v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
r(3, 3) = +(v3*this->data[0][0] -
v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
return r;
}
/// \brief Transpose this matrix.
public: void Transpose()
{
std::swap(this->data[0][1], this->data[1][0]);
std::swap(this->data[0][2], this->data[2][0]);
std::swap(this->data[0][3], this->data[3][0]);
std::swap(this->data[1][2], this->data[2][1]);
std::swap(this->data[1][3], this->data[3][1]);
std::swap(this->data[2][3], this->data[3][2]);
}
/// \brief Return the transpose of this matrix
/// \return Transpose of this matrix.
public: Matrix4<T> Transposed() const
{
return Matrix4<T>(
this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0],
this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1],
this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2],
this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3]);
}
/// \brief Equal operator. this = _mat
/// \param _mat Incoming matrix
/// \return itself
public: Matrix4<T> &operator=(const Matrix4<T> &_mat)
{
memcpy(this->data, _mat.data, sizeof(this->data[0][0])*16);
return *this;
}
/// \brief Equal operator for 3x3 matrix
/// \param _mat Incoming matrix
/// \return itself
public: const Matrix4<T> &operator=(const Matrix3<T> &_mat)
{
this->data[0][0] = _mat(0, 0);
this->data[0][1] = _mat(0, 1);
this->data[0][2] = _mat(0, 2);
this->data[1][0] = _mat(1, 0);
this->data[1][1] = _mat(1, 1);
this->data[1][2] = _mat(1, 2);
this->data[2][0] = _mat(2, 0);
this->data[2][1] = _mat(2, 1);
this->data[2][2] = _mat(2, 2);
return *this;
}
/// \brief Multiplication operator
/// \param _mat Incoming matrix
/// \return This matrix * _mat
public: Matrix4<T> operator*(const Matrix4<T> &_m2) const
{
return Matrix4<T>(
this->data[0][0] * _m2(0, 0) +
this->data[0][1] * _m2(1, 0) +
this->data[0][2] * _m2(2, 0) +
this->data[0][3] * _m2(3, 0),
this->data[0][0] * _m2(0, 1) +
this->data[0][1] * _m2(1, 1) +
this->data[0][2] * _m2(2, 1) +
this->data[0][3] * _m2(3, 1),
this->data[0][0] * _m2(0, 2) +
this->data[0][1] * _m2(1, 2) +
this->data[0][2] * _m2(2, 2) +
this->data[0][3] * _m2(3, 2),
this->data[0][0] * _m2(0, 3) +
this->data[0][1] * _m2(1, 3) +
this->data[0][2] * _m2(2, 3) +
this->data[0][3] * _m2(3, 3),
this->data[1][0] * _m2(0, 0) +
this->data[1][1] * _m2(1, 0) +
this->data[1][2] * _m2(2, 0) +
this->data[1][3] * _m2(3, 0),
this->data[1][0] * _m2(0, 1) +
this->data[1][1] * _m2(1, 1) +
this->data[1][2] * _m2(2, 1) +
this->data[1][3] * _m2(3, 1),
this->data[1][0] * _m2(0, 2) +
this->data[1][1] * _m2(1, 2) +
this->data[1][2] * _m2(2, 2) +
this->data[1][3] * _m2(3, 2),
this->data[1][0] * _m2(0, 3) +
this->data[1][1] * _m2(1, 3) +
this->data[1][2] * _m2(2, 3) +
this->data[1][3] * _m2(3, 3),
this->data[2][0] * _m2(0, 0) +
this->data[2][1] * _m2(1, 0) +
this->data[2][2] * _m2(2, 0) +
this->data[2][3] * _m2(3, 0),
this->data[2][0] * _m2(0, 1) +
this->data[2][1] * _m2(1, 1) +
this->data[2][2] * _m2(2, 1) +
this->data[2][3] * _m2(3, 1),
this->data[2][0] * _m2(0, 2) +
this->data[2][1] * _m2(1, 2) +
this->data[2][2] * _m2(2, 2) +
this->data[2][3] * _m2(3, 2),
this->data[2][0] * _m2(0, 3) +
this->data[2][1] * _m2(1, 3) +
this->data[2][2] * _m2(2, 3) +
this->data[2][3] * _m2(3, 3),
this->data[3][0] * _m2(0, 0) +
this->data[3][1] * _m2(1, 0) +
this->data[3][2] * _m2(2, 0) +
this->data[3][3] * _m2(3, 0),
this->data[3][0] * _m2(0, 1) +
this->data[3][1] * _m2(1, 1) +
this->data[3][2] * _m2(2, 1) +
this->data[3][3] * _m2(3, 1),
this->data[3][0] * _m2(0, 2) +
this->data[3][1] * _m2(1, 2) +
this->data[3][2] * _m2(2, 2) +
this->data[3][3] * _m2(3, 2),
this->data[3][0] * _m2(0, 3) +
this->data[3][1] * _m2(1, 3) +
this->data[3][2] * _m2(2, 3) +
this->data[3][3] * _m2(3, 3));
}
/// \brief Multiplication operator
/// \param _vec Vector3
/// \return Resulting vector from multiplication
public: Vector3<T> operator*(const Vector3<T> &_vec) const
{
return Vector3<T>(
this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
this->data[0][2]*_vec.Z() + this->data[0][3],
this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
this->data[1][2]*_vec.Z() + this->data[1][3],
this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
this->data[2][2]*_vec.Z() + this->data[2][3]);
}
/// \brief Get the value at the specified row, column index
/// \param[in] _col The column index
/// \param[in] _row the row index
/// \return The value at the specified index
public: inline const T &operator()(size_t _row, size_t _col) const
{
if (_row >= 4 || _col >= 4)
throw IndexException();
return this->data[_row][_col];
}
/// \brief Get a mutable version the value at the specified row,
/// column index
/// \param[in] _col The column index
/// \param[in] _row The row index
/// \return The value at the specified index
public: inline T &operator()(size_t _row, size_t _col)
{
if (_row >= 4 || _col >= 4)
throw IndexException();
return this->data[_row][_col];
}
/// \brief Equality test with tolerance.
/// \param[in] _m the matrix to compare to
/// \param[in] _tol equality tolerance.
/// \return true if the elements of the matrices are equal within
/// the tolerence specified by _tol.
public: bool Equal(const Matrix4 &_m, const T &_tol) const
{
return equal<T>(this->data[0][0], _m(0, 0), _tol)
&& equal<T>(this->data[0][1], _m(0, 1), _tol)
&& equal<T>(this->data[0][2], _m(0, 2), _tol)
&& equal<T>(this->data[0][3], _m(0, 3), _tol)
&& equal<T>(this->data[1][0], _m(1, 0), _tol)
&& equal<T>(this->data[1][1], _m(1, 1), _tol)
&& equal<T>(this->data[1][2], _m(1, 2), _tol)
&& equal<T>(this->data[1][3], _m(1, 3), _tol)
&& equal<T>(this->data[2][0], _m(2, 0), _tol)
&& equal<T>(this->data[2][1], _m(2, 1), _tol)
&& equal<T>(this->data[2][2], _m(2, 2), _tol)
&& equal<T>(this->data[2][3], _m(2, 3), _tol)
&& equal<T>(this->data[3][0], _m(3, 0), _tol)
&& equal<T>(this->data[3][1], _m(3, 1), _tol)
&& equal<T>(this->data[3][2], _m(3, 2), _tol)
&& equal<T>(this->data[3][3], _m(3, 3), _tol);
}
/// \brief Equality operator
/// \param[in] _m Matrix3 to test
/// \return true if the 2 matrices are equal (using the tolerance 1e-6),
/// false otherwise
public: bool operator==(const Matrix4<T> &_m) const
{
return this->Equal(_m, static_cast<T>(1e-6));
}
/// \brief Inequality test operator
/// \param[in] _m Matrix4<T> to test
/// \return True if not equal (using the default tolerance of 1e-6)
public: bool operator!=(const Matrix4<T> &_m) const
{
return !(*this == _m);
}
/// \brief Stream insertion operator
/// \param _out output stream
/// \param _m Matrix to output
/// \return the stream
public: friend std::ostream &operator<<(
std::ostream &_out, const ignition::math::Matrix4<T> &_m)
{
_out << precision(_m(0, 0), 6) << " "
<< precision(_m(0, 1), 6) << " "
<< precision(_m(0, 2), 6) << " "
<< precision(_m(0, 3), 6) << " "
<< precision(_m(1, 0), 6) << " "
<< precision(_m(1, 1), 6) << " "
<< precision(_m(1, 2), 6) << " "
<< precision(_m(1, 3), 6) << " "
<< precision(_m(2, 0), 6) << " "
<< precision(_m(2, 1), 6) << " "
<< precision(_m(2, 2), 6) << " "
<< precision(_m(2, 3), 6) << " "
<< precision(_m(3, 0), 6) << " "
<< precision(_m(3, 1), 6) << " "
<< precision(_m(3, 2), 6) << " "
<< precision(_m(3, 3), 6);
return _out;
}
/// \brief Stream extraction operator
/// \param _in input stream
/// \param _pt Matrix4<T> to read values into
/// \return the stream
public: friend std::istream &operator>>(
std::istream &_in, ignition::math::Matrix4<T> &_m)
{
// Skip white spaces
_in.setf(std::ios_base::skipws);
T d[16];
_in >> d[0] >> d[1] >> d[2] >> d[3]
>> d[4] >> d[5] >> d[6] >> d[7]
>> d[8] >> d[9] >> d[10] >> d[11]
>> d[12] >> d[13] >> d[14] >> d[15];
_m.Set(d[0], d[1], d[2], d[3],
d[4], d[5], d[6], d[7],
d[8], d[9], d[10], d[11],
d[12], d[13], d[14], d[15]);
return _in;
}
/// \brief Get transform which translates to _eye and rotates the X axis
/// so it faces the _target. The rotation is such that Z axis is in the
/// _up direction, if possible. The coordinate system is right-handed,
/// \param[in] _eye Coordinate frame translation.
/// \param[in] _target Point which the X axis should face. If _target is
/// equal to _eye, the X axis won't be rotated.
/// \param[in] _up Direction in which the Z axis should point. If _up is
/// zero or parallel to X, it will be set to +Z.
/// \return Transformation matrix.
public: static Matrix4<T> LookAt(const Vector3<T> &_eye,
const Vector3<T> &_target, const Vector3<T> &_up = Vector3<T>::UnitZ)
{
// Most important constraint: direction to point X axis at
auto front = _target - _eye;
// Case when _eye == _target
if (front == Vector3<T>::Zero)
front = Vector3<T>::UnitX;
front.Normalize();
// Desired direction to point Z axis at
auto up = _up;
// Case when _up == Zero
if (up == Vector3<T>::Zero)
up = Vector3<T>::UnitZ;
else
up.Normalize();
// Case when _up is parallel to X
if (up.Cross(Vector3<T>::UnitX) == Vector3<T>::Zero)
up = Vector3<T>::UnitZ;
// Find direction to point Y axis at
auto left = up.Cross(front);
// Case when front is parallel to up
if (left == Vector3<T>::Zero)
left = Vector3<T>::UnitY;
else
left.Normalize();
// Fix up direction so it's perpendicular to XY
up = (front.Cross(left)).Normalize();
return Matrix4<T>(
front.X(), left.X(), up.X(), _eye.X(),
front.Y(), left.Y(), up.Y(), _eye.Y(),
front.Z(), left.Z(), up.Z(), _eye.Z(),
0, 0, 0, 1);
}
/// \brief The 4x4 matrix
private: T data[4][4];
};
template<typename T>
const Matrix4<T> Matrix4<T>::Identity(
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
template<typename T>
const Matrix4<T> Matrix4<T>::Zero(
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0);
typedef Matrix4<int> Matrix4i;
typedef Matrix4<double> Matrix4d;
typedef Matrix4<float> Matrix4f;
}
}
#endif

187
ign-math/include/ignition/math/OrientedBox.hh Normal file
View File

@ -0,0 +1,187 @@
/*
* Copyright (C) 2017 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_ORIENTEDBOX_HH_
#define IGNITION_MATH_ORIENTEDBOX_HH_
#include <iostream>
#include <ignition/math/Helpers.hh>
#include <ignition/math/Matrix4.hh>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Vector3.hh>
namespace ignition
{
namespace math
{
/// \brief Mathematical representation of a box which can be arbitrarily
/// positioned and rotated.
template<typename T>
class OrientedBox
{
/// \brief Default constructor
public: OrientedBox() : size(Vector3<T>::Zero), pose(Pose3<T>::Zero)
{
}
/// \brief Constructor which takes size and pose.
/// \param[in] _size Box size, in its own coordinate frame. Its absolute
/// value will be taken, so the size is non-negative.
/// \param[in] _pose Box pose.
public: OrientedBox(const Vector3<T> &_size, const Pose3<T> &_pose)
: size(_size), pose(_pose)
{
// Enforce non-negative size
this->size = this->size.Abs();
}
/// \brief Constructor which takes only the size.
/// \param[in] _size Box size, in its own coordinate frame. Its absolute
/// value will be taken, so the size is non-negative.
public: explicit OrientedBox(const Vector3<T> &_size)
: size(_size), pose(Pose3<T>::Zero)
{
// Enforce non-negative size
this->size = this->size.Abs();
}
/// \brief Copy constructor.
/// \param[in] _b OrientedBox to copy.
public: OrientedBox(const OrientedBox<T> &_b)
: size(_b.size), pose(_b.pose)
{
}
/// \brief Destructor
public: virtual ~OrientedBox()
{
}
/// \brief Get the length along the x dimension
/// \return Value of the length in the x dimension
public: T XLength() const
{
return this->size.X();
}
/// \brief Get the length along the y dimension
/// \return Value of the length in the y dimension
public: T YLength() const
{
return this->size.Y();
}
/// \brief Get the length along the z dimension
/// \return Value of the length in the z dimension
public: T ZLength() const
{
return this->size.Z();
}
/// \brief Get the size of the box
/// \return Size of the box
public: const Vector3<T> &Size() const
{
return this->size;
}
/// \brief Get the box pose, which is the pose of its center.
/// \return The pose of the box.
public: const Pose3<T> &Pose() const
{
return this->pose;
}
/// \brief Set the box size.
/// \param[in] _size Box size, in its own coordinate frame. Its absolute
/// value will be taken, so the size is non-negative.
public: void Size(Vector3<T> &_size)
{
// Enforce non-negative size
this->size = _size.Abs();
}
/// \brief Set the box pose.
/// \param[in] _pose Box pose.
public: void Pose(Pose3<T> &_pose)
{
this->pose = _pose;
}
/// \brief Assignment operator. Set this box to the parameter
/// \param[in] _b OrientedBox to copy
/// \return The new box.
public: OrientedBox &operator=(const OrientedBox<T> &_b)
{
this->size = _b.size;
this->pose = _b.pose;
return *this;
}
/// \brief Equality test operator
/// \param[in] _b OrientedBox to test
/// \return True if equal
public: bool operator==(const OrientedBox<T> &_b) const
{
return this->size == _b.size && this->pose == _b.pose;
}
/// \brief Inequality test operator
/// \param[in] _b OrientedBox to test
/// \return True if not equal
public: bool operator!=(const OrientedBox<T> &_b) const
{
return this->size != _b.size || this->pose != _b.pose;
}
/// \brief Output operator
/// \param[in] _out Output stream
/// \param[in] _b OrientedBox to output to the stream
/// \return The stream
public: friend std::ostream &operator<<(std::ostream &_out,
const OrientedBox<T> &_b)
{
_out << "Size[" << _b.Size() << "] Pose[" << _b.Pose() << "]";
return _out;
}
/// \brief Check if a point lies inside the box.
/// \param[in] _p Point to check.
/// \return True if the point is inside the box.
public: bool Contains(const Vector3d &_p) const
{
// Move point to box frame
auto t = Matrix4<T>(this->pose).Inverse();
auto p = t *_p;
return p.X() >= -this->size.X()*0.5 && p.X() <= this->size.X()*0.5 &&
p.Y() >= -this->size.Y()*0.5 && p.Y() <= this->size.Y()*0.5 &&
p.Z() >= -this->size.Z()*0.5 && p.Z() <= this->size.Z()*0.5;
}
/// \brief The size of the box in its local frame.
private: Vector3<T> size;
/// \brief The pose of the center of the box.
private: Pose3<T> pose;
};
typedef OrientedBox<int> OrientedBoxi;
typedef OrientedBox<double> OrientedBoxd;
typedef OrientedBox<float> OrientedBoxf;
}
}
#endif

228
ign-math/include/ignition/math/PID.hh Normal file
View File

@ -0,0 +1,228 @@
/*
* 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_PID_HH_
#define IGNITION_MATH_PID_HH_
#include <chrono>
#include <ignition/math/Helpers.hh>
namespace ignition
{
namespace math
{
/// \class PID PID.hh ignition/math/PID.hh
/// \brief Generic PID controller class.
/// Generic proportional-integral-derivative controller class that
/// keeps track of PID-error states and control inputs given
/// the state of a system and a user specified target state.
/// It includes a user-adjustable command offset term (feed-forward).
class IGNITION_VISIBLE PID
{
/// \brief Constructor, zeros out Pid values when created and
/// initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2].
///
/// Disable command clamping by setting _cmdMin to a value larger
/// than _cmdMax. Command clamping is disabled by default.
///
/// Disable integral clamping by setting _iMin to a value larger
/// than _iMax. Integral clamping is disabled by default.
///
/// \param[in] _p The proportional gain.
/// \param[in] _i The integral gain.
/// \param[in] _d The derivative gain.
/// \param[in] _imax The integral upper limit.
/// \param[in] _imin The integral lower limit.
/// \param[in] _cmdMax Output max value.
/// \param[in] _cmdMin Output min value.
/// \param[in] _cmdOffset Command offset (feed-forward).
public: PID(const double _p = 0.0,
const double _i = 0.0,
const double _d = 0.0,
const double _imax = -1.0,
const double _imin = 0.0,
const double _cmdMax = -1.0,
const double _cmdMin = 0.0,
const double _cmdOffset = 0.0);
/// \brief Destructor
public: ~PID() = default;
/// \brief Initialize PID-gains and integral term
/// limits:[iMax:iMin]-[I1:I2].
///
/// Disable command clamping by setting _cmdMin to a value larger
/// than _cmdMax. Command clamping is disabled by default.
///
/// Disable integral clamping by setting _iMin to a value larger
/// than _iMax. Integral clamping is disabled by default.
///
/// \param[in] _p The proportional gain.
/// \param[in] _i The integral gain.
/// \param[in] _d The derivative gain.
/// \param[in] _imax The integral upper limit.
/// \param[in] _imin The integral lower limit.
/// \param[in] _cmdMax Output max value.
/// \param[in] _cmdMin Output min value.
/// \param[in] _cmdOffset Command offset (feed-forward).
public: void Init(const double _p = 0.0,
const double _i = 0.0,
const double _d = 0.0,
const double _imax = -1.0,
const double _imin = 0.0,
const double _cmdMax = -1.0,
const double _cmdMin = 0.0,
const double _cmdOffset = 0.0);
/// \brief Set the proportional Gain.
/// \param[in] _p proportional gain value
public: void SetPGain(const double _p);
/// \brief Set the integral Gain.
/// \param[in] _i integral gain value
public: void SetIGain(const double _i);
/// \brief Set the derivtive Gain.
/// \param[in] _d derivative gain value
public: void SetDGain(const double _d);
/// \brief Set the integral upper limit.
/// \param[in] _i integral upper limit value
public: void SetIMax(const double _i);
/// \brief Set the integral lower limit.
/// \param[in] _i integral lower limit value
public: void SetIMin(const double _i);
/// \brief Set the maximum value for the command.
/// \param[in] _c The maximum value
public: void SetCmdMax(const double _c);
/// \brief Set the minimum value for the command.
/// \param[in] _c The minimum value
public: void SetCmdMin(const double _c);
/// \brief Set the offset value for the command,
/// which is added to the result of the PID controller.
/// \param[in] _c The offset value
public: void SetCmdOffset(const double _c);
/// \brief Get the proportional Gain.
/// \return The proportional gain value
public: double PGain() const;
/// \brief Get the integral Gain.
/// \return The integral gain value
public: double IGain() const;
/// \brief Get the derivative Gain.
/// \return The derivative gain value
public: double DGain() const;
/// \brief Get the integral upper limit.
/// \return The integral upper limit value
public: double IMax() const;
/// \brief Get the integral lower limit.
/// \return The integral lower limit value
public: double IMin() const;
/// \brief Get the maximum value for the command.
/// \return The maximum value
public: double CmdMax() const;
/// \brief Get the maximum value for the command.
/// \return The maximum value
public: double CmdMin() const;
/// \brief Get the offset value for the command.
/// \return The offset value
public: double CmdOffset() const;
/// \brief Update the Pid loop with nonuniform time step size.
/// \param[in] _error Error since last call (p_state - p_target).
/// \param[in] _dt Change in time since last update call.
/// Normally, this is called at every time step,
/// The return value is an updated command to be passed
/// to the object being controlled.
/// \return the command value
public: double Update(const double _error,
const std::chrono::duration<double> &_dt);
/// \brief Set current target command for this PID controller.
/// \param[in] _cmd New command
public: void SetCmd(const double _cmd);
/// \brief Return current command for this PID controller.
/// \return the command value
public: double Cmd() const;
/// \brief Return PID error terms for the controller.
/// \param[in] _pe The proportional error.
/// \param[in] _ie The integral of gain times error.
/// \param[in] _de The derivative error.
public: void Errors(double &_pe, double &_ie, double &_de) const;
/// \brief Assignment operator
/// \param[in] _p a reference to a PID to assign values from
/// \return reference to this instance
public: PID &operator=(const PID &_p);
/// \brief Reset the errors and command.
public: void Reset();
/// \brief Error at a previous step.
private: double pErrLast = 0.0;
/// \brief Current error.
private: double pErr = 0.0;
/// \brief Integral of gain times error.
private: double iErr = 0.0;
/// \brief Derivative error.
private: double dErr = 0.0;
/// \brief Gain for proportional control.
private: double pGain;
/// \brief Gain for integral control.
private: double iGain = 0.0;
/// \brief Gain for derivative control.
private: double dGain = 0.0;
/// \brief Maximum clamping value for integral term.
private: double iMax = -1.0;
/// \brief Minim clamping value for integral term.
private: double iMin = 0.0;
/// \brief Command value.
private: double cmd = 0.0;
/// \brief Max command clamping value.
private: double cmdMax = -1.0;
/// \brief Min command clamping value.
private: double cmdMin = 0.0;
/// \brief Command offset.
private: double cmdOffset = 0.0;
};
}
}
#endif

232
ign-math/include/ignition/math/Plane.hh Normal file
View File

@ -0,0 +1,232 @@
/*
* Copyright (C) 2012-2014 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_PLANE_HH_
#define IGNITION_MATH_PLANE_HH_
#include <ignition/math/Box.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Vector2.hh>
namespace ignition
{
namespace math
{
/// \class Plane Plane.hh ignition/math/Plane.hh
/// \brief A plane and related functions.
template<typename T>
class Plane
{
/// \brief Enum used to indicate a side of the plane, no side, or both
/// sides for entities on the plane.
/// \sa Side
public: enum PlaneSide
{
/// \brief Negative side of the plane. This is the side that is
/// opposite the normal.
NEGATIVE_SIDE = 0,
/// \brief Positive side of the plane. This is the side that has the
/// normal vector.
POSITIVE_SIDE = 1,
/// \brief On the plane.
NO_SIDE = 2,
/// \brief On both sides of the plane.
BOTH_SIDE = 3
};
/// \brief Constructor
public: Plane()
: 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)
{
}
/// \brief Constructor
/// \param[in] _normal The plane normal
/// \param[in] _size Size of the plane
/// \param[in] _offset Offset along the normal
public: Plane(const Vector3<T> &_normal, const Vector2<T> &_size,
T _offset)
{
this->Set(_normal, _size, _offset);
}
/// \brief Destructor
public: virtual ~Plane() {}
/// \brief Set the plane
/// \param[in] _normal The plane normal
/// \param[in] _offset Offset along the normal
public: void Set(const Vector3<T> &_normal, T _offset)
{
this->normal = _normal;
this->d = _offset;
}
/// \brief Set the plane
/// \param[in] _normal The plane normal
/// \param[in] _size Size of the plane
/// \param[in] _offset Offset along the normal
public: void Set(const Vector3<T> &_normal, const Vector2<T> &_size,
T _offset)
{
this->normal = _normal;
this->size = _size;
this->d = _offset;
}
/// \brief The distance to the plane from the given point. The
/// distance can be negative, which indicates the point is on the
/// negative side of the plane.
/// \param[in] _point 3D point to calculate distance from.
/// \return Distance from the point to the plane.
/// \sa Side
public: T Distance(const Vector3<T> &_point) const
{
return this->normal.Dot(_point) - this->d;
}
/// \brief The side of the plane a point is on.
/// \param[in] _point The 3D point to check.
/// \return Plane::NEGATIVE_SIDE if the distance from the point to the
/// plane is negative, Plane::POSITIVE_SIDE if the distance from the
/// point to the plane is positive, or Plane::NO_SIDE if the
/// point is on the plane.
public: PlaneSide Side(const Vector3<T> &_point) const
{
T dist = this->Distance(_point);
if (dist < 0.0)
return NEGATIVE_SIDE;
if (dist > 0.0)
return POSITIVE_SIDE;
return NO_SIDE;
}
/// \brief The side of the plane a box is on.
/// \param[in] _box The 3D box to check.
/// \return Plane::NEGATIVE_SIDE if the distance from the box to the
/// plane is negative, Plane::POSITIVE_SIDE if the distance from the
/// box to the plane is positive, or Plane::BOTH_SIDE if the
/// box is on the plane.
public: PlaneSide Side(const math::Box &_box) const
{
double dist = this->Distance(_box.Center());
double maxAbsDist = this->normal.AbsDot(_box.Size()/2.0);
if (dist < -maxAbsDist)
return NEGATIVE_SIDE;
if (dist > maxAbsDist)
return POSITIVE_SIDE;
return BOTH_SIDE;
}
/// \brief Get distance to the plane give an origin and direction
/// \param[in] _origin the origin
/// \param[in] _dir a direction
/// \return the shortest distance
public: T Distance(const Vector3<T> &_origin,
const Vector3<T> &_dir) const
{
T denom = this->normal.Dot(_dir);
if (std::abs(denom) < 1e-3)
{
// parallel
return 0;
}
else
{
T nom = _origin.Dot(this->normal) - this->d;
T t = -(nom/denom);
return t;
}
}
/// \brief Get the plane size
public: inline const Vector2<T> &Size() const
{
return this->size;
}
/// \brief Get the plane size
public: inline Vector2<T> &Size()
{
return this->size;
}
/// \brief Get the plane offset
public: inline const Vector3<T> &Normal() const
{
return this->normal;
}
/// \brief Get the plane offset
public: inline Vector3<T> &Normal()
{
return this->normal;
}
/// \brief Get the plane offset
public: inline T Offset() const
{
return this->d;
}
/// \brief Equal operator
/// \param _p another plane
/// \return itself
public: Plane<T> &operator=(const Plane<T> &_p)
{
this->normal = _p.normal;
this->size = _p.size;
this->d = _p.d;
return *this;
}
/// \brief Plane normal
private: Vector3<T> normal;
/// \brief Plane size
private: Vector2<T> size;
/// \brief Plane offset
private: T d;
};
typedef Plane<int> Planei;
typedef Plane<double> Planed;
typedef Plane<float> Planef;
}
}
#endif

411
ign-math/include/ignition/math/Pose3.hh Normal file
View File

@ -0,0 +1,411 @@
/*
* Copyright (C) 2012-2014 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_POSE_HH_
#define IGNITION_MATH_POSE_HH_
#include <ignition/math/Quaternion.hh>
#include <ignition/math/Vector3.hh>
namespace ignition
{
namespace math
{
/// \class Pose3 Pose3.hh ignition/math/Pose3.hh
/// \brief Encapsulates a position and rotation in three space
template<typename T>
class Pose3
{
/// \brief math::Pose3<T>(0, 0, 0, 0, 0, 0)
public: static const Pose3<T> Zero;
/// \brief Default constructors
public: Pose3() : p(0, 0, 0), q(1, 0, 0, 0)
{
}
/// \brief Constructor
/// \param[in] _pos A position
/// \param[in] _rot A rotation
public: Pose3(const Vector3<T> &_pos, const Quaternion<T> &_rot)
: p(_pos), q(_rot)
{
}
/// \brief Constructor
/// \param[in] _x x position in meters.
/// \param[in] _y y position in meters.
/// \param[in] _z z position in meters.
/// \param[in] _roll Roll (rotation about X-axis) in radians.
/// \param[in] _pitch Pitch (rotation about y-axis) in radians.
/// \param[in] _yaw Yaw (rotation about z-axis) in radians.
public: Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
: p(_x, _y, _z), q(_roll, _pitch, _yaw)
{
}
/// \brief Constructor
/// \param[in] _x x position in meters.
/// \param[in] _y y position in meters.
/// \param[in] _z z position in meters.
/// \param[in] _qw Quaternion w value.
/// \param[in] _qx Quaternion x value.
/// \param[in] _qy Quaternion y value.
/// \param[in] _qz Quaternion z value.
public: Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
: p(_x, _y, _z), q(_qw, _qx, _qy, _qz)
{
}
/// \brief Copy constructor
/// \param[in] _pose Pose3<T> to copy
public: Pose3(const Pose3<T> &_pose)
: p(_pose.p), q(_pose.q)
{
}
/// \brief Destructor
public: virtual ~Pose3()
{
}
/// \brief Set the pose from a Vector3 and a Quaternion<T>
/// \param[in] _pos The position.
/// \param[in] _rot The rotation.
public: void Set(const Vector3<T> &_pos, const Quaternion<T> &_rot)
{
this->p = _pos;
this->q = _rot;
}
/// \brief Set the pose from pos and rpy vectors
/// \param[in] _pos The position.
/// \param[in] _rpy The rotation expressed as Euler angles.
public: void Set(const Vector3<T> &_pos, const Vector3<T> &_rpy)
{
this->p = _pos;
this->q.Euler(_rpy);
}
/// \brief Set the pose from a six tuple.
/// \param[in] _x x position in meters.
/// \param[in] _y y position in meters.
/// \param[in] _z z position in meters.
/// \param[in] _roll Roll (rotation about X-axis) in radians.
/// \param[in] _pitch Pitch (rotation about y-axis) in radians.
/// \param[in] _yaw Pitch (rotation about z-axis) in radians.
public: void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
{
this->p.Set(_x, _y, _z);
this->q.Euler(math::Vector3<T>(_roll, _pitch, _yaw));
}
/// \brief See if a pose is finite (e.g., not nan)
public: bool IsFinite() const
{
return this->p.IsFinite() && this->q.IsFinite();
}
/// \brief Fix any nan values
public: inline void Correct()
{
this->p.Correct();
this->q.Correct();
}
/// \brief Get the inverse of this pose
/// \return the inverse pose
public: Pose3<T> Inverse() const
{
Quaternion<T> inv = this->q.Inverse();
return Pose3<T>(inv * (this->p*-1), inv);
}
/// \brief Addition operator
/// A is the transform from O to P specified in frame O
/// B is the transform from P to Q specified in frame P
/// then, B + A is the transform from O to Q specified in frame O
/// \param[in] _pose Pose3<T> to add to this pose
/// \return The resulting pose
public: Pose3<T> operator+(const Pose3<T> &_pose) const
{
Pose3<T> result;
result.p = this->CoordPositionAdd(_pose);
result.q = this->CoordRotationAdd(_pose.q);
return result;
}
/// \brief Add-Equals operator
/// \param[in] _pose Pose3<T> to add to this pose
/// \return The resulting pose
public: const Pose3<T> &operator+=(const Pose3<T> &_pose)
{
this->p = this->CoordPositionAdd(_pose);
this->q = this->CoordRotationAdd(_pose.q);
return *this;
}
/// \brief Negation operator
/// A is the transform from O to P in frame O
/// then -A is transform from P to O specified in frame P
/// \return The resulting pose
public: inline Pose3<T> operator-() const
{
return Pose3<T>() - *this;
}
/// \brief Subtraction operator
/// A is the transform from O to P in frame O
/// B is the transform from O to Q in frame O
/// B - A is the transform from P to Q in frame P
/// \param[in] _pose Pose3<T> to subtract from this one
/// \return The resulting pose
public: inline Pose3<T> operator-(const Pose3<T> &_pose) const
{
return Pose3<T>(this->CoordPositionSub(_pose),
this->CoordRotationSub(_pose.q));
}
/// \brief Subtraction operator
/// \param[in] _pose Pose3<T> to subtract from this one
/// \return The resulting pose
public: const Pose3<T> &operator-=(const Pose3<T> &_pose)
{
this->p = this->CoordPositionSub(_pose);
this->q = this->CoordRotationSub(_pose.q);
return *this;
}
/// \brief Equality operator
/// \param[in] _pose Pose3<T> for comparison
/// \return True if equal
public: bool operator==(const Pose3<T> &_pose) const
{
return this->p == _pose.p && this->q == _pose.q;
}
/// \brief Inequality operator
/// \param[in] _pose Pose3<T> for comparison
/// \return True if not equal
public: bool operator!=(const Pose3<T> &_pose) const
{
return this->p != _pose.p || this->q != _pose.q;
}
/// \brief Multiplication operator
/// \param[in] _pose the other pose
/// \return itself
public: Pose3<T> operator*(const Pose3<T> &_pose)
{
return Pose3<T>(this->CoordPositionAdd(_pose), _pose.q * this->q);
}
/// \brief Equal operator
/// \param[in] _pose Pose3<T> to copy
public: Pose3<T> &operator=(const Pose3<T> &_pose)
{
this->p = _pose.p;
this->q = _pose.q;
return *this;
}
/// \brief Add one point to a vector: result = this + pos
/// \param[in] _pos Position to add to this pose
/// \return the resulting position
public: Vector3<T> CoordPositionAdd(const Vector3<T> &_pos) const
{
Quaternion<T> tmp(0.0, _pos.X(), _pos.Y(), _pos.Z());
// result = pose.q + pose.q * this->p * pose.q!
tmp = this->q * (tmp * this->q.Inverse());
return Vector3<T>(this->p.X() + tmp.X(),
this->p.Y() + tmp.Y(),
this->p.Z() + tmp.Z());
}
/// \brief Add one point to another: result = this + pose
/// \param[in] _pose The Pose3<T> to add
/// \return The resulting position
public: Vector3<T> CoordPositionAdd(const Pose3<T> &_pose) const
{
Quaternion<T> tmp(static_cast<T>(0),
this->p.X(), this->p.Y(), this->p.Z());
// result = _pose.q + _pose.q * this->p * _pose.q!
tmp = _pose.q * (tmp * _pose.q.Inverse());
return Vector3<T>(_pose.p.X() + tmp.X(),
_pose.p.Y() + tmp.Y(),
_pose.p.Z() + tmp.Z());
}
/// \brief Subtract one position from another: result = this - pose
/// \param[in] _pose Pose3<T> to subtract
/// \return The resulting position
public: inline Vector3<T> CoordPositionSub(const Pose3<T> &_pose) const
{
Quaternion<T> tmp(0,
this->p.X() - _pose.p.X(),
this->p.Y() - _pose.p.Y(),
this->p.Z() - _pose.p.Z());
tmp = _pose.q.Inverse() * (tmp * _pose.q);
return Vector3<T>(tmp.X(), tmp.Y(), tmp.Z());
}
/// \brief Add one rotation to another: result = this->q + rot
/// \param[in] _rot Rotation to add
/// \return The resulting rotation
public: Quaternion<T> CoordRotationAdd(const Quaternion<T> &_rot) const
{
return Quaternion<T>(_rot * this->q);
}
/// \brief Subtract one rotation from another: result = this->q - rot
/// \param[in] _rot The rotation to subtract
/// \return The resulting rotation
public: inline Quaternion<T> CoordRotationSub(
const Quaternion<T> &_rot) const
{
Quaternion<T> result(_rot.Inverse() * this->q);
result.Normalize();
return result;
}
/// \brief Find the inverse of a pose; i.e., if b = this + a, given b and
/// this, find a
/// \param[in] _b the other pose
public: Pose3<T> CoordPoseSolve(const Pose3<T> &_b) const
{
Quaternion<T> qt;
Pose3<T> a;
a.q = this->q.Inverse() * _b.q;
qt = a.q * Quaternion<T>(0, this->p.X(), this->p.Y(), this->p.Z());
qt = qt * a.q.Inverse();
a.p = _b.p - Vector3<T>(qt.X(), qt.Y(), qt.Z());
return a;
}
/// \brief Reset the pose
public: void Reset()
{
// set the position to zero
this->p.Set();
this->q = Quaterniond::Identity;
}
/// \brief Rotate vector part of a pose about the origin
/// \param[in] _rot rotation
/// \return the rotated pose
public: Pose3<T> RotatePositionAboutOrigin(const Quaternion<T> &_q) const
{
Pose3<T> a = *this;
a.p.X((1.0 - 2.0*_q.Y()*_q.Y() - 2.0*_q.Z()*_q.Z()) * this->p.X()
+(2.0*(_q.X()*_q.Y()+_q.W()*_q.Z())) * this->p.Y()
+(2.0*(_q.X()*_q.Z()-_q.W()*_q.Y())) * this->p.Z());
a.p.Y((2.0*(_q.X()*_q.Y()-_q.W()*_q.Z())) * this->p.X()
+(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Z()*_q.Z()) * this->p.Y()
+(2.0*(_q.Y()*_q.Z()+_q.W()*_q.X())) * this->p.Z());
a.p.Z((2.0*(_q.X()*_q.Z()+_q.W()*_q.Y())) * this->p.X()
+(2.0*(_q.Y()*_q.Z()-_q.W()*_q.X())) * this->p.Y()
+(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Y()*_q.Y()) * this->p.Z());
return a;
}
/// \brief Round all values to _precision decimal places
/// \param[in] _precision
public: void Round(int _precision)
{
this->q.Round(_precision);
this->p.Round(_precision);
}
/// \brief Get the position.
/// \return Origin of the pose.
public: inline const Vector3<T> &Pos() const
{
return this->p;
}
/// \brief Get a mutable reference to the position.
/// \return Origin of the pose.
public: inline Vector3<T> &Pos()
{
return this->p;
}
/// \brief Get the rotation.
/// \return Quaternion representation of the rotation.
public: inline const Quaternion<T> &Rot() const
{
return this->q;
}
/// \brief Get a mutuable reference to the rotation.
/// \return Quaternion representation of the rotation.
public: inline Quaternion<T> &Rot()
{
return this->q;
}
/// \brief Stream insertion operator
/// \param[in] _out output stream
/// \param[in] _pose pose to output
/// \return the stream
public: friend std::ostream &operator<<(
std::ostream &_out, const ignition::math::Pose3<T> &_pose)
{
_out << _pose.Pos() << " " << _pose.Rot();
return _out;
}
/// \brief Stream extraction operator
/// \param[in] _in the input stream
/// \param[in] _pose the pose
/// \return the stream
public: friend std::istream &operator>>(
std::istream &_in, ignition::math::Pose3<T> &_pose)
{
// Skip white spaces
_in.setf(std::ios_base::skipws);
Vector3<T> pos;
Quaternion<T> rot;
_in >> pos >> rot;
_pose.Set(pos, rot);
return _in;
}
/// \brief The position
private: Vector3<T> p;
/// \brief The rotation
private: Quaternion<T> q;
};
template<typename T> const Pose3<T> Pose3<T>::Zero(0, 0, 0, 0, 0, 0);
typedef Pose3<int> Pose3i;
typedef Pose3<double> Pose3d;
typedef Pose3<float> Pose3f;
}
}
#endif

1074
ign-math/include/ignition/math/Quaternion.hh Normal file
View File

File diff suppressed because it is too large Load Diff

94
ign-math/include/ignition/math/Rand.hh Normal file
View File

@ -0,0 +1,94 @@
/*
* Copyright (C) 2012-2014 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_RAND_HH_
#define IGNITION_MATH_RAND_HH_
#include <random>
#include <cmath>
#include <cstdint>
#include <memory>
#include <ignition/math/Helpers.hh>
namespace ignition
{
namespace math
{
/// \def GeneratorType
/// \brief std::mt19937
typedef std::mt19937 GeneratorType;
/// \def UniformRealDist
/// \brief std::uniform_real_distribution<double>
typedef std::uniform_real_distribution<double> UniformRealDist;
/// \def NormalRealDist
/// \brief std::normal_distribution<double>
typedef std::normal_distribution<double> NormalRealDist;
/// \def UniformIntDist
/// \brief std::uniform_int<int>
typedef std::uniform_int_distribution<int32_t> UniformIntDist;
/// \class Rand Rand.hh ignition/math/Rand.hh
/// \brief Random number generator class
class IGNITION_VISIBLE Rand
{
/// \brief Set the seed value.
/// \param[in] _seed The seed used to initialize the randon number
/// generator.
public: static void Seed(unsigned int _seed);
/// \brief Get the seed value.
/// \return The seed value used to initialize the random number
/// generator.
public: static unsigned int Seed();
/// \brief Get a double from a uniform distribution
/// \param[in] _min Minimum bound for the random number
/// \param[in] _max Maximum bound for the random number
public: static double DblUniform(double _min = 0, double _max = 1);
/// \brief Get a double from a normal distribution
/// \param[in] _mean Mean value for the distribution
/// \param[in] _sigma Sigma value for the distribution
public: static double DblNormal(double _mean = 0, double _sigma = 1);
/// \brief Get a integer from a uniform distribution
/// \param[in] _min Minimum bound for the random number
/// \param[in] _max Maximum bound for the random number
public: static int32_t IntUniform(int _min, int _max);
/// \brief Get a double from a normal distribution
/// \param[in] _mean Mean value for the distribution
/// \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 std::unique_ptr<GeneratorType> randGenerator;
#ifdef _WIN32
#pragma warning(pop)
#endif
/// \brief Random number seed.
private: static uint32_t seed;
};
}
}
#endif

116
ign-math/include/ignition/math/RotationSpline.hh Normal file
View File

@ -0,0 +1,116 @@
/*
* Copyright (C) 2012-2014 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_ROTATIONSPLINE_HH_
#define IGNITION_MATH_ROTATIONSPLINE_HH_
#include <ignition/math/IndexException.hh>
#include <ignition/math/Quaternion.hh>
namespace ignition
{
namespace math
{
// Forward declare private data
class RotationSplinePrivate;
/// \class RotationSpline RotationSpline.hh ignition/math/RotationSpline.hh
/// \brief Spline for rotations
class IGNITION_VISIBLE RotationSpline
{
/// \brief Constructor. Sets the autoCalc to true
public: RotationSpline();
/// \brief Destructor. Nothing is done
public: ~RotationSpline();
/// \brief Adds a control point to the end of the spline.
/// \param[in] _p control point
public: void AddPoint(const Quaterniond &_p);
/// \brief Gets the detail of one of the control points of the spline.
/// \param[in] _index the index of the control point.
/// \remarks This point must already exist in the spline.
/// \throws IndexException if _index >= PointCount()
/// \return a quaternion (out of bound index result in assertion)
public: const Quaterniond &Point(unsigned int _index) const;
/// \brief Gets the number of control points in the spline.
/// \return the count
public: unsigned int PointCount() const;
/// \brief Clears all the points in the spline.
public: void Clear();
/// \brief Updates a single point in the spline.
/// \remarks This point must already exist in the spline.
/// \param[in] _index index
/// \param[in] _value the new control point value
/// \throws IndexException if _index >= PointCount()
public: void UpdatePoint(unsigned int _index, const Quaterniond &_value);
/// \brief Returns an interpolated point based on a parametric
/// value over the whole series.
/// \remarks Given a t value between 0 and 1 representing the
/// parametric distance along the whole length of the spline,
/// this method returns an interpolated point.
/// \param[in] _t Parametric value.
/// \param[in] _useShortestPath Defines if rotation should take the
/// shortest possible path
/// \return the rotation
public: Quaterniond Interpolate(double _t, bool _useShortestPath = true);
/// \brief Interpolates a single segment of the spline
/// given a parametric value.
/// \param[in] _fromIndex The point index to treat as t = 0.
/// _fromIndex + 1 is deemed to be t = 1
/// \param[in] _t Parametric value
/// \param[in] _useShortestPath Defines if rotation should take the
/// shortest possible path
/// \throws IndexException if _fromIndex >= PointCount()
/// \return the rotation
public: Quaterniond Interpolate(unsigned int _fromIndex, double _t,
bool _useShortestPath = true);
/// \brief Tells the spline whether it should automatically calculate
/// tangents on demand as points are added.
/// \remarks The spline calculates tangents at each point automatically
/// based on the input points. Normally it does this every
/// time a point changes. However, if you have a lot of points
/// to add in one go, you probably don't want to incur this
/// overhead and would prefer to defer the calculation until
/// you are finished setting all the points. You can do this
/// by calling this method with a parameter of 'false'. Just
/// remember to manually call the recalcTangents method when
/// you are done.
/// \param[in] _autoCalc If true, tangents are calculated for you
/// whenever a point changes. If false, you must call reclacTangents to
/// recalculate them when it best suits.
public: void AutoCalculate(bool _autoCalc);
/// \brief Recalculates the tangents associated with this spline.
/// \remarks If you tell the spline not to update on demand by calling
/// setAutoCalculate(false) then you must call this after
/// completing your updates to the spline points.
public: void RecalcTangents();
/// \brief Private data pointer
private: RotationSplinePrivate *dataPtr;
};
}
}
#endif

46
ign-math/include/ignition/math/RotationSplinePrivate.hh Normal file
View File

@ -0,0 +1,46 @@
/*
* 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_MATH_ROTATIONSPLINE_PRIVATE_HH_
#define IGNITION_MATH_ROTATIONSPLINE_PRIVATE_HH_
#include <vector>
#include "ignition/math/Quaternion.hh"
namespace ignition
{
namespace math
{
/// \internal
/// \brief Private data for RotationSpline
class RotationSplinePrivate
{
/// \brief Constructor
public: RotationSplinePrivate();
/// \brief Automatic recalculation of tangents when control points are
/// updated
public: bool autoCalc;
/// \brief the control points
public: std::vector<Quaterniond> points;
/// \brief the tangents
public: std::vector<Quaterniond> tangents;
};
}
}
#endif

160
ign-math/include/ignition/math/SemanticVersion.hh Normal file
View File

@ -0,0 +1,160 @@
/*
* 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_SEMANTICVERSION_HH_
#define IGNITION_MATH_SEMANTICVERSION_HH_
#include <memory>
#include <string>
#include <ignition/math/Helpers.hh>
namespace ignition
{
namespace math
{
// Forward declare private data class
class SemanticVersionPrivate;
/// \class SemanticVersion SemanticVersion.hh
/// ignition/math/SemanticVersion.hh
/// \brief Version comparison class based on Semantic Versioning 2.0.0
/// http://semver.org/
/// Compares versions and converts versions from string.
class IGNITION_VISIBLE SemanticVersion
{
/// \brief Default constructor. Use the Parse function to populate
/// an instance with version information.
public: SemanticVersion();
/// \brief Constructor
/// \param[in] _v the string version. ex: "0.3.2"
// cppcheck-suppress noExplicitConstructor
public: SemanticVersion(const std::string &_v);
/// \brief Copy constructor
/// \param[in] _copy the other version
public: SemanticVersion(const SemanticVersion &_copy);
/// \brief Assignment operator
/// \param[in] _other The version to assign from.
/// \return The reference to this instance
public: SemanticVersion &operator=(const SemanticVersion &_other);
/// \brief Constructor
/// \param[in] _major The major number
/// \param[in] _minor The minor number
/// \param[in] _patch The patch number
/// \param[in] _prerelease The prerelease string
/// \param[in] _build The build metadata string
public: SemanticVersion(const unsigned int _major,
const unsigned int _minor = 0,
const unsigned int _patch = 0,
const std::string &_prerelease = "",
const std::string &_build = "");
/// \brief Destructor
public: ~SemanticVersion();
/// \brief Parse a version string and set the major, minor, patch
/// numbers, and prerelease and build strings.
/// \param[in] _versionStr The version string, such as "1.2.3-pr+123"
/// \retur True on success.
public: bool Parse(const std::string &_versionStr);
/// \brief Returns the version as a string
/// \return The semantic version string
public: std::string Version() const;
/// \brief Get the major number
/// \return The major number
public: unsigned int Major() const;
/// \brief Get the minor number
/// \return The minor number
public: unsigned int Minor() const;
/// \brief Get the patch number
/// \return The patch number
public: unsigned int Patch() const;
/// \brief Get the prerelease string.
/// \return Prelrease string, empty if a prerelease string was not
/// specified.
public: std::string Prerelease() const;
/// \brief Get the build metadata string. Build meta data is not used
/// when determining precedence.
/// \return Build metadata string, empty if a build metadata string was
/// not specified.
public: std::string Build() const;
/// \brief Less than comparison operator
/// \param[in] _other The other version to compare to
/// \return True if _other version is newer
public: bool operator<(const SemanticVersion &_other) const;
/// \brief Less than or equal comparison operator
/// \param[in] _other The other version to compare to
/// \return True if _other version is newer or equal
public: bool operator<=(const SemanticVersion &_other) const;
/// \brief Greater than comparison operator
/// \param[in] _other The other version to compare to
/// \return True if _other version is older
public: bool operator>(const SemanticVersion &_other) const;
/// \brief Greater than or equal comparison operator
/// \param[in] _other The other version to compare to
/// \return True if _other version is older or the same
public: bool operator>=(const SemanticVersion &_other) const;
/// \brief Equality comparison operator
/// \param[in] _other The other version to compare to
/// \return True if _other version is the same
public: bool operator==(const SemanticVersion &_other) const;
/// \brief Inequality comparison operator
/// \param[in] _other The other version to compare to
/// \return True if _other version is different
public: bool operator!=(const SemanticVersion &_other) const;
/// \brief Stream insertion operator
/// \param _out output stream
/// \param _v Semantic version to output
/// \return the stream
public: friend std::ostream &operator<<(std::ostream &_out,
const SemanticVersion &_v)
{
_out << _v.Version();
return _out;
}
#ifdef _WIN32
// Disable warning C4251 which is triggered by
// std::unique_ptr
#pragma warning(push)
#pragma warning(disable: 4251)
#endif
/// \brief Pointer to private data
private: std::unique_ptr<SemanticVersionPrivate> dataPtr;
#ifdef _WIN32
#pragma warning(pop)
#endif
};
}
}
#endif

231
ign-math/include/ignition/math/SignalStats.hh Normal file
View File

@ -0,0 +1,231 @@
/*
* 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_MATH_SIGNAL_STATS_HH_
#define IGNITION_MATH_SIGNAL_STATS_HH_
#include <map>
#include <string>
#include <ignition/math/Helpers.hh>
namespace ignition
{
namespace math
{
/// \brief Forward declare private data class.
class SignalStatisticPrivate;
/// \class SignalStatistic SignalStats.hh ignition/math/SignalStats.hh
/// \brief Statistical properties of a discrete time scalar signal.
class IGNITION_VISIBLE SignalStatistic
{
/// \brief Constructor
public: SignalStatistic();
/// \brief Destructor
public: virtual ~SignalStatistic();
/// \brief Get the current value of the statistical measure.
/// \return Current value of the statistical measure.
public: virtual double Value() const = 0;
/// \brief Get a short version of the name of this statistical measure.
/// \return Short name of the statistical measure.
public: virtual std::string ShortName() const = 0;
/// \brief Get number of data points in measurement.
/// \return Number of data points in measurement.
public: virtual size_t Count() const;
/// \brief Add a new sample to the statistical measure.
/// \param[in] _data New signal data point.
public: virtual void InsertData(const double _data) = 0;
/// \brief Forget all previous data.
public: virtual void Reset();
/// \brief Pointer to private data.
protected: SignalStatisticPrivate *dataPtr;
};
/// \}
/// \class SignalMaximum SignalStats.hh ignition/math/SignalStats.hh
/// \brief Computing the maximum value of a discretely sampled signal.
class IGNITION_VISIBLE SignalMaximum : public SignalStatistic
{
// Documentation inherited.
public: virtual double Value() const;
/// \brief Get a short version of the name of this statistical measure.
/// \return "max"
public: virtual std::string ShortName() const;
// Documentation inherited.
public: virtual void InsertData(const double _data);
};
/// \}
/// \class SignalMean SignalStats.hh ignition/math/SignalStats.hh
/// \brief Computing the mean value of a discretely sampled signal.
class IGNITION_VISIBLE SignalMean : public SignalStatistic
{
// Documentation inherited.
public: virtual double Value() const;
/// \brief Get a short version of the name of this statistical measure.
/// \return "mean"
public: virtual std::string ShortName() const;
// Documentation inherited.
public: virtual void InsertData(const double _data);
};
/// \}
/// \class SignalMinimum SignalStats.hh ignition/math/SignalStats.hh
/// \brief Computing the minimum value of a discretely sampled signal.
class IGNITION_VISIBLE SignalMinimum : public SignalStatistic
{
// Documentation inherited.
public: virtual double Value() const;
/// \brief Get a short version of the name of this statistical measure.
/// \return "min"
public: virtual std::string ShortName() const;
// Documentation inherited.
public: virtual void InsertData(const double _data);
};
/// \}
/// \class SignalRootMeanSquare SignalStats.hh ignition/math/SignalStats.hh
/// \brief Computing the square root of the mean squared value
/// of a discretely sampled signal.
class IGNITION_VISIBLE SignalRootMeanSquare : public SignalStatistic
{
// Documentation inherited.
public: virtual double Value() const;
/// \brief Get a short version of the name of this statistical measure.
/// \return "rms"
public: virtual std::string ShortName() const;
// Documentation inherited.
public: virtual void InsertData(const double _data);
};
/// \}
/// \class SignalMaxAbsoluteValue SignalStats.hh
/// ignition/math/SignalStats.hh
/// \brief Computing the maximum of the absolute value
/// of a discretely sampled signal.
/// Also known as the maximum norm, infinity norm, or supremum norm.
class IGNITION_VISIBLE SignalMaxAbsoluteValue : public SignalStatistic
{
// Documentation inherited.
public: virtual double Value() const;
/// \brief Get a short version of the name of this statistical measure.
/// \return "maxAbs"
public: virtual std::string ShortName() const;
// Documentation inherited.
public: virtual void InsertData(const double _data);
};
/// \}
/// \class SignalVariance SignalStats.hh ignition/math/SignalStats.hh
/// \brief Computing the incremental variance
/// of a discretely sampled signal.
class IGNITION_VISIBLE SignalVariance : public SignalStatistic
{
// Documentation inherited.
public: virtual double Value() const;
/// \brief Get a short version of the name of this statistical measure.
/// \return "var"
public: virtual std::string ShortName() const;
// Documentation inherited.
public: virtual void InsertData(const double _data);
};
/// \}
/// \brief Forward declare private data class.
class SignalStatsPrivate;
/// \class SignalStats SignalStats.hh ignition/math/SignalStats.hh
/// \brief Collection of statistics for a scalar signal.
class IGNITION_VISIBLE SignalStats
{
/// \brief Constructor
public: SignalStats();
/// \brief Destructor
public: ~SignalStats();
/// \brief Get number of data points in first statistic.
/// Technically you can have different numbers of data points
/// in each statistic if you call InsertStatistic after InsertData,
/// but this is not a recommended use case.
/// \return Number of data points in first statistic.
public: size_t Count() const;
/// \brief Get the current values of each statistical measure,
/// stored in a map using the short name as the key.
/// \return Map with short name of each statistic as key
/// and value of statistic as the value.
public: std::map<std::string, double> Map() const;
/// \brief Add a new sample to the statistical measures.
/// \param[in] _data New signal data point.
public: void InsertData(const double _data);
/// \brief Add a new type of statistic.
/// \param[in] _name Short name of new statistic.
/// Valid values include:
/// "maxAbs"
/// "mean"
/// "rms"
/// \return True if statistic was successfully added,
/// false if name was not recognized or had already
/// been inserted.
public: bool InsertStatistic(const std::string &_name);
/// \brief Add multiple statistics.
/// \param[in] _names Comma-separated list of new statistics.
/// For example, all statistics could be added with:
/// "maxAbs,mean,rms"
/// \return True if all statistics were successfully added,
/// false if any names were not recognized or had already
/// been inserted.
public: bool InsertStatistics(const std::string &_names);
/// \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;
};
/// \}
}
}
#endif

74
ign-math/include/ignition/math/SignalStatsPrivate.hh Normal file
View File

@ -0,0 +1,74 @@
/*
* 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_MATH_SIGNAL_STATS_PRIVATE_HH_
#define IGNITION_MATH_SIGNAL_STATS_PRIVATE_HH_
#include <memory>
#include <vector>
namespace ignition
{
namespace math
{
/// \brief Private data class for the SignalStatistic class.
class SignalStatisticPrivate
{
/// \brief Scalar representation of signal data.
public: double data;
/// \brief Scalar representation of extra signal data.
/// For example, the standard deviation statistic needs an extra variable.
public: double extraData;
/// \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;
/// \def SignalStatisticPtr
/// \brief Shared pointer to SignalStatistic object
typedef std::shared_ptr<SignalStatistic> SignalStatisticPtr;
/// \def SignalStatistic_V
/// \brief Vector of SignalStatisticPtr
typedef std::vector<SignalStatisticPtr> SignalStatistic_V;
/// \brief Private data class for the SignalStats class.
class SignalStatsPrivate
{
/// \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);
}
};
}
}
#endif

232
ign-math/include/ignition/math/SphericalCoordinates.hh Normal file
View File

@ -0,0 +1,232 @@
/*
* Copyright (C) 2012-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_SPHERICALCOORDINATES_HH_
#define IGNITION_MATH_SPHERICALCOORDINATES_HH_
#include <memory>
#include <string>
#include <ignition/math/Angle.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Helpers.hh>
namespace ignition
{
namespace math
{
class SphericalCoordinatesPrivate;
/// \class SphericalCoordinates SphericalCoordinates.hh commmon/common.hh
/// \brief Convert spherical coordinates for planetary surfaces.
class IGNITION_VISIBLE SphericalCoordinates
{
/// \enum SurfaceType
/// \brief Unique identifiers for planetary surface models.
public: enum SurfaceType
{
/// \brief Model of reference ellipsoid for earth, based on
/// WGS 84 standard. see wikipedia: World_Geodetic_System
EARTH_WGS84 = 1
};
/// \enum CoordinateType
/// \brief Unique identifiers for coordinate types.
public: enum CoordinateType
{
/// \brief Latitude, Longitude and Altitude by SurfaceType
SPHERICAL = 1,
/// \brief Earth centered, earth fixed Cartesian
ECEF = 2,
/// \brief Local tangent plane (East, North, Up)
GLOBAL = 3,
/// \brief Heading-adjusted tangent plane (X, Y, Z)
LOCAL = 4
};
/// \brief Constructor.
public: SphericalCoordinates();
/// \brief Constructor with surface type input.
/// \param[in] _type SurfaceType specification.
public: explicit SphericalCoordinates(const SurfaceType _type);
/// \brief Constructor with surface type, angle, and elevation inputs.
/// \param[in] _type SurfaceType specification.
/// \param[in] _latitude Reference latitude.
/// \param[in] _longitude Reference longitude.
/// \param[in] _elevation Reference elevation.
/// \param[in] _heading Heading offset.
public: SphericalCoordinates(const SurfaceType _type,
const ignition::math::Angle &_latitude,
const ignition::math::Angle &_longitude,
const double _elevation,
const ignition::math::Angle &_heading);
/// \brief Copy constructor.
/// \param[in] _sc Spherical coordinates to copy.
public: SphericalCoordinates(const SphericalCoordinates &_sc);
/// \brief Destructor.
public: ~SphericalCoordinates();
/// \brief Convert a Cartesian position vector to geodetic coordinates.
/// \param[in] _xyz Cartesian position vector in the world frame.
/// \return Cooordinates: geodetic latitude (deg), longitude (deg),
/// altitude above sea level (m).
public: ignition::math::Vector3d SphericalFromLocalPosition(
const ignition::math::Vector3d &_xyz) const;
/// \brief Convert a Cartesian velocity vector in the local frame
/// to a global Cartesian frame with components East, North, Up.
/// \param[in] _xyz Cartesian velocity vector in the world frame.
/// \return Rotated vector with components (x,y,z): (East, North, Up).
public: ignition::math::Vector3d GlobalFromLocalVelocity(
const ignition::math::Vector3d &_xyz) const;
/// \brief Convert a string to a SurfaceType.
/// Allowed values: ["EARTH_WGS84"].
/// \param[in] _str String to convert.
/// \return Conversion to SurfaceType.
public: static SurfaceType Convert(const std::string &_str);
/// \brief Get the distance between two points expressed in geographic
/// latitude and longitude. It assumes that both points are at sea level.
/// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents
/// the point with latitude 38d 0'6.00"N and longitude 123d 0'6.00"W.
/// \param[in] _latA Latitude of point A.
/// \param[in] _longA Longitude of point A.
/// \param[in] _latB Latitude of point B.
/// \param[in] _longB Longitude of point B.
/// \return Distance in meters.
public: static double Distance(const ignition::math::Angle &_latA,
const ignition::math::Angle &_lonA,
const ignition::math::Angle &_latB,
const ignition::math::Angle &_lonB);
/// \brief Get SurfaceType currently in use.
/// \return Current SurfaceType value.
public: SurfaceType Surface() const;
/// \brief Get reference geodetic latitude.
/// \return Reference geodetic latitude.
public: ignition::math::Angle LatitudeReference() const;
/// \brief Get reference longitude.
/// \return Reference longitude.
public: ignition::math::Angle LongitudeReference() const;
/// \brief Get reference elevation in meters.
/// \return Reference elevation.
public: double ElevationReference() const;
/// \brief Get heading offset for the reference frame, expressed as
/// angle from East to x-axis, or equivalently
/// from North to y-axis.
/// \return Heading offset of reference frame.
public: ignition::math::Angle HeadingOffset() const;
/// \brief Set SurfaceType for planetary surface model.
/// \param[in] _type SurfaceType value.
public: void SetSurface(const SurfaceType &_type);
/// \brief Set reference geodetic latitude.
/// \param[in] _angle Reference geodetic latitude.
public: void SetLatitudeReference(const ignition::math::Angle &_angle);
/// \brief Set reference longitude.
/// \param[in] _angle Reference longitude.
public: void SetLongitudeReference(const ignition::math::Angle &_angle);
/// \brief Set reference elevation above sea level in meters.
/// \param[in] _elevation Reference elevation.
public: void SetElevationReference(const double _elevation);
/// \brief Set heading angle offset for the frame.
/// \param[in] _angle Heading offset for the frame.
public: void SetHeadingOffset(const ignition::math::Angle &_angle);
/// \brief Convert a geodetic position vector to Cartesian coordinates.
/// \param[in] _xyz Geodetic position in the planetary frame of reference
/// \return Cartesian position vector in the world frame
public: ignition::math::Vector3d LocalFromSphericalPosition(
const ignition::math::Vector3d &_xyz) const;
/// \brief Convert a Cartesian velocity vector with components East,
/// North, Up to a local cartesian frame vector XYZ.
/// \param[in] Vector with components (x,y,z): (East, North, Up).
/// \return Cartesian vector in the world frame.
public: ignition::math::Vector3d LocalFromGlobalVelocity(
const ignition::math::Vector3d &_xyz) const;
/// \brief Update coordinate transformation matrix with reference location
public: void UpdateTransformationMatrix();
/// \brief Convert between positions in SPHERICAL/ECEF/LOCAL/GLOBAL frame
/// \param[in] _pos Position vector in frame defined by parameter _in
/// \param[in] _in CoordinateType for input
/// \param[in] _out CoordinateType for output
/// \return Transformed coordinate using cached orgin
public: ignition::math::Vector3d
PositionTransform(const ignition::math::Vector3d &_pos,
const CoordinateType &_in, const CoordinateType &_out) const;
/// \brief Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame
/// \param[in] _vel Velocity vector in frame defined by parameter _in
/// \param[in] _in CoordinateType for input
/// \param[in] _out CoordinateType for output
/// \return Transformed velocity vector
public: ignition::math::Vector3d VelocityTransform(
const ignition::math::Vector3d &_vel,
const CoordinateType &_in, const CoordinateType &_out) const;
/// \brief Equality operator, result = this == _sc
/// \param[in] _sc Spherical coordinates to check for equality
/// \return true if this == _sc
public: bool operator==(const SphericalCoordinates &_sc) const;
/// \brief Inequality
/// \param[in] _sc Spherical coordinates to check for inequality
/// \return true if this != _sc
public: bool operator!=(const SphericalCoordinates &_sc) const;
/// \brief Assignment operator
/// \param[in] _sc The spherical coordinates to copy from.
/// \return this
public: SphericalCoordinates &operator=(
const SphericalCoordinates &_sc);
#ifdef _WIN32
// Disable warning C4251 which is triggered by
// std::unique_ptr
#pragma warning(push)
#pragma warning(disable: 4251)
#endif
/// \internal
/// \brief Pointer to the private data
private: std::unique_ptr<SphericalCoordinatesPrivate> dataPtr;
#ifdef _WIN32
#pragma warning(pop)
#endif
};
/// \}
}
}
#endif

124
ign-math/include/ignition/math/Spline.hh Normal file
View File

@ -0,0 +1,124 @@
/*
* Copyright (C) 2012-2014 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.
*
*/
// Note: Originally cribbed from Ogre3d. Modified to implement Cardinal
// spline and catmull-rom spline
#ifndef IGNITION_MATH_SPLINE_HH_
#define IGNITION_MATH_SPLINE_HH_
#include <ignition/math/Helpers.hh>
#include <ignition/math/IndexException.hh>
#include <ignition/math/Vector3.hh>
namespace ignition
{
namespace math
{
// Forward declare private data class
class SplinePrivate;
/// \class Spline Spline.hh ignition/math/Spline.hh
/// \brief Splines
class IGNITION_VISIBLE Spline
{
/// \brief constructor
public: Spline();
/// \brief destructor
public: ~Spline();
/// \brief Set the tension parameter. A value of 0 = Catmull-Rom
/// spline.
/// \param[in] _t Tension value between 0.0 and 1.0
public: void Tension(double _t);
/// \brief Get the tension value
/// \return The value of the tension, which is between 0.0 and 1.0
public: double Tension() const;
/// \brief Adds a control point to the end of the spline.
/// \param[in] _pt point to add
public: void AddPoint(const Vector3d &_pt);
/// \brief Gets the detail of one of the control points of the spline.
/// \param[in] _index the control point index
/// \return the control point, or [0,0,0] and a message on the error
/// stream
/// \throws IndexException if _index >= PointCount()
public: Vector3d Point(unsigned int _index) const;
/// \brief Gets the number of control points in the spline.
/// \return the count
public: size_t PointCount() const;
/// \brief Get the tangent value for a point
/// \param[in] _index the control point index
/// \throws IndexException if _index >= PointCount()
public: Vector3d Tangent(unsigned int _index) const;
/// \brief Clears all the points in the spline.
public: void Clear();
/// \brief Updates a single point in the spline.
/// \remarks an error to the error stream is printed when the index is
/// out of bounds
/// \param[in] _index the control point index
/// \param[in] _value the new position
/// \throws IndexException if _index >= PointCount()
public: void UpdatePoint(unsigned int _index, const Vector3d &_value);
/// \brief Returns an interpolated point based on a parametric value
/// over the whole series.
/// \param[in] _t parameter (range 0 to 1)
public: Vector3d Interpolate(double _t) const;
/// \brief Interpolates a single segment of the spline given a
/// parametric value.
/// \param[in] _fromIndex The point index to treat as t = 0.
/// fromIndex + 1 is deemed to be t = 1
/// \param[in] _t Parametric value
/// \throws IndexException if _fromIndex >= PointCount()
public: Vector3d Interpolate(unsigned int _fromIndex, double _t) const;
/// \brief Tells the spline whether it should automatically
/// calculate tangents on demand as points are added.
/// \remarks The spline calculates tangents at each point
/// automatically based on the input points. Normally it
/// does this every time a point changes. However, if you
/// have a lot of points to add in one go, you probably
/// don't want to incur this overhead and would prefer to
/// defer the calculation until you are finished setting all
/// the points. You can do this by calling this method with a
/// parameter of 'false'. Just remember to manually call the
/// recalcTangents method when you are done.
/// \param[in] _autoCalc If true, tangents are calculated for you whenever
/// a point changes. If false, you must call reclacTangents to
/// recalculate them when it best suits.
public: void AutoCalculate(bool _autoCalc);
/// \brief Recalculates the tangents associated with this spline.
/// \remarks If you tell the spline not to update on demand by
/// calling setAutoCalculate(false) then you must call this
/// after completing your updates to the spline points.
public: void RecalcTangents();
/// \internal
/// \brief Private data pointer
private: SplinePrivate *dataPtr;
};
}
}
#endif

49
ign-math/include/ignition/math/SplinePrivate.hh Normal file
View File

@ -0,0 +1,49 @@
/*
* 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_MATH_SPLINE_PRIVATE_HH_
#define IGNITION_MATH_SPLINE_PRIVATE_HH_
#include <vector>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Matrix4.hh>
namespace ignition
{
namespace math
{
class SplinePrivate
{
/// \brief when true, the tangents are recalculated when the control
/// point change
public: bool autoCalc;
/// \brief control points
public: std::vector<Vector3d> points;
/// \brief tangents
public: std::vector<Vector3d> tangents;
/// Matrix of coefficients
public: Matrix4d coeffs;
/// Tension of 0 = Catmull-Rom spline, otherwise a Cardinal spline
public: double tension;
};
}
}
#endif

375
ign-math/include/ignition/math/Temperature.hh Normal file
View File

@ -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

247
ign-math/include/ignition/math/Triangle.hh Normal file
View File

@ -0,0 +1,247 @@
/*
* Copyright (C) 2014 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_TRIANGLE_HH_
#define IGNITION_MATH_TRIANGLE_HH_
#include <set>
#include <ignition/math/Line2.hh>
#include <ignition/math/Vector2.hh>
#include <ignition/math/IndexException.hh>
namespace ignition
{
namespace math
{
/// \class Triangle Triangle.hh ignition/math/Triangle.hh
/// \brief Triangle class and related functions.
template<typename T>
class Triangle
{
/// \brief Default constructor
public: Triangle() = default;
/// \brief Constructor
/// \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: Triangle(const math::Vector2<T> &_pt1,
const math::Vector2<T> &_pt2,
const math::Vector2<T> &_pt3)
{
this->Set(_pt1, _pt2, _pt3);
}
/// \brief Set one vertex of the triangle.
/// \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(unsigned int _index, const math::Vector2<T> &_pt)
{
if (_index >2)
throw IndexException();
else
this->pts[_index] = _pt;
}
/// \brief Set all vertices of the triangle.
/// \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 math::Vector2<T> &_pt1,
const math::Vector2<T> &_pt2,
const math::Vector2<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 retreive, where
/// 0 == Line2(pt1, pt2),
/// 1 == Line2(pt2, pt3),
/// 2 == Line2(pt3, pt1)
/// \return Line segment of the requested side.
/// \throws IndexException if _index is > 2.
public: Line2<T> Side(unsigned int _index) const
{
if (_index > 2)
throw IndexException();
else if (_index == 0)
return Line2<T>(this->pts[0], this->pts[1]);
else if (_index == 1)
return Line2<T>(this->pts[1], this->pts[2]);
else
return Line2<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 Line2<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 math::Vector2<T> &_pt) const
{
// Compute vectors
math::Vector2<T> v0 = this->pts[2] -this->pts[0];
math::Vector2<T> v1 = this->pts[1] -this->pts[0];
math::Vector2<T> v2 = _pt - this->pts[0];
// Compute dot products
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);
}
/// \brief Get whether the given line intersects this triangle.
/// \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.
/// \param[out] _ipt2 Return value of the second 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 Line2<T> &_line,
math::Vector2<T> &_ipt1,
math::Vector2<T> &_ipt2) const
{
if (this->Contains(_line))
{
_ipt1 = _line[0];
_ipt2 = _line[1];
return true;
}
Line2<T> line1(this->pts[0], this->pts[1]);
Line2<T> line2(this->pts[1], this->pts[2]);
Line2<T> line3(this->pts[2], this->pts[0]);
math::Vector2<T> pt;
std::set<math::Vector2<T> > points;
if (line1.Intersect(_line, pt))
points.insert(pt);
if (line2.Intersect(_line, pt))
points.insert(pt);
if (line3.Intersect(_line, pt))
points.insert(pt);
if (points.empty())
{
return false;
}
else if (points.size() == 1)
{
auto iter = points.begin();
_ipt1 = *iter;
if (this->Contains(_line[0]))
_ipt2 = _line[0];
else
{
_ipt2 = _line[1];
}
}
else
{
auto iter = points.begin();
_ipt1 = *(iter++);
_ipt2 = *iter;
}
return true;
}
/// \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: math::Vector2<T> operator[](size_t _index) const
{
if (_index > 2)
throw IndexException();
return this->pts[_index];
}
/// The points of the triangle
private: math::Vector2<T> pts[3];
};
/// Integer specialization of the Triangle class.
typedef Triangle<int> Trianglei;
/// Double specialization of the Triangle class.
typedef Triangle<double> Triangled;
/// Float specialization of the Triangle class.
typedef Triangle<float> Trianglef;
}
}
#endif

284
ign-math/include/ignition/math/Triangle3.hh Normal file
View File

@ -0,0 +1,284 @@
/*
* 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_TRIANGLE3_HH_
#define IGNITION_MATH_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

480
ign-math/include/ignition/math/Vector2.hh Normal file
View File

@ -0,0 +1,480 @@
/*
* Copyright (C) 2012-2014 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_VECTOR2_HH_
#define IGNITION_MATH_VECTOR2_HH_
#include <ignition/math/IndexException.hh>
namespace ignition
{
namespace math
{
/// \class Vector2 Vector2.hh ignition/math/Vector2.hh
/// \brief Two dimensional (x, y) vector.
template<typename T>
class Vector2
{
/// \brief math::Vector2(0, 0)
public: static const Vector2<T> Zero;
/// \brief math::Vector2(1, 1)
public: static const Vector2<T> One;
/// \brief Default Constructor
public: Vector2()
{
this->data[0] = 0;
this->data[1] = 0;
}
/// \brief Constructor
/// \param[in] _x value along x
/// \param[in] _y value along y
public: Vector2(const T &_x, const T &_y)
{
this->data[0] = _x;
this->data[1] = _y;
}
/// \brief Copy constructor
/// \param[in] _v the value
public: Vector2(const Vector2<T> &_v)
{
this->data[0] = _v[0];
this->data[1] = _v[1];
}
/// \brief Destructor
public: virtual ~Vector2() {}
/// \brief Calc distance to the given point
/// \param[in] _pt The point to measure to
/// \return the distance
public: double Distance(const Vector2 &_pt) const
{
return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
(this->data[1]-_pt[1])*(this->data[1]-_pt[1]));
}
/// \brief Returns the length (magnitude) of the vector
/// \return The length
public: T Length() const
{
return sqrt(this->SquaredLength());
}
/// \brief Returns the square of the length (magnitude) of the vector
/// \return The squared length
public: T SquaredLength() const
{
return std::pow(this->data[0], 2)
+ std::pow(this->data[1], 2);
}
/// \brief Normalize the vector length
public: void Normalize()
{
double d = this->Length();
if (!equal<T>(d, static_cast<T>(0.0)))
{
this->data[0] /= d;
this->data[1] /= d;
}
}
/// \brief Set the contents of the vector
/// \param[in] _x value along x
/// \param[in] _y value along y
public: void Set(T _x, T _y)
{
this->data[0] = _x;
this->data[1] = _y;
}
/// \brief Get the dot product of this vector and _v
/// \param[in] _v the vector
/// \return The dot product
public: T Dot(const Vector2<T> &_v) const
{
return (this->data[0] * _v[0]) + (this->data[1] * _v[1]);
}
/// \brief Assignment operator
/// \param[in] _v a value for x and y element
/// \return this
public: Vector2 &operator=(const Vector2 &_v)
{
this->data[0] = _v[0];
this->data[1] = _v[1];
return *this;
}
/// \brief Assignment operator
/// \param[in] _v the value for x and y element
/// \return this
public: const Vector2 &operator=(T _v)
{
this->data[0] = _v;
this->data[1] = _v;
return *this;
}
/// \brief Addition operator
/// \param[in] _v vector to add
/// \return sum vector
public: Vector2 operator+(const Vector2 &_v) const
{
return Vector2(this->data[0] + _v[0], this->data[1] + _v[1]);
}
/// \brief Addition assignment operator
/// \param[in] _v the vector to add
// \return this
public: const Vector2 &operator+=(const Vector2 &_v)
{
this->data[0] += _v[0];
this->data[1] += _v[1];
return *this;
}
/// \brief Addition operators
/// \param[in] _s the scalar addend
/// \return sum vector
public: inline Vector2<T> operator+(const T _s) const
{
return Vector2<T>(this->data[0] + _s,
this->data[1] + _s);
}
/// \brief Addition operators
/// \param[in] _s the scalar addend
/// \param[in] _v input vector
/// \return sum vector
public: friend inline Vector2<T> operator+(const T _s,
const Vector2<T> &_v)
{
return _v + _s;
}
/// \brief Addition assignment operator
/// \param[in] _s scalar addend
/// \return this
public: const Vector2<T> &operator+=(const T _s)
{
this->data[0] += _s;
this->data[1] += _s;
return *this;
}
/// \brief Negation operator
/// \return negative of this vector
public: inline Vector2 operator-() const
{
return Vector2(-this->data[0], -this->data[1]);
}
/// \brief Subtraction operator
/// \param[in] _v the vector to substract
/// \return the subtracted vector
public: Vector2 operator-(const Vector2 &_v) const
{
return Vector2(this->data[0] - _v[0], this->data[1] - _v[1]);
}
/// \brief Subtraction assignment operator
/// \param[in] _v the vector to substract
/// \return this
public: const Vector2 &operator-=(const Vector2 &_v)
{
this->data[0] -= _v[0];
this->data[1] -= _v[1];
return *this;
}
/// \brief Subtraction operators
/// \param[in] _s the scalar subtrahend
/// \return difference vector
public: inline Vector2<T> operator-(const T _s) const
{
return Vector2<T>(this->data[0] - _s,
this->data[1] - _s);
}
/// \brief Subtraction operators
/// \param[in] _s the scalar minuend
/// \param[in] _v vector subtrahend
/// \return difference vector
public: friend inline Vector2<T> operator-(const T _s,
const Vector2<T> &_v)
{
return {_s - _v.X(), _s - _v.Y()};
}
/// \brief Subtraction assignment operator
/// \param[in] _s scalar subtrahend
/// \return this
public: const Vector2<T> &operator-=(T _s)
{
this->data[0] -= _s;
this->data[1] -= _s;
return *this;
}
/// \brief Division operator
/// \remarks this is an element wise division
/// \param[in] _v a vector
/// \result a result
public: const Vector2 operator/(const Vector2 &_v) const
{
return Vector2(this->data[0] / _v[0], this->data[1] / _v[1]);
}
/// \brief Division operator
/// \remarks this is an element wise division
/// \param[in] _v a vector
/// \return this
public: const Vector2 &operator/=(const Vector2 &_v)
{
this->data[0] /= _v[0];
this->data[1] /= _v[1];
return *this;
}
/// \brief Division operator
/// \param[in] _v the value
/// \return a vector
public: const Vector2 operator/(T _v) const
{
return Vector2(this->data[0] / _v, this->data[1] / _v);
}
/// \brief Division operator
/// \param[in] _v the divisor
/// \return a vector
public: const Vector2 &operator/=(T _v)
{
this->data[0] /= _v;
this->data[1] /= _v;
return *this;
}
/// \brief Multiplication operators
/// \param[in] _v the vector
/// \return the result
public: const Vector2 operator*(const Vector2 &_v) const
{
return Vector2(this->data[0] * _v[0], this->data[1] * _v[1]);
}
/// \brief Multiplication assignment operator
/// \remarks this is an element wise multiplication
/// \param[in] _v the vector
/// \return this
public: const Vector2 &operator*=(const Vector2 &_v)
{
this->data[0] *= _v[0];
this->data[1] *= _v[1];
return *this;
}
/// \brief Multiplication operators
/// \param[in] _v the scaling factor
/// \return a scaled vector
public: const Vector2 operator*(T _v) const
{
return Vector2(this->data[0] * _v, this->data[1] * _v);
}
/// \brief Scalar left multiplication operators
/// \param[in] _s the scaling factor
/// \param[in] _v the vector to scale
/// \return a scaled vector
public: friend inline const Vector2 operator*(const T _s,
const Vector2 &_v)
{
return Vector2(_v * _s);
}
/// \brief Multiplication assignment operator
/// \param[in] _v the scaling factor
/// \return a scaled vector
public: const Vector2 &operator*=(T _v)
{
this->data[0] *= _v;
this->data[1] *= _v;
return *this;
}
/// \brief Equality test with tolerance.
/// \param[in] _v the vector to compare to
/// \param[in] _tol equality tolerance.
/// \return true if the elements of the vectors are equal within
/// the tolerence specified by _tol.
public: bool Equal(const Vector2 &_v, const T &_tol) const
{
return equal<T>(this->data[0], _v[0], _tol)
&& equal<T>(this->data[1], _v[1], _tol);
}
/// \brief Equal to operator
/// \param[in] _v the vector to compare to
/// \return true if the elements of the 2 vectors are equal within
/// a tolerence (1e-6)
public: bool operator==(const Vector2 &_v) const
{
return this->Equal(_v, static_cast<T>(1e-6));
}
/// \brief Not equal to operator
/// \return true if elements are of diffent values (tolerence 1e-6)
public: bool operator!=(const Vector2 &_v) const
{
return !(*this == _v);
}
/// \brief See if a point is finite (e.g., not nan)
/// \return true if finite, false otherwise
public: bool IsFinite() const
{
// std::isfinite works with floating point values,
// need to explicit cast to avoid ambiguity in vc++.
return std::isfinite(static_cast<double>(this->data[0])) &&
std::isfinite(static_cast<double>(this->data[1]));
}
/// \brief Array subscript operator
/// \param[in] _index the index
/// \return the value. Throws an IndexException if _index is out of
/// bounds.
/// \throws IndexException if _index is >= 2.
public: inline T operator[](size_t _index) const
{
if (_index > 1)
throw IndexException();
return this->data[_index];
}
/// \brief Return the x value.
/// \return Value of the X component.
/// \throws N/A.
public: inline T X() const
{
return this->data[0];
}
/// \brief Return the y value.
/// \return Value of the Y component.
/// \throws N/A.
public: inline T Y() const
{
return this->data[1];
}
/// \brief Return a mutable x value.
/// \return Value of the X component.
/// \throws N/A.
public: inline T &X()
{
return this->data[0];
}
/// \brief Return a mutable y value.
/// \return Value of the Y component.
/// \throws N/A.
public: inline T &Y()
{
return this->data[1];
}
/// \brief Set the x value.
/// \param[in] _v Value for the x component.
public: inline void X(const T &_v)
{
this->data[0] = _v;
}
/// \brief Set the y value.
/// \param[in] _v Value for the y component.
public: inline void Y(const T &_v)
{
this->data[1] = _v;
}
/// \brief Stream extraction operator
/// \param[in] _out output stream
/// \param[in] _pt Vector2 to output
/// \return The stream
/// \throws N/A.
public: friend std::ostream
&operator<<(std::ostream &_out, const Vector2<T> &_pt)
{
_out << _pt[0] << " " << _pt[1];
return _out;
}
/// \brief Less than operator.
/// \param[in] _pt Vector to compare.
/// \return True if this vector's first or second value is less than
/// the given vector's first or second value.
public: bool operator<(const Vector2<T> &_pt) const
{
return this->data[0] < _pt[0] || this->data[1] < _pt[1];
}
/// \brief Stream extraction operator
/// \param[in] _in input stream
/// \param[in] _pt Vector2 to read values into
/// \return The stream
/// \throws N/A.
public: friend std::istream
&operator>>(std::istream &_in, Vector2<T> &_pt)
{
T x, y;
// Skip white spaces
_in.setf(std::ios_base::skipws);
_in >> x >> y;
_pt.Set(x, y);
return _in;
}
/// \brief The x and y values.
private: T data[2];
};
template<typename T>
const Vector2<T> Vector2<T>::Zero(0, 0);
template<typename T>
const Vector2<T> Vector2<T>::One(1, 1);
typedef Vector2<int> Vector2i;
typedef Vector2<double> Vector2d;
typedef Vector2<float> Vector2f;
}
}
#endif

752
ign-math/include/ignition/math/Vector3.hh Normal file
View File

@ -0,0 +1,752 @@
/*
* Copyright (C) 2014 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_VECTOR3_HH_
#define IGNITION_MATH_VECTOR3_HH_
#include <iostream>
#include <fstream>
#include <cmath>
#include <algorithm>
#include <ignition/math/Helpers.hh>
#include <ignition/math/IndexException.hh>
namespace ignition
{
namespace math
{
/// \class Vector3 Vector3.hh ignition/math/Vector3.hh
/// \brief The Vector3 class represents the generic vector containing 3
/// elements. Since it's commonly used to keep coordinate system
/// related information, its elements are labeled by x, y, z.
template<typename T>
class Vector3
{
/// \brief math::Vector3(0, 0, 0)
public: static const Vector3 Zero;
/// \brief math::Vector3(1, 1, 1)
public: static const Vector3 One;
/// \brief math::Vector3(1, 0, 0)
public: static const Vector3 UnitX;
/// \brief math::Vector3(0, 1, 0)
public: static const Vector3 UnitY;
/// \brief math::Vector3(0, 0, 1)
public: static const Vector3 UnitZ;
/// \brief Constructor
public: Vector3()
{
this->data[0] = 0;
this->data[1] = 0;
this->data[2] = 0;
}
/// \brief Constructor
/// \param[in] _x value along x
/// \param[in] _y value along y
/// \param[in] _z value along z
public: Vector3(const T &_x, const T &_y, const T &_z)
{
this->data[0] = _x;
this->data[1] = _y;
this->data[2] = _z;
}
/// \brief Copy constructor
/// \param[in] _v a vector
public: Vector3(const Vector3<T> &_v)
{
this->data[0] = _v[0];
this->data[1] = _v[1];
this->data[2] = _v[2];
}
/// \brief Destructor
public: virtual ~Vector3() {}
/// \brief Return the sum of the values
/// \return the sum
public: T Sum() const
{
return this->data[0] + this->data[1] + this->data[2];
}
/// \brief Calc distance to the given point
/// \param[in] _pt the point
/// \return the distance
public: T Distance(const Vector3<T> &_pt) const
{
return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
(this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
(this->data[2]-_pt[2])*(this->data[2]-_pt[2]));
}
/// \brief Calc distance to the given point
/// \param[in] _x value along x
/// \param[in] _y value along y
/// \param[in] _z value along z
/// \return the distance
public: T Distance(T _x, T _y, T _z) const
{
return this->Distance(Vector3(_x, _y, _z));
}
/// \brief Returns the length (magnitude) of the vector
/// \return the length
public: T Length() const
{
return sqrt(this->SquaredLength());
}
/// \brief Return the square of the length (magnitude) of the vector
/// \return the squared length
public: T SquaredLength() const
{
return std::pow(this->data[0], 2)
+ std::pow(this->data[1], 2)
+ std::pow(this->data[2], 2);
}
/// \brief Normalize the vector length
/// \return unit length vector
public: Vector3 Normalize()
{
T d = this->Length();
if (!equal<T>(d, static_cast<T>(0.0)))
{
this->data[0] /= d;
this->data[1] /= d;
this->data[2] /= d;
}
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()
{
this->data[0] = nearbyint(this->data[0]);
this->data[1] = nearbyint(this->data[1]);
this->data[2] = nearbyint(this->data[2]);
return *this;
}
/// \brief Get a rounded version of this vector
/// \return a rounded vector
public: Vector3 Rounded() const
{
Vector3<T> result = *this;
result.Round();
return result;
}
/// \brief Set the contents of the vector
/// \param[in] _x value along x
/// \param[in] _y value along y
/// \param[in] _z value aling z
public: inline void Set(T _x = 0, T _y = 0, T _z = 0)
{
this->data[0] = _x;
this->data[1] = _y;
this->data[2] = _z;
}
/// \brief Return the cross product of this vector with another vector.
/// \param[in] _v a vector
/// \return the cross product
public: Vector3 Cross(const Vector3<T> &_v) const
{
return Vector3(this->data[1] * _v[2] - this->data[2] * _v[1],
this->data[2] * _v[0] - this->data[0] * _v[2],
this->data[0] * _v[1] - this->data[1] * _v[0]);
}
/// \brief Return the dot product of this vector and another vector
/// \param[in] _v the vector
/// \return the dot product
public: T Dot(const Vector3<T> &_v) const
{
return this->data[0] * _v[0] +
this->data[1] * _v[1] +
this->data[2] * _v[2];
}
/// \brief Return the absolute dot product of this vector and
/// another vector. This is similar to the Dot function, except the
/// absolute value of each component of the vector is used.
///
/// result = abs(x1 * x2) + abs(y1 * y2) + abs(z1 *z2)
///
/// \param[in] _v the vector
/// \return The absolute dot product
public: T AbsDot(const Vector3<T> &_v) const
{
return std::abs(this->data[0] * _v[0]) +
std::abs(this->data[1] * _v[1]) +
std::abs(this->data[2] * _v[2]);
}
/// \brief Get the absolute value of the vector
/// \return a vector with positive elements
public: Vector3 Abs() const
{
return Vector3(std::abs(this->data[0]),
std::abs(this->data[1]),
std::abs(this->data[2]));
}
/// \brief Return a vector that is perpendicular to this one.
/// \return an orthogonal vector
public: Vector3 Perpendicular() const
{
static const T sqrZero = 1e-06 * 1e-06;
Vector3<T> perp = this->Cross(Vector3(1, 0, 0));
// Check the length of the vector
if (perp.SquaredLength() < sqrZero)
{
perp = this->Cross(Vector3(0, 1, 0));
}
return perp;
}
/// \brief Get a normal vector to a triangle
/// \param[in] _v1 first vertex of the triangle
/// \param[in] _v2 second vertex
/// \param[in] _v3 third vertex
/// \return the normal
public: static Vector3 Normal(const Vector3<T> &_v1,
const Vector3<T> &_v2, const Vector3<T> &_v3)
{
Vector3<T> a = _v2 - _v1;
Vector3<T> b = _v3 - _v1;
Vector3<T> n = a.Cross(b);
return n.Normalize();
}
/// \brief Get distance to a line
/// \param[in] _pt1 first point on the line
/// \param[in] _pt2 second point on the line
/// \return the minimum distance from this point to the line
public: T DistToLine(const Vector3<T> &_pt1, const Vector3 &_pt2)
{
T d = ((*this) - _pt1).Cross((*this) - _pt2).Length();
d = d / (_pt2 - _pt1).Length();
return d;
}
/// \brief Set this vector's components to the maximum of itself and the
/// passed in vector
/// \param[in] _v the maximum clamping vector
public: void Max(const Vector3<T> &_v)
{
if (_v[0] > this->data[0])
this->data[0] = _v[0];
if (_v[1] > this->data[1])
this->data[1] = _v[1];
if (_v[2] > this->data[2])
this->data[2] = _v[2];
}
/// \brief Set this vector's components to the minimum of itself and the
/// passed in vector
/// \param[in] _v the minimum clamping vector
public: void Min(const Vector3<T> &_v)
{
if (_v[0] < this->data[0])
this->data[0] = _v[0];
if (_v[1] < this->data[1])
this->data[1] = _v[1];
if (_v[2] < this->data[2])
this->data[2] = _v[2];
}
/// \brief Get the maximum value in the vector
/// \return the maximum element
public: T Max() const
{
return std::max(std::max(this->data[0], this->data[1]), this->data[2]);
}
/// \brief Get the minimum value in the vector
/// \return the minimum element
public: T Min() const
{
return std::min(std::min(this->data[0], this->data[1]), this->data[2]);
}
/// \brief Assignment operator
/// \param[in] _v a new value
/// \return this
public: Vector3 &operator=(const Vector3<T> &_v)
{
this->data[0] = _v[0];
this->data[1] = _v[1];
this->data[2] = _v[2];
return *this;
}
/// \brief Assignment operator
/// \param[in] _value assigned to all elements
/// \return this
public: Vector3 &operator=(T _v)
{
this->data[0] = _v;
this->data[1] = _v;
this->data[2] = _v;
return *this;
}
/// \brief Addition operator
/// \param[in] _v vector to add
/// \return the sum vector
public: Vector3 operator+(const Vector3<T> &_v) const
{
return Vector3(this->data[0] + _v[0],
this->data[1] + _v[1],
this->data[2] + _v[2]);
}
/// \brief Addition assignment operator
/// \param[in] _v vector to add
/// \return the sum vector
public: const Vector3 &operator+=(const Vector3<T> &_v)
{
this->data[0] += _v[0];
this->data[1] += _v[1];
this->data[2] += _v[2];
return *this;
}
/// \brief Addition operators
/// \param[in] _s the scalar addend
/// \return sum vector
public: inline Vector3<T> operator+(const T _s) const
{
return Vector3<T>(this->data[0] + _s,
this->data[1] + _s,
this->data[2] + _s);
}
/// \brief Addition operators
/// \param[in] _s the scalar addend
/// \param[in] _v input vector
/// \return sum vector
public: friend inline Vector3<T> operator+(const T _s,
const Vector3<T> &_v)
{
return {_v.X() + _s, _v.Y() + _s, _v.Z() + _s};
}
/// \brief Addition assignment operator
/// \param[in] _s scalar addend
/// \return this
public: const Vector3<T> &operator+=(const T _s)
{
this->data[0] += _s;
this->data[1] += _s;
this->data[2] += _s;
return *this;
}
/// \brief Negation operator
/// \return negative of this vector
public: inline Vector3 operator-() const
{
return Vector3(-this->data[0], -this->data[1], -this->data[2]);
}
/// \brief Subtraction operators
/// \param[in] _pt a vector to substract
/// \return a vector after the substraction
public: inline Vector3<T> operator-(const Vector3<T> &_pt) const
{
return Vector3(this->data[0] - _pt[0],
this->data[1] - _pt[1],
this->data[2] - _pt[2]);
}
/// \brief Subtraction assignment operators
/// \param[in] _pt subtrahend
/// \return a vector after the substraction
public: const Vector3<T> &operator-=(const Vector3<T> &_pt)
{
this->data[0] -= _pt[0];
this->data[1] -= _pt[1];
this->data[2] -= _pt[2];
return *this;
}
/// \brief Subtraction operators
/// \param[in] _s the scalar subtrahend
/// \return difference vector
public: inline Vector3<T> operator-(const T _s) const
{
return Vector3<T>(this->data[0] - _s,
this->data[1] - _s,
this->data[2] - _s);
}
/// \brief Subtraction operators
/// \param[in] _s the scalar minuend
/// \param[in] _v vector subtrahend
/// \return difference vector
public: friend inline Vector3<T> operator-(const T _s,
const Vector3<T> &_v)
{
return {_s - _v.X(), _s - _v.Y(), _s - _v.Z()};
}
/// \brief Subtraction assignment operator
/// \param[in] _s scalar subtrahend
/// \return this
public: const Vector3<T> &operator-=(const T _s)
{
this->data[0] -= _s;
this->data[1] -= _s;
this->data[2] -= _s;
return *this;
}
/// \brief Division operator
/// \remarks this is an element wise division
/// \param[in] _pt the vector divisor
/// \return a vector
public: const Vector3<T> operator/(const Vector3<T> &_pt) const
{
return Vector3(this->data[0] / _pt[0],
this->data[1] / _pt[1],
this->data[2] / _pt[2]);
}
/// \brief Division assignment operator
/// \remarks this is an element wise division
/// \param[in] _pt the vector divisor
/// \return a vector
public: const Vector3<T> &operator/=(const Vector3<T> &_pt)
{
this->data[0] /= _pt[0];
this->data[1] /= _pt[1];
this->data[2] /= _pt[2];
return *this;
}
/// \brief Division operator
/// \remarks this is an element wise division
/// \param[in] _v the divisor
/// \return a vector
public: const Vector3<T> operator/(T _v) const
{
return Vector3(this->data[0] / _v,
this->data[1] / _v,
this->data[2] / _v);
}
/// \brief Division assignment operator
/// \remarks this is an element wise division
/// \param[in] _v the divisor
/// \return this
public: const Vector3<T> &operator/=(T _v)
{
this->data[0] /= _v;
this->data[1] /= _v;
this->data[2] /= _v;
return *this;
}
/// \brief Multiplication operator
/// \remarks this is an element wise multiplication, not a cross product
/// \param[in] _p multiplier operator
/// \return a vector
public: Vector3<T> operator*(const Vector3<T> &_p) const
{
return Vector3(this->data[0] * _p[0],
this->data[1] * _p[1],
this->data[2] * _p[2]);
}
/// \brief Multiplication assignment operators
/// \remarks this is an element wise multiplication, not a cross product
/// \param[in] _v a vector
/// \return this
public: const Vector3<T> &operator*=(const Vector3<T> &_v)
{
this->data[0] *= _v[0];
this->data[1] *= _v[1];
this->data[2] *= _v[2];
return *this;
}
/// \brief Multiplication operators
/// \param[in] _s the scaling factor
/// \return a scaled vector
public: inline Vector3<T> operator*(T _s) const
{
return Vector3<T>(this->data[0] * _s,
this->data[1] * _s,
this->data[2] * _s);
}
/// \brief Multiplication operators
/// \param[in] _s the scaling factor
/// \param[in] _v input vector
/// \return a scaled vector
public: friend inline Vector3<T> operator*(T _s, const Vector3<T> &_v)
{
return {_v.X() * _s, _v.Y() * _s, _v.Z() * _s};
}
/// \brief Multiplication operator
/// \param[in] _v scaling factor
/// \return this
public: const Vector3<T> &operator*=(T _v)
{
this->data[0] *= _v;
this->data[1] *= _v;
this->data[2] *= _v;
return *this;
}
/// \brief Equality test with tolerance.
/// \param[in] _v the vector to compare to
/// \param[in] _tol equality tolerance.
/// \return true if the elements of the vectors are equal within
/// the tolerence specified by _tol.
public: bool Equal(const Vector3 &_v, const T &_tol) const
{
return equal<T>(this->data[0], _v[0], _tol)
&& equal<T>(this->data[1], _v[1], _tol)
&& equal<T>(this->data[2], _v[2], _tol);
}
/// \brief Equal to operator
/// \param[in] _v The vector to compare against
/// \return true if each component is equal within a
/// default tolerence (1e-3), false otherwise
public: bool operator==(const Vector3<T> &_v) const
{
return this->Equal(_v, static_cast<T>(1e-3));
}
/// \brief Not equal to operator
/// \param[in] _v The vector to compare against
/// \return false if each component is equal within a
/// default tolerence (1e-3), true otherwise
public: bool operator!=(const Vector3<T> &_v) const
{
return !(*this == _v);
}
/// \brief See if a point is finite (e.g., not nan)
/// \return true if is finite or false otherwise
public: bool IsFinite() const
{
// std::isfinite works with floating point values,
// need to explicit cast to avoid ambiguity in vc++.
return std::isfinite(static_cast<double>(this->data[0])) &&
std::isfinite(static_cast<double>(this->data[1])) &&
std::isfinite(static_cast<double>(this->data[2]));
}
/// \brief Corrects any nan values
public: inline void Correct()
{
// std::isfinite works with floating point values,
// need to explicit cast to avoid ambiguity in vc++.
if (!std::isfinite(static_cast<double>(this->data[0])))
this->data[0] = 0;
if (!std::isfinite(static_cast<double>(this->data[1])))
this->data[1] = 0;
if (!std::isfinite(static_cast<double>(this->data[2])))
this->data[2] = 0;
}
/// \brief Array subscript operator
/// \param[in] _index The index, where 0 == x, 1 == y, 2 == z.
/// \return The value. Throws an IndexException if _index is out of
/// bounds.
/// \throws IndexException if _index is >= 3.
public: T operator[](size_t _index) const
{
if (_index > 2)
throw IndexException();
return this->data[_index];
}
/// \brief Round all values to _precision decimal places
/// \param[in] _precision the decimal places
public: void Round(int _precision)
{
this->data[0] = precision(this->data[0], _precision);
this->data[1] = precision(this->data[1], _precision);
this->data[2] = precision(this->data[2], _precision);
}
/// \brief Equality test
/// \remarks This is equivalent to the == operator
/// \param[in] _v the other vector
/// \return true if the 2 vectors have the same values, false otherwise
public: bool Equal(const Vector3<T> &_v) const
{
return equal<T>(this->data[0], _v[0]) &&
equal<T>(this->data[1], _v[1]) &&
equal<T>(this->data[2], _v[2]);
}
/// \brief Get the x value.
/// \return The x component of the vector
public: inline T X() const
{
return this->data[0];
}
/// \brief Get the y value.
/// \return The y component of the vector
public: inline T Y() const
{
return this->data[1];
}
/// \brief Get the z value.
/// \return The z component of the vector
public: inline T Z() const
{
return this->data[2];
}
/// \brief Get a mutable reference to the x value.
/// \return The x component of the vector
public: inline T &X()
{
return this->data[0];
}
/// \brief Get a mutable reference to the y value.
/// \return The y component of the vector
public: inline T &Y()
{
return this->data[1];
}
/// \brief Get a mutable reference to the z value.
/// \return The z component of the vector
public: inline T &Z()
{
return this->data[2];
}
/// \brief Set the x value.
/// \param[in] _v Value for the x component.
public: inline void X(const T &_v)
{
this->data[0] = _v;
}
/// \brief Set the y value.
/// \param[in] _v Value for the y component.
public: inline void Y(const T &_v)
{
this->data[1] = _v;
}
/// \brief Set the z value.
/// \param[in] _v Value for the z component.
public: inline void Z(const T &_v)
{
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
/// \return the stream
public: friend std::ostream &operator<<(
std::ostream &_out, const ignition::math::Vector3<T> &_pt)
{
_out << precision(_pt[0], 6) << " " << precision(_pt[1], 6) << " "
<< precision(_pt[2], 6);
return _out;
}
/// \brief Stream extraction operator
/// \param _in input stream
/// \param _pt vector3 to read values into
/// \return the stream
public: friend std::istream &operator>>(
std::istream &_in, ignition::math::Vector3<T> &_pt)
{
// Skip white spaces
_in.setf(std::ios_base::skipws);
T x, y, z;
_in >> x >> y >> z;
_pt.Set(x, y, z);
return _in;
}
/// \brief The x, y, and z values
private: T data[3];
};
template<typename T> const Vector3<T> Vector3<T>::Zero(0, 0, 0);
template<typename T> const Vector3<T> Vector3<T>::One(1, 1, 1);
template<typename T> const Vector3<T> Vector3<T>::UnitX(1, 0, 0);
template<typename T> const Vector3<T> Vector3<T>::UnitY(0, 1, 0);
template<typename T> const Vector3<T> Vector3<T>::UnitZ(0, 0, 1);
typedef Vector3<int> Vector3i;
typedef Vector3<double> Vector3d;
typedef Vector3<float> Vector3f;
}
}
#endif

107
ign-math/include/ignition/math/Vector3Stats.hh Normal file
View File

@ -0,0 +1,107 @@
/*
* 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_MATH_VECTOR3_STATS_HH_
#define IGNITION_MATH_VECTOR3_STATS_HH_
#include <string>
#include <ignition/math/Helpers.hh>
#include <ignition/math/SignalStats.hh>
#include <ignition/math/Vector3.hh>
namespace ignition
{
namespace math
{
/// \brief Forward declare private data class.
class Vector3StatsPrivate;
/// \class Vector3Stats Vector3Stats.hh ignition/math/Vector3Stats.hh
/// \brief Collection of statistics for a Vector3 signal.
class IGNITION_VISIBLE Vector3Stats
{
/// \brief Constructor
public: Vector3Stats();
/// \brief Destructor
public: ~Vector3Stats();
/// \brief Add a new sample to the statistical measures.
/// \param[in] _data New signal data point.
public: void InsertData(const Vector3d &_data);
/// \brief Add a new type of statistic.
/// \param[in] _name Short name of new statistic.
/// Valid values include:
/// "maxAbs"
/// "mean"
/// "rms"
/// \return True if statistic was successfully added,
/// false if name was not recognized or had already
/// been inserted.
public: bool InsertStatistic(const std::string &_name);
/// \brief Add multiple statistics.
/// \param[in] _names Comma-separated list of new statistics.
/// For example, all statistics could be added with:
/// "maxAbs,mean,rms"
/// \return True if all statistics were successfully added,
/// false if any names were not recognized or had already
/// been inserted.
public: bool InsertStatistics(const std::string &_names);
/// \brief Forget all previous data.
public: void Reset();
/// \brief Get statistics for x component of signal.
/// \return Statistics for x component of signal.
public: SignalStats X() const;
/// \brief Get statistics for y component of signal.
/// \return Statistics for y component of signal.
public: SignalStats Y() const;
/// \brief Get statistics for z component of signal.
/// \return Statistics for z component of signal.
public: SignalStats Z() const;
/// \brief Get statistics for magnitude component of signal.
/// \return Statistics for magnitude component of signal.
public: SignalStats Mag() const;
/// \brief Get mutable reference to statistics for x component of signal.
/// \return Statistics for x component of signal.
public: SignalStats &X();
/// \brief Get mutable reference to statistics for y component of signal.
/// \return Statistics for y component of signal.
public: SignalStats &Y();
/// \brief Get mutable reference to statistics for z component of signal.
/// \return Statistics for z component of signal.
public: SignalStats &Z();
/// \brief Get mutable reference to statistics for magnitude of signal.
/// \return Statistics for magnitude of signal.
public: SignalStats &Mag();
/// \brief Pointer to private data.
protected: Vector3StatsPrivate *dataPtr;
};
}
}
#endif

44
ign-math/include/ignition/math/Vector3StatsPrivate.hh Normal file
View File

@ -0,0 +1,44 @@
/*
* 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_MATH_VECTOR3_STATS_PRIVATE_HH_
#define IGNITION_MATH_VECTOR3_STATS_PRIVATE_HH_
#include <ignition/math/SignalStats.hh>
namespace ignition
{
namespace math
{
/// \brief Private data class for the Vector3Stats class.
class Vector3StatsPrivate
{
/// \brief Statistics for x component of signal.
public: SignalStats x;
/// \brief Statistics for y component of signal.
public: SignalStats y;
/// \brief Statistics for z component of signal.
public: SignalStats z;
/// \brief Statistics for magnitude of signal.
public: SignalStats mag;
};
}
}
#endif

553
ign-math/include/ignition/math/Vector4.hh Normal file
View File

@ -0,0 +1,553 @@
/*
* Copyright (C) 2012-2014 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_VECTOR4_HH_
#define IGNITION_MATH_VECTOR4_HH_
#include <ignition/math/Matrix4.hh>
namespace ignition
{
namespace math
{
/// \class Vector4 Vector4.hh ignition/math/Vector4.hh
/// \brief T Generic x, y, z, w vector
template<typename T>
class Vector4
{
/// \brief math::Vector4(0, 0, 0, 0)
public: static const Vector4<T> Zero;
/// \brief math::Vector4(1, 1, 1, 1)
public: static const Vector4<T> One;
/// \brief Constructor
public: Vector4()
{
this->data[0] = this->data[1] = this->data[2] = this->data[3] = 0;
}
/// \brief Constructor with component values
/// \param[in] _x value along x axis
/// \param[in] _y value along y axis
/// \param[in] _z value along z axis
/// \param[in] _w value along w axis
public: Vector4(const T &_x, const T &_y, const T &_z, const T &_w)
{
this->data[0] = _x;
this->data[1] = _y;
this->data[2] = _z;
this->data[3] = _w;
}
/// \brief Copy constructor
/// \param[in] _v vector
public: Vector4(const Vector4<T> &_v)
{
this->data[0] = _v[0];
this->data[1] = _v[1];
this->data[2] = _v[2];
this->data[3] = _v[3];
}
/// \brief Destructor
public: virtual ~Vector4() {}
/// \brief Calc distance to the given point
/// \param[in] _pt the point
/// \return the distance
public: T Distance(const Vector4<T> &_pt) const
{
return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) +
(this->data[1]-_pt[1])*(this->data[1]-_pt[1]) +
(this->data[2]-_pt[2])*(this->data[2]-_pt[2]) +
(this->data[3]-_pt[3])*(this->data[3]-_pt[3]));
}
/// \brief Returns the length (magnitude) of the vector
/// \return The length
public: T Length() const
{
return sqrt(this->SquaredLength());
}
/// \brief Return the square of the length (magnitude) of the vector
/// \return the length
public: T SquaredLength() const
{
return std::pow(this->data[0], 2)
+ std::pow(this->data[1], 2)
+ std::pow(this->data[2], 2)
+ std::pow(this->data[3], 2);
}
/// \brief Normalize the vector length
public: void Normalize()
{
T d = this->Length();
if (!equal<T>(d, static_cast<T>(0.0)))
{
this->data[0] /= d;
this->data[1] /= d;
this->data[2] /= d;
this->data[3] /= d;
}
}
/// \brief Set the contents of the vector
/// \param[in] _x value along x axis
/// \param[in] _y value along y axis
/// \param[in] _z value along z axis
/// \param[in] _w value along w axis
public: void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0)
{
this->data[0] = _x;
this->data[1] = _y;
this->data[2] = _z;
this->data[3] = _w;
}
/// \brief Assignment operator
/// \param[in] _v the vector
/// \return a reference to this vector
public: Vector4<T> &operator=(const Vector4<T> &_v)
{
this->data[0] = _v[0];
this->data[1] = _v[1];
this->data[2] = _v[2];
this->data[3] = _v[3];
return *this;
}
/// \brief Assignment operator
/// \param[in] _value
public: Vector4<T> &operator=(T _value)
{
this->data[0] = _value;
this->data[1] = _value;
this->data[2] = _value;
this->data[3] = _value;
return *this;
}
/// \brief Addition operator
/// \param[in] _v the vector to add
/// \result a sum vector
public: Vector4<T> operator+(const Vector4<T> &_v) const
{
return Vector4<T>(this->data[0] + _v[0],
this->data[1] + _v[1],
this->data[2] + _v[2],
this->data[3] + _v[3]);
}
/// \brief Addition operator
/// \param[in] _v the vector to add
/// \return this vector
public: const Vector4<T> &operator+=(const Vector4<T> &_v)
{
this->data[0] += _v[0];
this->data[1] += _v[1];
this->data[2] += _v[2];
this->data[3] += _v[3];
return *this;
}
/// \brief Addition operators
/// \param[in] _s the scalar addend
/// \return sum vector
public: inline Vector4<T> operator+(const T _s) const
{
return Vector4<T>(this->data[0] + _s,
this->data[1] + _s,
this->data[2] + _s,
this->data[3] + _s);
}
/// \brief Addition operators
/// \param[in] _s the scalar addend
/// \param[in] _v input vector
/// \return sum vector
public: friend inline Vector4<T> operator+(const T _s,
const Vector4<T> &_v)
{
return _v + _s;
}
/// \brief Addition assignment operator
/// \param[in] _s scalar addend
/// \return this
public: const Vector4<T> &operator+=(const T _s)
{
this->data[0] += _s;
this->data[1] += _s;
this->data[2] += _s;
this->data[3] += _s;
return *this;
}
/// \brief Negation operator
/// \return negative of this vector
public: inline Vector4 operator-() const
{
return Vector4(-this->data[0], -this->data[1],
-this->data[2], -this->data[3]);
}
/// \brief Subtraction operator
/// \param[in] _v the vector to substract
/// \return a vector
public: Vector4<T> operator-(const Vector4<T> &_v) const
{
return Vector4<T>(this->data[0] - _v[0],
this->data[1] - _v[1],
this->data[2] - _v[2],
this->data[3] - _v[3]);
}
/// \brief Subtraction assigment operators
/// \param[in] _v the vector to substract
/// \return this vector
public: const Vector4<T> &operator-=(const Vector4<T> &_v)
{
this->data[0] -= _v[0];
this->data[1] -= _v[1];
this->data[2] -= _v[2];
this->data[3] -= _v[3];
return *this;
}
/// \brief Subtraction operators
/// \param[in] _s the scalar subtrahend
/// \return difference vector
public: inline Vector4<T> operator-(const T _s) const
{
return Vector4<T>(this->data[0] - _s,
this->data[1] - _s,
this->data[2] - _s,
this->data[3] - _s);
}
/// \brief Subtraction operators
/// \param[in] _s the scalar minuend
/// \param[in] _v vector subtrahend
/// \return difference vector
public: friend inline Vector4<T> operator-(const T _s,
const Vector4<T> &_v)
{
return {_s - _v.X(), _s - _v.Y(), _s - _v.Z(), _s - _v.W()};
}
/// \brief Subtraction assignment operator
/// \param[in] _s scalar subtrahend
/// \return this
public: const Vector4<T> &operator-=(const T _s)
{
this->data[0] -= _s;
this->data[1] -= _s;
this->data[2] -= _s;
this->data[3] -= _s;
return *this;
}
/// \brief Division assignment operator
/// \remarks Performs element wise division,
/// which has limited use.
/// \param[in] _v the vector to perform element wise division with
/// \return a result vector
public: const Vector4<T> operator/(const Vector4<T> &_v) const
{
return Vector4<T>(this->data[0] / _v[0],
this->data[1] / _v[1],
this->data[2] / _v[2],
this->data[3] / _v[3]);
}
/// \brief Division assignment operator
/// \remarks Performs element wise division,
/// which has limited use.
/// \param[in] _v the vector to perform element wise division with
/// \return this
public: const Vector4<T> &operator/=(const Vector4<T> &_v)
{
this->data[0] /= _v[0];
this->data[1] /= _v[1];
this->data[2] /= _v[2];
this->data[3] /= _v[3];
return *this;
}
/// \brief Division assignment operator
/// \remarks Performs element wise division,
/// which has limited use.
/// \param[in] _pt another vector
/// \return a result vector
public: const Vector4<T> operator/(T _v) const
{
return Vector4<T>(this->data[0] / _v, this->data[1] / _v,
this->data[2] / _v, this->data[3] / _v);
}
/// \brief Division operator
/// \param[in] _v scaling factor
/// \return a vector
public: const Vector4<T> &operator/=(T _v)
{
this->data[0] /= _v;
this->data[1] /= _v;
this->data[2] /= _v;
this->data[3] /= _v;
return *this;
}
/// \brief Multiplication operator.
/// \remarks Performs element wise multiplication,
/// which has limited use.
/// \param[in] _pt another vector
/// \return result vector
public: const Vector4<T> operator*(const Vector4<T> &_pt) const
{
return Vector4<T>(this->data[0] * _pt[0],
this->data[1] * _pt[1],
this->data[2] * _pt[2],
this->data[3] * _pt[3]);
}
/// \brief Matrix multiplication operator.
/// \param[in] _m matrix
/// \return the vector multiplied by _m
public: const Vector4<T> operator*(const Matrix4<T> &_m) const
{
return Vector4<T>(
this->data[0]*_m(0, 0) + this->data[1]*_m(1, 0) +
this->data[2]*_m(2, 0) + this->data[3]*_m(3, 0),
this->data[0]*_m(0, 1) + this->data[1]*_m(1, 1) +
this->data[2]*_m(2, 1) + this->data[3]*_m(3, 1),
this->data[0]*_m(0, 2) + this->data[1]*_m(1, 2) +
this->data[2]*_m(2, 2) + this->data[3]*_m(3, 2),
this->data[0]*_m(0, 3) + this->data[1]*_m(1, 3) +
this->data[2]*_m(2, 3) + this->data[3]*_m(3, 3));
}
/// \brief Multiplication assignment operator
/// \remarks Performs element wise multiplication,
/// which has limited use.
/// \param[in] _pt a vector
/// \return this
public: const Vector4<T> &operator*=(const Vector4<T> &_pt)
{
this->data[0] *= _pt[0];
this->data[1] *= _pt[1];
this->data[2] *= _pt[2];
this->data[3] *= _pt[3];
return *this;
}
/// \brief Multiplication operators
/// \param[in] _v scaling factor
/// \return a scaled vector
public: const Vector4<T> operator*(T _v) const
{
return Vector4<T>(this->data[0] * _v, this->data[1] * _v,
this->data[2] * _v, this->data[3] * _v);
}
/// \brief Scalar left multiplication operators
/// \param[in] _s the scaling factor
/// \param[in] _v the vector to scale
/// \return a scaled vector
public: friend inline const Vector4 operator*(const T _s,
const Vector4 &_v)
{
return Vector4(_v * _s);
}
/// \brief Multiplication assignment operator
/// \param[in] _v scaling factor
/// \return this
public: const Vector4<T> &operator*=(T _v)
{
this->data[0] *= _v;
this->data[1] *= _v;
this->data[2] *= _v;
this->data[3] *= _v;
return *this;
}
/// \brief Equality test with tolerance.
/// \param[in] _v the vector to compare to
/// \param[in] _tol equality tolerance.
/// \return true if the elements of the vectors are equal within
/// the tolerence specified by _tol.
public: bool Equal(const Vector4 &_v, const T &_tol) const
{
return equal<T>(this->data[0], _v[0], _tol)
&& equal<T>(this->data[1], _v[1], _tol)
&& equal<T>(this->data[2], _v[2], _tol)
&& equal<T>(this->data[3], _v[3], _tol);
}
/// \brief Equal to operator
/// \param[in] _v the other vector
/// \return true if each component is equal within a
/// default tolerence (1e-6), false otherwise
public: bool operator==(const Vector4<T> &_v) const
{
return this->Equal(_v, static_cast<T>(1e-6));
}
/// \brief Not equal to operator
/// \param[in] _pt the other vector
/// \return false if each component is equal within a
/// default tolerence (1e-6), true otherwise
public: bool operator!=(const Vector4<T> &_pt) const
{
return !(*this == _pt);
}
/// \brief See if a point is finite (e.g., not nan)
/// \return true if finite, false otherwise
public: bool IsFinite() const
{
// std::isfinite works with floating point values,
// need to explicit cast to avoid ambiguity in vc++.
return std::isfinite(static_cast<double>(this->data[0])) &&
std::isfinite(static_cast<double>(this->data[1])) &&
std::isfinite(static_cast<double>(this->data[2])) &&
std::isfinite(static_cast<double>(this->data[3]));
}
/// \brief Array subscript operator
/// \param[in] _index The index, where 0 == x, 1 == y, 2 == z, 3 == w.
/// \return The value. Throws an IndexException if _index is out of
/// bounds.
/// \throws IndexException if _index is >= 4.
public: inline T operator[](size_t _index) const
{
if (_index > 3)
throw IndexException();
return this->data[_index];
}
/// \brief Get the x value.
/// \return The x component of the vector
public: inline T X() const
{
return this->data[0];
}
/// \brief Get the y value.
/// \return The y component of the vector
public: inline T Y() const
{
return this->data[1];
}
/// \brief Get the z value.
/// \return The z component of the vector
public: inline T Z() const
{
return this->data[2];
}
/// \brief Get the w value.
/// \return The w component of the vector
public: inline T W() const
{
return this->data[3];
}
/// \brief Set the x value.
/// \param[in] _v Value for the x component.
public: inline void X(const T &_v)
{
this->data[0] = _v;
}
/// \brief Set the y value.
/// \param[in] _v Value for the y component.
public: inline void Y(const T &_v)
{
this->data[1] = _v;
}
/// \brief Set the z value.
/// \param[in] _v Value for the z component.
public: inline void Z(const T &_v)
{
this->data[2] = _v;
}
/// \brief Set the w value.
/// \param[in] _v Value for the w component.
public: inline void W(const T &_v)
{
this->data[3] = _v;
}
/// \brief Stream insertion operator
/// \param[in] _out output stream
/// \param[in] _pt Vector4 to output
/// \return The stream
public: friend std::ostream &operator<<(
std::ostream &_out, const ignition::math::Vector4<T> &_pt)
{
_out << _pt[0] << " " << _pt[1] << " " << _pt[2] << " " << _pt[3];
return _out;
}
/// \brief Stream extraction operator
/// \param[in] _in input stream
/// \param[in] _pt Vector4 to read values into
/// \return the stream
public: friend std::istream &operator>>(
std::istream &_in, ignition::math::Vector4<T> &_pt)
{
T x, y, z, w;
// Skip white spaces
_in.setf(std::ios_base::skipws);
_in >> x >> y >> z >> w;
_pt.Set(x, y, z, w);
return _in;
}
/// \brief Data values, 0==x, 1==y, 2==z, 3==w
private: T data[4];
};
template<typename T>
const Vector4<T> Vector4<T>::Zero(0, 0, 0, 0);
template<typename T>
const Vector4<T> Vector4<T>::One(1, 1, 1, 1);
typedef Vector4<int> Vector4i;
typedef Vector4<double> Vector4d;
typedef Vector4<float> Vector4f;
}
}
#endif

3
ign-math/include/ignition/math/math.hh.in Normal file
View File

@ -0,0 +1,3 @@
// Automatically generated
#include <ignition/math/config.hh>
${ign_headers}

173
ign-math/src/Angle.cc Normal file
View File

@ -0,0 +1,173 @@
/*
* Copyright (C) 2012-2014 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/Helpers.hh"
#include "ignition/math/Angle.hh"
using namespace ignition;
using namespace math;
const Angle Angle::Zero = math::Angle(0);
const Angle Angle::Pi = math::Angle(IGN_PI);
const Angle Angle::HalfPi = math::Angle(IGN_PI_2);
const Angle Angle::TwoPi = math::Angle(IGN_PI * 2.0);
//////////////////////////////////////////////////
Angle::Angle()
{
this->value = 0;
}
//////////////////////////////////////////////////
Angle::Angle(double _radian)
{
this->value = _radian;
}
//////////////////////////////////////////////////
Angle::Angle(const Angle &_angle)
{
this->value = _angle.value;
}
//////////////////////////////////////////////////
Angle::~Angle()
{
}
//////////////////////////////////////////////////
void Angle::Radian(double _radian)
{
this->value = _radian;
}
//////////////////////////////////////////////////
void Angle::Degree(double _degree)
{
this->value = _degree * IGN_PI / 180.0;
}
//////////////////////////////////////////////////
double Angle::Radian() const
{
return this->value;
}
//////////////////////////////////////////////////
double Angle::Degree() const
{
return this->value * 180.0 / IGN_PI;
}
//////////////////////////////////////////////////
void Angle::Normalize()
{
this->value = atan2(sin(this->value), cos(this->value));
}
//////////////////////////////////////////////////
Angle Angle::operator-(const Angle &angle) const
{
return Angle(this->value - angle.value);
}
//////////////////////////////////////////////////
Angle Angle::operator+(const Angle &angle) const
{
return Angle(this->value + angle.value);
}
//////////////////////////////////////////////////
Angle Angle::operator*(const Angle &angle) const
{
return Angle(this->value * angle.value);
}
//////////////////////////////////////////////////
Angle Angle::operator/(const Angle &angle) const
{
return Angle(this->value / angle.value);
}
//////////////////////////////////////////////////
Angle Angle::operator-=(const Angle &angle)
{
this->value -= angle.value;
return *this;
}
//////////////////////////////////////////////////
Angle Angle::operator+=(const Angle &angle)
{
this->value += angle.value;
return *this;
}
//////////////////////////////////////////////////
Angle Angle::operator*=(const Angle &angle)
{
this->value *= angle.value;
return *this;
}
//////////////////////////////////////////////////
Angle Angle::operator/=(const Angle &angle)
{
this->value /= angle.value;
return *this;
}
//////////////////////////////////////////////////
bool Angle::operator==(const Angle &angle) const
{
return equal(this->value, angle.value, 0.001);
}
//////////////////////////////////////////////////
bool Angle::operator!=(const Angle &angle) const
{
return !(*this == angle);
}
//////////////////////////////////////////////////
bool Angle::operator<(const Angle &angle) const
{
return this->value < angle.value;
}
//////////////////////////////////////////////////
bool Angle::operator<=(const Angle &angle) const
{
return this->value < angle.value || math::equal(this->value, angle.value);
}
//////////////////////////////////////////////////
bool Angle::operator>(const Angle &angle) const
{
return this->value > angle.value;
}
//////////////////////////////////////////////////
bool Angle::operator>=(const Angle &angle) const
{
return this->value > angle.value || math::equal(this->value, angle.value);
}
//////////////////////////////////////////////////
double Angle::operator()() const
{
return this->value;
}

119
ign-math/src/Angle_TEST.cc Normal file
View File

@ -0,0 +1,119 @@
/*
* Copyright (C) 2012-2014 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/Helpers.hh"
#include "ignition/math/Angle.hh"
using namespace ignition;
/////////////////////////////////////////////////
TEST(AngleTest, Angle)
{
math::Angle angle1;
EXPECT_TRUE(math::equal(0.0, angle1.Radian()));
angle1.Degree(180.0);
EXPECT_TRUE(angle1 == IGN_PI);
EXPECT_FALSE(angle1 == IGN_PI + 0.1);
EXPECT_TRUE(angle1 == IGN_PI + 0.0001);
EXPECT_TRUE(angle1 == IGN_PI - 0.0001);
EXPECT_TRUE(math::Angle(0) == math::Angle(0));
EXPECT_TRUE(math::Angle(0) == math::Angle(0.001));
angle1 = math::Angle(0.1) - math::Angle(0.3);
EXPECT_TRUE(angle1 == -0.2);
math::Angle angle(0.5);
EXPECT_TRUE(math::equal(0.5, angle.Radian()));
angle.Radian(IGN_PI);
EXPECT_TRUE(math::equal(IGN_RTOD(IGN_PI), angle.Degree()));
angle.Normalize();
EXPECT_TRUE(math::equal(IGN_RTOD(IGN_PI), angle.Degree()));
angle = math::Angle(0.1) + math::Angle(0.2);
EXPECT_TRUE(math::equal(0.3, angle.Radian()));
angle = math::Angle(0.1) * math::Angle(0.2);
EXPECT_TRUE(math::equal(0.02, angle.Radian()));
angle = math::Angle(0.1) / math::Angle(0.2);
EXPECT_TRUE(math::equal(0.5, angle.Radian()));
angle -= math::Angle(0.1);
EXPECT_TRUE(math::equal(0.4, angle.Radian()));
angle += math::Angle(0.2);
EXPECT_TRUE(math::equal(0.6, angle.Radian()));
angle *= math::Angle(0.5);
EXPECT_TRUE(math::equal(0.3, angle.Radian()));
angle /= math::Angle(0.1);
EXPECT_TRUE(math::equal(3.0, angle.Radian()));
EXPECT_TRUE(angle == math::Angle(3));
EXPECT_TRUE(angle != math::Angle(2));
EXPECT_TRUE(angle < math::Angle(4));
EXPECT_TRUE(angle > math::Angle(2));
EXPECT_TRUE(angle >= math::Angle(3));
EXPECT_TRUE(angle <= math::Angle(3));
angle = 1.2;
EXPECT_TRUE(angle <= 1.21);
EXPECT_FALSE(angle <= 1.19);
EXPECT_TRUE(angle <= 1.2);
EXPECT_FALSE(angle <= -1.19);
EXPECT_TRUE(math::Angle(1.2) <= math::Angle(1.2000000001));
EXPECT_TRUE(math::Angle(1.2000000001) <= math::Angle(1.2));
angle = 1.2;
EXPECT_FALSE(angle >= 1.21);
EXPECT_TRUE(angle >= 1.19);
EXPECT_TRUE(angle >= 1.2);
EXPECT_TRUE(angle >= -1.19);
EXPECT_TRUE(math::Angle(1.2) >= math::Angle(1.2000000001));
EXPECT_TRUE(math::Angle(1.2000000001) >= math::Angle(1.2));
}
/////////////////////////////////////////////////
TEST(AngleTest, StreamExtraction)
{
math::Angle angle;
std::istringstream stream("1.25");
EXPECT_NEAR(*angle, 0, 1e-2);
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");
}

332
ign-math/src/Box.cc Normal file
View File

@ -0,0 +1,332 @@
/*
* Copyright (C) 2012-2014 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <cmath>
#include <ignition/math/Box.hh>
#include "ignition/math/BoxPrivate.hh"
using namespace ignition;
using namespace math;
//////////////////////////////////////////////////
Box::Box()
: dataPtr(new BoxPrivate)
{
}
//////////////////////////////////////////////////
Box::Box(double _vec1X, double _vec1Y, double _vec1Z,
double _vec2X, double _vec2Y, double _vec2Z)
: dataPtr(new BoxPrivate)
{
this->dataPtr->extent = BoxPrivate::EXTENT_FINITE;
this->dataPtr->min.Set(_vec1X, _vec1Y, _vec1Z);
this->dataPtr->max.Set(_vec2X, _vec2Y, _vec2Z);
this->dataPtr->min.Min(math::Vector3d(_vec2X, _vec2Y, _vec2Z));
this->dataPtr->max.Max(math::Vector3d(_vec1X, _vec1Y, _vec1Z));
}
//////////////////////////////////////////////////
Box::Box(const Vector3d &_vec1, const Vector3d &_vec2)
: dataPtr(new BoxPrivate)
{
this->dataPtr->extent = BoxPrivate::EXTENT_FINITE;
this->dataPtr->min = _vec1;
this->dataPtr->min.Min(_vec2);
this->dataPtr->max = _vec2;
this->dataPtr->max.Max(_vec1);
}
//////////////////////////////////////////////////
Box::Box(const Box &_b)
: dataPtr(new BoxPrivate)
{
this->dataPtr->min = _b.dataPtr->min;
this->dataPtr->max = _b.dataPtr->max;
this->dataPtr->extent = _b.dataPtr->extent;
}
//////////////////////////////////////////////////
Box::~Box()
{
delete this->dataPtr;
this->dataPtr = NULL;
}
//////////////////////////////////////////////////
double Box::XLength() const
{
return std::abs(this->dataPtr->max.X() - this->dataPtr->min.X());
}
//////////////////////////////////////////////////
double Box::YLength() const
{
return std::abs(this->dataPtr->max.Y() - this->dataPtr->min.Y());
}
//////////////////////////////////////////////////
double Box::ZLength() const
{
return std::abs(this->dataPtr->max.Z() - this->dataPtr->min.Z());
}
//////////////////////////////////////////////////
math::Vector3d Box::Size() const
{
return math::Vector3d(this->XLength(),
this->YLength(),
this->ZLength());
}
//////////////////////////////////////////////////
math::Vector3d Box::Center() const
{
return this->dataPtr->min + (this->dataPtr->max - this->dataPtr->min) * 0.5;
}
//////////////////////////////////////////////////
void Box::Merge(const Box &_box)
{
if (this->dataPtr->extent == BoxPrivate::EXTENT_NULL)
{
this->dataPtr->min = _box.dataPtr->min;
this->dataPtr->max = _box.dataPtr->max;
this->dataPtr->extent = _box.dataPtr->extent;
}
else
{
this->dataPtr->min.Min(_box.dataPtr->min);
this->dataPtr->max.Max(_box.dataPtr->max);
}
}
//////////////////////////////////////////////////
Box &Box::operator =(const Box &_b)
{
this->dataPtr->max = _b.dataPtr->max;
this->dataPtr->min = _b.dataPtr->min;
this->dataPtr->extent = _b.dataPtr->extent;
return *this;
}
//////////////////////////////////////////////////
Box Box::operator+(const Box &_b) const
{
Vector3d mn, mx;
if (this->dataPtr->extent != BoxPrivate::EXTENT_NULL)
{
mn = this->dataPtr->min;
mx = this->dataPtr->max;
mn.Min(_b.dataPtr->min);
mx.Max(_b.dataPtr->max);
}
else
{
mn = _b.dataPtr->min;
mx = _b.dataPtr->max;
}
return Box(mn, mx);
}
//////////////////////////////////////////////////
const Box &Box::operator+=(const Box &_b)
{
if (this->dataPtr->extent != BoxPrivate::EXTENT_NULL)
{
this->dataPtr->min.Min(_b.dataPtr->min);
this->dataPtr->max.Max(_b.dataPtr->max);
}
else
{
this->dataPtr->min = _b.dataPtr->min;
this->dataPtr->max = _b.dataPtr->max;
this->dataPtr->extent = _b.dataPtr->extent;
}
return *this;
}
//////////////////////////////////////////////////
bool Box::operator==(const Box &_b) const
{
return this->dataPtr->min == _b.dataPtr->min &&
this->dataPtr->max == _b.dataPtr->max;
}
//////////////////////////////////////////////////
bool Box::operator!=(const Box &_b) const
{
return !(*this == _b);
}
//////////////////////////////////////////////////
Box Box::operator-(const Vector3d &_v)
{
return Box(this->dataPtr->min - _v, this->dataPtr->max - _v);
}
//////////////////////////////////////////////////
bool Box::Intersects(const Box &_box) const
{
// Check the six separating planes.
if (this->Max().X() < _box.Min().X())
return false;
if (this->Max().Y() < _box.Min().Y())
return false;
if (this->Max().Z() < _box.Min().Z())
return false;
if (this->Min().X() > _box.Max().X())
return false;
if (this->Min().Y() > _box.Max().Y())
return false;
if (this->Min().Z() > _box.Max().Z())
return false;
// Otherwise the two boxes must intersect.
return true;
}
//////////////////////////////////////////////////
const Vector3d &Box::Min() const
{
return this->dataPtr->min;
}
//////////////////////////////////////////////////
const Vector3d &Box::Max() const
{
return this->dataPtr->max;
}
//////////////////////////////////////////////////
Vector3d &Box::Min()
{
return this->dataPtr->min;
}
//////////////////////////////////////////////////
Vector3d &Box::Max()
{
return this->dataPtr->max;
}
//////////////////////////////////////////////////
bool Box::Contains(const Vector3d &_p) const
{
return _p.X() >= this->dataPtr->min.X() && _p.X() <= this->dataPtr->max.X() &&
_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);
}

507
ign-math/src/Box_TEST.cc Normal file
View File

@ -0,0 +1,507 @@
/*
* Copyright (C) 2012-2014 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/Box.hh"
using namespace ignition;
class myBox : public math::Box
{
public: myBox()
: math::Box()
{}
};
class ExampleBox : public ::testing::Test
{
protected:
virtual void SetUp()
{
box = math::Box(math::Vector3d(0, -1, 2), math::Vector3d(1, -2, 3));
}
math::Box box;
};
/////////////////////////////////////////////////
TEST(BoxTest, Inherit)
{
myBox *box = NULL;
{
box = new myBox();
EXPECT_TRUE(box != NULL);
}
EXPECT_TRUE(box->Min() == math::Vector3d(0, 0, 0));
EXPECT_TRUE(box->Max() == math::Vector3d(0, 0, 0));
{
delete box;
box = NULL;
}
}
/////////////////////////////////////////////////
TEST(BoxTest, EmptyConstructorNew)
{
math::Box *box = NULL;
{
box = new math::Box;
EXPECT_TRUE(box != NULL);
}
EXPECT_TRUE(box->Min() == math::Vector3d(0, 0, 0));
EXPECT_TRUE(box->Max() == math::Vector3d(0, 0, 0));
{
delete box;
box = NULL;
}
}
/////////////////////////////////////////////////
TEST(BoxTest, EmptyConstructor)
{
math::Box box;
EXPECT_TRUE(box.Min() == math::Vector3d(0, 0, 0));
EXPECT_TRUE(box.Max() == math::Vector3d(0, 0, 0));
}
/////////////////////////////////////////////////
TEST_F(ExampleBox, Constructor)
{
EXPECT_EQ(box.Min(), math::Vector3d(0, -2, 2));
EXPECT_EQ(box.Max(), math::Vector3d(1, -1, 3));
}
/////////////////////////////////////////////////
TEST_F(ExampleBox, CopyConstructor)
{
math::Box box1(box);
EXPECT_TRUE(box1.Min() == box.Min());
EXPECT_TRUE(box1.Max() == box.Max());
}
/////////////////////////////////////////////////
TEST_F(ExampleBox, Length)
{
EXPECT_DOUBLE_EQ(box.XLength(), 1);
EXPECT_DOUBLE_EQ(box.YLength(), 1);
EXPECT_DOUBLE_EQ(box.ZLength(), 1);
}
/////////////////////////////////////////////////
TEST_F(ExampleBox, Size)
{
EXPECT_TRUE(box.Size() == math::Vector3d(1, 1, 1));
}
/////////////////////////////////////////////////
TEST_F(ExampleBox, Center)
{
EXPECT_TRUE(box.Center() == math::Vector3d(0.5, -1.5, 2.5));
}
/////////////////////////////////////////////////
TEST(BoxTest, MergeEmpty)
{
math::Box box1;
math::Box box2;
box1.Merge(box2);
EXPECT_NEAR(box1.Min().X(), 0, 1e-6);
EXPECT_NEAR(box1.Min().Y(), 0, 1e-6);
EXPECT_NEAR(box1.Min().Z(), 0, 1e-6);
EXPECT_NEAR(box1.Max().X(), 0, 1e-6);
EXPECT_NEAR(box1.Max().Y(), 0, 1e-6);
EXPECT_NEAR(box1.Max().Z(), 0, 1e-6);
}
/////////////////////////////////////////////////
TEST(BoxTest, Minus)
{
math::Box box1(1, 2, 3, 4, 5, 6);
math::Vector3d sub(1, 1, 1);
math::Box box2 = box1 - sub;
EXPECT_EQ(box2.Min(), box1.Min() - sub);
EXPECT_EQ(box2.Max(), box1.Max() - sub);
}
/////////////////////////////////////////////////
TEST(BoxTest, PlusEmpty)
{
math::Box box1;
math::Box box2;
box1 += box2;
EXPECT_NEAR(box1.Min().X(), 0, 1e-6);
EXPECT_NEAR(box1.Min().Y(), 0, 1e-6);
EXPECT_NEAR(box1.Min().Z(), 0, 1e-6);
EXPECT_NEAR(box1.Max().X(), 0, 1e-6);
EXPECT_NEAR(box1.Max().Y(), 0, 1e-6);
EXPECT_NEAR(box1.Max().Z(), 0, 1e-6);
math::Box box3 = box2 + box1;
EXPECT_NEAR(box3.Min().X(), 0, 1e-6);
EXPECT_NEAR(box3.Min().Y(), 0, 1e-6);
EXPECT_NEAR(box3.Min().Z(), 0, 1e-6);
EXPECT_NEAR(box3.Max().X(), 0, 1e-6);
EXPECT_NEAR(box3.Max().Y(), 0, 1e-6);
EXPECT_NEAR(box3.Max().Z(), 0, 1e-6);
}
/////////////////////////////////////////////////
TEST_F(ExampleBox, Merge)
{
box.Merge(math::Box(math::Vector3d(-1, -1, -1), math::Vector3d(2, 2, 2)));
EXPECT_TRUE(box == math::Box(math::Vector3d(-1, -2, -1),
math::Vector3d(2, 2, 3)));
}
/////////////////////////////////////////////////
TEST(BoxTest, OperatorEqual)
{
math::Box box = math::Box(math::Vector3d(1, 1, 1), math::Vector3d(3, 3, 3));
math::Box box2 = math::Box(math::Vector3d(1, 1, 1), math::Vector3d(1, 3, 3));
math::Box box3 = math::Box(math::Vector3d(0, 1, 1), math::Vector3d(1, 3, 3));
EXPECT_TRUE(box == math::Box(math::Vector3d(1, 1, 1),
math::Vector3d(3, 3, 3)));
EXPECT_FALSE(box == box2);
EXPECT_FALSE(box3 == box);
}
/////////////////////////////////////////////////
TEST(BoxTest, OperatorNotEqual)
{
math::Box box = math::Box(math::Vector3d(1, 1, 1), math::Vector3d(3, 3, 3));
math::Box box2 = math::Box(math::Vector3d(1, 1, 1), math::Vector3d(1, 3, 3));
math::Box box3 = math::Box(math::Vector3d(0, 1, 1), math::Vector3d(1, 3, 3));
EXPECT_FALSE(box != math::Box(math::Vector3d(1, 1, 1),
math::Vector3d(3, 3, 3)));
EXPECT_TRUE(box != box2);
EXPECT_TRUE(box3 != box);
}
/////////////////////////////////////////////////
TEST(BoxTest, OperatorPlusEqual)
{
math::Box box = math::Box(math::Vector3d(1, 1, 1), math::Vector3d(3, 3, 3));
box += math::Box(math::Vector3d(2, 2, 2), math::Vector3d(4, 4, 4));
EXPECT_TRUE(box == math::Box(math::Vector3d(1, 1, 1),
math::Vector3d(4, 4, 4)));
}
/////////////////////////////////////////////////
TEST(BoxTest, OperatorPlus)
{
math::Box box = math::Box(math::Vector3d(1, 1, 1), math::Vector3d(4, 4, 4));
box = box + math::Box(math::Vector3d(-2, -2, -2), math::Vector3d(4, 4, 4));
EXPECT_TRUE(box == math::Box(math::Vector3d(-2, -2, -2),
math::Vector3d(4, 4, 4)));
}
/////////////////////////////////////////////////
TEST(BoxTest, Intersects)
{
math::Box box = math::Box(math::Vector3d(0, 0, 0), math::Vector3d(1, 1, 1));
EXPECT_FALSE(box.Intersects(math::Box(
math::Vector3d(1.1, 0, 0), math::Vector3d(2, 1, 1))));
EXPECT_FALSE(box.Intersects(math::Box(
math::Vector3d(0, 1.1, 0), math::Vector3d(1, 2, 1))));
EXPECT_FALSE(box.Intersects(math::Box(
math::Vector3d(0, 0, 1.1), math::Vector3d(1, 1, 2))));
EXPECT_FALSE(box.Intersects(math::Box(
math::Vector3d(-1, -1, -1), math::Vector3d(-0.1, 0, 0))));
EXPECT_FALSE(box.Intersects(math::Box(
math::Vector3d(-1, -1, -1), math::Vector3d(0, -0.1, 0))));
EXPECT_FALSE(box.Intersects(math::Box(
math::Vector3d(-1, -1, -1), math::Vector3d(0, 0, -0.1))));
EXPECT_TRUE(box.Intersects(math::Box(
math::Vector3d(0, 0, 0), math::Vector3d(1, 1, 1))));
}
/////////////////////////////////////////////////
TEST(BoxTest, Contains)
{
math::Box box = math::Box(math::Vector3d(0, 0, 0), math::Vector3d(1, 1, 1));
EXPECT_TRUE(box.Contains(math::Vector3d(0, 0, 0)));
EXPECT_TRUE(box.Contains(math::Vector3d(0, 0, 1)));
EXPECT_TRUE(box.Contains(math::Vector3d(0, 1, 1)));
EXPECT_TRUE(box.Contains(math::Vector3d(1, 1, 1)));
EXPECT_TRUE(box.Contains(math::Vector3d(1, 1, 0)));
EXPECT_TRUE(box.Contains(math::Vector3d(1, 0, 0)));
EXPECT_TRUE(box.Contains(math::Vector3d(0.5, 0.5, 0.5)));
EXPECT_FALSE(box.Contains(math::Vector3d(0, 0, -1)));
EXPECT_FALSE(box.Contains(math::Vector3d(0, -1, -1)));
EXPECT_FALSE(box.Contains(math::Vector3d(-1, -1, -1)));
EXPECT_FALSE(box.Contains(math::Vector3d(-1, -1, 0)));
EXPECT_FALSE(box.Contains(math::Vector3d(-1, 0, 0)));
EXPECT_FALSE(box.Contains(math::Vector3d(0.5, 0.5, -0.5)));
EXPECT_FALSE(box.Contains(math::Vector3d(0.5, -0.5, 0.5)));
EXPECT_FALSE(box.Contains(math::Vector3d(-0.5, 0.5, 0.5)));
EXPECT_FALSE(box.Contains(math::Vector3d(-0.5, -0.5, 0.5)));
EXPECT_FALSE(box.Contains(math::Vector3d(-0.5, -0.5, -0.5)));
EXPECT_FALSE(box.Contains(math::Vector3d(0, 0, -0.01)));
EXPECT_FALSE(box.Contains(math::Vector3d(0, -0.01, 0)));
EXPECT_FALSE(box.Contains(math::Vector3d(-0.01, 0, 0)));
}
/////////////////////////////////////////////////
TEST(BoxTest, OperatorStreamOut)
{
math::Box b(0.1, 1.2, 2.3, 1.1, 2.2, 4.3);
std::ostringstream stream;
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));
}

60
ign-math/src/CMakeLists.txt Normal file
View File

@ -0,0 +1,60 @@
include (${project_cmake_dir}/Utils.cmake)
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
set (sources
Angle.cc
Box.cc
Color.cc
Frustum.cc
Helpers.cc
IndexException.cc
Kmeans.cc
PID.cc
Rand.cc
RotationSpline.cc
RotationSplinePrivate.cc
SemanticVersion.cc
SignalStats.cc
SphericalCoordinates.cc
Spline.cc
Temperature.cc
Vector3Stats.cc
)
set (gtest_sources
Angle_TEST.cc
Box_TEST.cc
Color_TEST.cc
Filter_TEST.cc
Frustum_TEST.cc
Helpers_TEST.cc
Inertial_TEST.cc
Kmeans_TEST.cc
Line2_TEST.cc
Line3_TEST.cc
MassMatrix3_TEST.cc
Matrix3_TEST.cc
Matrix4_TEST.cc
OrientedBox_TEST.cc
PID_TEST.cc
Plane_TEST.cc
Pose_TEST.cc
Quaternion_TEST.cc
Rand_TEST.cc
RotationSpline_TEST.cc
SemanticVersion_TEST.cc
SignalStats_TEST.cc
SphericalCoordinates_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(ignition-math${PROJECT_MAJOR_VERSION} ${sources})
ign_build_tests(${gtest_sources})
ign_install_library(ignition-math${PROJECT_MAJOR_VERSION})

583
ign-math/src/Color.cc Normal file
View File

@ -0,0 +1,583 @@
/*
* Copyright (C) 2017 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 <math.h>
#include <algorithm>
#include "ignition/math/Color.hh"
using namespace ignition;
using namespace math;
const Color Color::White = Color(1, 1, 1, 1);
const Color Color::Black = Color(0, 0, 0, 1);
const Color Color::Red = Color(1, 0, 0, 1);
const Color Color::Green = Color(0, 1, 0, 1);
const Color Color::Blue = Color(0, 0, 1, 1);
const Color Color::Yellow = Color(1, 1, 0, 1);
const Color Color::Magenta = Color(1, 0, 1, 1);
const Color Color::Cyan = Color(0, 1, 1, 1);
//////////////////////////////////////////////////
Color::Color()
{
}
//////////////////////////////////////////////////
Color::Color(const float _r, const float _g, const float _b, const float _a)
: r(_r), g(_g), b(_b), a(_a)
{
this->Clamp();
}
//////////////////////////////////////////////////
Color::Color(const Color &_pt)
: r(_pt.r), g(_pt.g), b(_pt.b), a(_pt.a)
{
this->Clamp();
}
//////////////////////////////////////////////////
Color::~Color()
{
}
//////////////////////////////////////////////////
void Color::Reset()
{
this->r = this->g = this->b = 0;
this->a = 1;
}
//////////////////////////////////////////////////
void Color::Set(const float _r, const float _g, const float _b, const float _a)
{
this->r = _r;
this->g = _g;
this->b = _b;
this->a = _a;
this->Clamp();
}
//////////////////////////////////////////////////
void Color::SetFromHSV(const float _h, const float _s, const float _v)
{
int i;
float f, p , q, t;
float h = static_cast<float>(static_cast<int>(_h < 0 ? 0 : _h) % 360);
if (equal(_s, 0.0f))
{
// acromatic (grey)
this->r = this->g = this->b = _v;
return;
}
// sector 0 - 5
h /= 60;
i = static_cast<int>(floor(h));
f = h - i;
p = _v * (1-_s);
q = _v * (1 - _s * f);
t = _v * (1 - _s * (1-f));
switch (i)
{
case 0:
this->r = _v;
this->g = t;
this->b = p;
break;
case 1:
this->r = q;
this->g = _v;
this->b = p;
break;
case 2:
this->r = p;
this->g = _v;
this->b = t;
break;
case 3:
this->r = p;
this->g = q;
this->b = _v;
break;
case 4:
this->r = t;
this->g = p;
this->b = _v;
break;
case 5:
default:
this->r = _v;
this->g = p;
this->b = q;
break;
}
this->Clamp();
}
//////////////////////////////////////////////////
Vector3f Color::HSV() const
{
Vector3f hsv;
float min = std::min(this->r, std::min(this->g, this->b));
float max = std::max(this->r, std::max(this->g, this->b));
float delta = max - min;
hsv.Y() = delta / max;
hsv.Z() = max;
if (equal(delta, 0.0f))
{
hsv.X() = -1;
hsv.Y() = 0.0;
}
else if (equal(this->r, min))
hsv.X() = 3 - ((this->g - this->b) / delta);
else if (equal(this->g, min))
hsv.X() = 5 - ((this->b - this->r) / delta);
else
hsv.X() = 1 - ((this->r - this->g) / delta);
return hsv;
}
//////////////////////////////////////////////////
Vector3f Color::YUV() const
{
Vector3f yuv;
yuv.X() = 0.299f*this->r + 0.587f*this->g + 0.114f*this->b;
yuv.Y() = -0.1679f*this->r - 0.332f*this->g + 0.5f*this->b + 0.5f;
yuv.Z() = 0.5f*this->r - 0.4189f*this->g - 0.08105f*this->b + 0.5f;
yuv.X() = yuv.X() < 0 ? 0: yuv.X();
yuv.X() = yuv.X() > 255 ? 255.0f: yuv.X();
yuv.Y() = yuv.Y() < 0 ? 0: yuv.Y();
yuv.Y() = yuv.Y() > 255 ? 255.0f: yuv.Y();
yuv.Z() = yuv.Z() < 0 ? 0: yuv.Z();
yuv.Z() = yuv.Z() > 255 ? 255.0f: yuv.Z();
return yuv;
}
//////////////////////////////////////////////////
void Color::SetFromYUV(const float _y, const float _u, const float _v)
{
this->r = _y + 1.140f*_v;
this->g = _y - 0.395f*_u - 0.581f*_v;
this->b = _y + 2.032f*_u;
this->Clamp();
}
//////////////////////////////////////////////////
float Color::operator[](const unsigned int index)
{
switch (index)
{
case 0:
return this->r;
case 1:
return this->g;
case 2:
return this->b;
case 3:
return this->a;
default:
break;
}
return NAN_F;
}
//////////////////////////////////////////////////
Color::RGBA Color::AsRGBA() const
{
uint8_t val8;
unsigned int val32;
// Convert to 32bit pattern
// (RGBA = 8888)
val8 = static_cast<uint8_t>(this->r * 255);
val32 = val8 << 24;
val8 = static_cast<uint8_t>(this->g * 255);
val32 += val8 << 16;
val8 = static_cast<uint8_t>(this->b * 255);
val32 += val8 << 8;
val8 = static_cast<uint8_t>(this->a * 255);
val32 += val8;
return val32;
}
//////////////////////////////////////////////////
Color::BGRA Color::AsBGRA() const
{
uint8_t val8;
unsigned int val32 = 0;
// Convert to 32bit pattern
// (BGRA = 8888)
val8 = static_cast<uint8_t>(this->b * 255);
val32 = val8 << 24;
val8 = static_cast<uint8_t>(this->g * 255);
val32 += val8 << 16;
val8 = static_cast<uint8_t>(this->r * 255);
val32 += val8 << 8;
val8 = static_cast<uint8_t>(this->a * 255);
val32 += val8;
return val32;
}
//////////////////////////////////////////////////
Color::ARGB Color::AsARGB() const
{
uint8_t val8;
unsigned int val32 = 0;
// Convert to 32bit pattern
// (ARGB = 8888)
val8 = static_cast<uint8_t>(this->a * 255);
val32 = val8 << 24;
val8 = static_cast<uint8_t>(this->r * 255);
val32 += val8 << 16;
val8 = static_cast<uint8_t>(this->g * 255);
val32 += val8 << 8;
val8 = static_cast<uint8_t>(this->b * 255);
val32 += val8;
return val32;
}
//////////////////////////////////////////////////
Color::ABGR Color::AsABGR() const
{
uint8_t val8;
unsigned int val32 = 0;
// Convert to 32bit pattern
// (ABGR = 8888)
val8 = static_cast<uint8_t>(this->a * 255);
val32 = val8 << 24;
val8 = static_cast<uint8_t>(this->b * 255);
val32 += val8 << 16;
val8 = static_cast<uint8_t>(this->g * 255);
val32 += val8 << 8;
val8 = static_cast<uint8_t>(this->r * 255);
val32 += val8;
return val32;
}
//////////////////////////////////////////////////
void Color::SetFromRGBA(const Color::RGBA _v)
{
unsigned int val32 = _v;
// Convert from 32bit pattern
// (RGBA = 8888)
this->r = ((val32 >> 24) & 0xFF) / 255.0f;
this->g = ((val32 >> 16) & 0xFF) / 255.0f;
this->b = ((val32 >> 8) & 0xFF) / 255.0f;
this->a = (val32 & 0xFF) / 255.0f;
}
//////////////////////////////////////////////////
void Color::SetFromBGRA(const Color::BGRA _v)
{
unsigned int val32 = _v;
// Convert from 32bit pattern
// (BGRA = 8888)
this->b = ((val32 >> 24) & 0xFF) / 255.0f;
this->g = ((val32 >> 16) & 0xFF) / 255.0f;
this->r = ((val32 >> 8) & 0xFF) / 255.0f;
this->a = (val32 & 0xFF) / 255.0f;
}
//////////////////////////////////////////////////
void Color::SetFromARGB(const Color::ARGB _v)
{
unsigned int val32 = _v;
// Convert from 32bit pattern
// (ARGB = 8888)
this->a = ((val32 >> 24) & 0xFF) / 255.0f;
this->r = ((val32 >> 16) & 0xFF) / 255.0f;
this->g = ((val32 >> 8) & 0xFF) / 255.0f;
this->b = (val32 & 0xFF) / 255.0f;
}
//////////////////////////////////////////////////
void Color::SetFromABGR(const Color::ABGR _v)
{
unsigned int val32 = _v;
// Convert from 32bit pattern
// (ABGR = 8888)
this->a = ((val32 >> 24) & 0xFF) / 255.0f;
this->b = ((val32 >> 16) & 0xFF) / 255.0f;
this->g = ((val32 >> 8) & 0xFF) / 255.0f;
this->r = (val32 & 0xFF) / 255.0f;
}
//////////////////////////////////////////////////
Color &Color::operator=(const Color &_clr)
{
this->r = _clr.r;
this->g = _clr.g;
this->b = _clr.b;
this->a = _clr.a;
return *this;
}
//////////////////////////////////////////////////
Color Color::operator+(const Color &_pt) const
{
return Color(this->r + _pt.r, this->g + _pt.g,
this->b + _pt.b, this->a + _pt.a);
}
//////////////////////////////////////////////////
Color Color::operator+(const float &_v) const
{
return Color(this->r + _v, this->g + _v, this->b + _v, this->a + _v);
}
//////////////////////////////////////////////////
const Color &Color::operator+=(const Color &_pt)
{
this->r += _pt.r;
this->g += _pt.g;
this->b += _pt.b;
this->a += _pt.a;
this->Clamp();
return *this;
}
//////////////////////////////////////////////////
Color Color::operator-(const Color &_pt) const
{
return Color(this->r - _pt.r, this->g - _pt.g,
this->b - _pt.b, this->a - _pt.a);
}
//////////////////////////////////////////////////
Color Color::operator-(const float &_v) const
{
return Color(this->r - _v, this->g - _v, this->b - _v, this->a - _v);
}
//////////////////////////////////////////////////
const Color &Color::operator-=(const Color &_pt)
{
this->r -= _pt.r;
this->g -= _pt.g;
this->b -= _pt.b;
this->a -= _pt.a;
this->Clamp();
return *this;
}
//////////////////////////////////////////////////
const Color Color::operator/(const float &_i) const
{
return Color(this->r / _i, this->g / _i, this->b / _i, this->a / _i);
}
//////////////////////////////////////////////////
const Color Color::operator/(const Color &_pt) const
{
return Color(this->r / _pt.r, this->g / _pt.g,
this->b / _pt.b, this->a / _pt.a);
}
//////////////////////////////////////////////////
const Color &Color::operator/=(const Color &_pt)
{
this->r /= _pt.r;
this->g /= _pt.g;
this->b /= _pt.b;
this->a /= _pt.a;
this->Clamp();
return *this;
}
//////////////////////////////////////////////////
const Color Color::operator*(const float &_i) const
{
return Color(this->r * _i, this->g * _i, this->b * _i, this->a * _i);
}
//////////////////////////////////////////////////
const Color Color::operator*(const Color &_pt) const
{
return Color(this->r * _pt.r, this->g * _pt.g,
this->b * _pt.b, this->a * _pt.a);
}
//////////////////////////////////////////////////
const Color &Color::operator*=(const Color &_pt)
{
this->r *= _pt.r;
this->g *= _pt.g;
this->b *= _pt.b;
this->a *= _pt.a;
this->Clamp();
return *this;
}
//////////////////////////////////////////////////
bool Color::operator==(const Color &_pt) const
{
return equal(this->r, _pt.r) && equal(this->g, _pt.g) &&
equal(this->b, _pt.b) && equal(this->a, _pt.a);
}
//////////////////////////////////////////////////
bool Color::operator!=(const Color &_pt) const
{
return !(*this == _pt);
}
//////////////////////////////////////////////////
void Color::Clamp()
{
this->r = this->r < 0 || isnan(this->r) ? 0: this->r;
this->r = this->r > 1 ? this->r/255.0f: this->r;
this->g = this->g < 0 || isnan(this->g) ? 0: this->g;
this->g = this->g > 1 ? this->g/255.0f: this->g;
this->b = this->b < 0 || isnan(this->b) ? 0: this->b;
this->b = this->b > 1 ? this->b/255.0f: this->b;
this->a = this->a < 0 || isnan(this->a) ? 0: this->a;
this->a = this->a > 1 ? 1.0f: this->a;
}
//////////////////////////////////////////////////
float Color::R() const
{
return this->r;
}
//////////////////////////////////////////////////
float Color::G() const
{
return this->g;
}
//////////////////////////////////////////////////
float Color::B() const
{
return this->b;
}
//////////////////////////////////////////////////
float Color::A() const
{
return this->a;
}
//////////////////////////////////////////////////
float &Color::R()
{
return this->r;
}
//////////////////////////////////////////////////
float &Color::G()
{
return this->g;
}
//////////////////////////////////////////////////
float &Color::B()
{
return this->b;
}
//////////////////////////////////////////////////
float &Color::A()
{
return this->a;
}
//////////////////////////////////////////////////
void Color::R(const float _r)
{
this->r = _r;
}
//////////////////////////////////////////////////
void Color::G(const float _g)
{
this->g = _g;
}
//////////////////////////////////////////////////
void Color::B(const float _b)
{
this->b = _b;
}
//////////////////////////////////////////////////
void Color::A(const float _a)
{
this->a = _a;
}

379
ign-math/src/Color_TEST.cc Normal file
View File

@ -0,0 +1,379 @@
/*
* Copyright (C) 2017 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.A()pache.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 <math.h>
#include <gtest/gtest.h>
#include <ignition/math/Color.hh>
using namespace ignition;
/////////////////////////////////////////////////
TEST(Color, ConstColors)
{
EXPECT_FLOAT_EQ(1.0f, math::Color::White.R());
EXPECT_FLOAT_EQ(1.0f, math::Color::White.G());
EXPECT_FLOAT_EQ(1.0f, math::Color::White.B());
EXPECT_FLOAT_EQ(1.0f, math::Color::White.A());
EXPECT_FLOAT_EQ(0.0f, math::Color::Black.R());
EXPECT_FLOAT_EQ(0.0f, math::Color::Black.G());
EXPECT_FLOAT_EQ(0.0f, math::Color::Black.B());
EXPECT_FLOAT_EQ(1.0f, math::Color::Black.A());
EXPECT_FLOAT_EQ(1.0f, math::Color::Red.R());
EXPECT_FLOAT_EQ(0.0f, math::Color::Red.G());
EXPECT_FLOAT_EQ(0.0f, math::Color::Red.B());
EXPECT_FLOAT_EQ(1.0f, math::Color::Red.A());
EXPECT_FLOAT_EQ(0.0f, math::Color::Green.R());
EXPECT_FLOAT_EQ(1.0f, math::Color::Green.G());
EXPECT_FLOAT_EQ(0.0f, math::Color::Green.B());
EXPECT_FLOAT_EQ(1.0f, math::Color::Green.A());
EXPECT_FLOAT_EQ(0.0f, math::Color::Blue.R());
EXPECT_FLOAT_EQ(0.0f, math::Color::Blue.G());
EXPECT_FLOAT_EQ(1.0f, math::Color::Blue.B());
EXPECT_FLOAT_EQ(1.0f, math::Color::Blue.A());
EXPECT_FLOAT_EQ(1.0f, math::Color::Yellow.R());
EXPECT_FLOAT_EQ(1.0f, math::Color::Yellow.G());
EXPECT_FLOAT_EQ(0.0f, math::Color::Yellow.B());
EXPECT_FLOAT_EQ(1.0f, math::Color::Yellow.A());
EXPECT_FLOAT_EQ(1.0f, math::Color::Magenta.R());
EXPECT_FLOAT_EQ(0.0f, math::Color::Magenta.G());
EXPECT_FLOAT_EQ(1.0f, math::Color::Magenta.B());
EXPECT_FLOAT_EQ(1.0f, math::Color::Magenta.A());
EXPECT_FLOAT_EQ(0.0f, math::Color::Cyan.R());
EXPECT_FLOAT_EQ(1.0f, math::Color::Cyan.G());
EXPECT_FLOAT_EQ(1.0f, math::Color::Cyan.B());
EXPECT_FLOAT_EQ(1.0f, math::Color::Cyan.A());
}
/////////////////////////////////////////////////
TEST(Color, Color)
{
math::Color clr0;
EXPECT_FLOAT_EQ(0.0f, clr0.R());
EXPECT_FLOAT_EQ(0.0f, clr0.G());
EXPECT_FLOAT_EQ(0.0f, clr0.B());
EXPECT_FLOAT_EQ(1.0f, clr0.A());
EXPECT_EQ(clr0.AsRGBA(), 255);
clr0.A(0.0);
EXPECT_EQ(clr0.AsRGBA(), 0);
math::Color clr(.1f, .2f, .3f, 1.0f);
EXPECT_FLOAT_EQ(0.1f, clr.R());
EXPECT_FLOAT_EQ(0.2f, clr.G());
EXPECT_FLOAT_EQ(0.3f, clr.B());
EXPECT_FLOAT_EQ(1.0f, clr.A());
clr.Set(1, 0, 0, 0);
EXPECT_EQ(clr.AsRGBA(), static_cast<uint32_t>(255) << 24);
EXPECT_EQ(clr.AsBGRA(), static_cast<uint32_t>(255) << 8);
EXPECT_EQ(clr.AsARGB(), static_cast<uint32_t>(255) << 16);
EXPECT_EQ(clr.AsABGR(), static_cast<uint32_t>(255));
clr0.SetFromRGBA(static_cast<uint32_t>(255) << 24);
EXPECT_EQ(clr0, clr);
clr0.SetFromBGRA(static_cast<uint32_t>(255) << 8);
EXPECT_EQ(clr0, clr);
clr0.SetFromARGB(static_cast<uint32_t>(255) << 16);
EXPECT_EQ(clr0, clr);
clr0.SetFromABGR(static_cast<uint32_t>(255));
EXPECT_EQ(clr0, clr);
clr.Set(0, 1, 0, 0);
EXPECT_EQ(clr.AsRGBA(), static_cast<uint32_t>(255) << 16);
EXPECT_EQ(clr.AsBGRA(), static_cast<uint32_t>(255) << 16);
EXPECT_EQ(clr.AsARGB(), static_cast<uint32_t>(255) << 8);
EXPECT_EQ(clr.AsABGR(), static_cast<uint32_t>(255) << 8);
clr0.SetFromRGBA(static_cast<uint32_t>(255) << 16);
EXPECT_EQ(clr0, clr);
clr0.SetFromBGRA(static_cast<uint32_t>(255) << 16);
EXPECT_EQ(clr0, clr);
clr0.SetFromARGB(static_cast<uint32_t>(255) << 8);
EXPECT_EQ(clr0, clr);
clr0.SetFromABGR(static_cast<uint32_t>(255) << 8);
EXPECT_EQ(clr0, clr);
clr.Set(0, 0, 1, 0);
EXPECT_EQ(clr.AsRGBA(), static_cast<uint32_t>(255) << 8);
EXPECT_EQ(clr.AsBGRA(), static_cast<uint32_t>(255) << 24);
EXPECT_EQ(clr.AsARGB(), static_cast<uint32_t>(255));
EXPECT_EQ(clr.AsABGR(), static_cast<uint32_t>(255) << 16);
clr0.SetFromRGBA(static_cast<uint32_t>(255) << 8);
EXPECT_EQ(clr0, clr);
clr0.SetFromBGRA(static_cast<uint32_t>(255) << 24);
EXPECT_EQ(clr0, clr);
clr0.SetFromARGB(static_cast<uint32_t>(255));
EXPECT_EQ(clr0, clr);
clr0.SetFromABGR(static_cast<uint32_t>(255) << 16);
EXPECT_EQ(clr0, clr);
clr.Set(0, 0, 0, 1);
EXPECT_EQ(clr.AsRGBA(), static_cast<uint32_t>(255));
EXPECT_EQ(clr.AsBGRA(), static_cast<uint32_t>(255));
EXPECT_EQ(clr.AsARGB(), static_cast<uint32_t>(255) << 24);
EXPECT_EQ(clr.AsABGR(), static_cast<uint32_t>(255) << 24);
clr0.SetFromRGBA(static_cast<uint32_t>(255));
EXPECT_EQ(clr0, clr);
clr0.SetFromBGRA(static_cast<uint32_t>(255));
EXPECT_EQ(clr0, clr);
clr0.SetFromARGB(static_cast<uint32_t>(255) << 24);
EXPECT_EQ(clr0, clr);
clr0.SetFromABGR(static_cast<uint32_t>(255) << 24);
EXPECT_EQ(clr0, clr);
clr.Reset();
EXPECT_FLOAT_EQ(0.0f, clr.R());
EXPECT_FLOAT_EQ(0.0f, clr.G());
EXPECT_FLOAT_EQ(0.0f, clr.B());
EXPECT_FLOAT_EQ(1.0f, clr.A());
clr.SetFromHSV(0, 0.5, 1.0);
EXPECT_FLOAT_EQ(1.0f, clr.R());
EXPECT_FLOAT_EQ(0.5f, clr.G());
EXPECT_FLOAT_EQ(0.5f, clr.B());
EXPECT_FLOAT_EQ(1.0f, clr.A());
EXPECT_TRUE(clr.HSV() == math::Vector3f(6, 0.5, 1));
clr.SetFromHSV(60, 0.0, 1.0);
EXPECT_FLOAT_EQ(1.0f, clr.R());
EXPECT_FLOAT_EQ(1.0f, clr.G());
EXPECT_FLOAT_EQ(1.0f, clr.B());
EXPECT_FLOAT_EQ(1.0f, clr.A());
clr.SetFromHSV(120, 0.5, 1.0);
EXPECT_FLOAT_EQ(0.5f, clr.R());
EXPECT_FLOAT_EQ(1.0f, clr.G());
EXPECT_FLOAT_EQ(0.5f, clr.B());
EXPECT_FLOAT_EQ(1.0f, clr.A());
clr.SetFromHSV(180, 0.5, 1.0);
EXPECT_FLOAT_EQ(0.5f, clr.R());
EXPECT_FLOAT_EQ(1.0f, clr.G());
EXPECT_FLOAT_EQ(1.0f, clr.B());
EXPECT_FLOAT_EQ(1.0f, clr.A());
clr.SetFromHSV(240, 0.5, 1.0);
EXPECT_FLOAT_EQ(0.5f, clr.R());
EXPECT_FLOAT_EQ(0.5f, clr.G());
EXPECT_FLOAT_EQ(1.0f, clr.B());
EXPECT_FLOAT_EQ(1.0f, clr.A());
clr.SetFromHSV(300, 0.5, 1.0);
EXPECT_FLOAT_EQ(1.0f, clr[0]);
EXPECT_FLOAT_EQ(0.5f, clr[1]);
EXPECT_FLOAT_EQ(1.0f, clr[2]);
EXPECT_FLOAT_EQ(1.0f, clr[3]);
EXPECT_TRUE(std::isnan(clr[4]));
clr.R() = 0.1f;
clr.G() = 0.2f;
clr.B() = 0.3f;
clr.A() = 0.4f;
EXPECT_FLOAT_EQ(0.1f, clr[0]);
EXPECT_FLOAT_EQ(0.2f, clr[1]);
EXPECT_FLOAT_EQ(0.3f, clr[2]);
EXPECT_FLOAT_EQ(0.4f, clr[3]);
clr.Set(0.1f, 0.2f, 0.3f, 0.4f);
clr = clr + 0.2f;
EXPECT_TRUE(clr == math::Color(0.3f, 0.4f, 0.5f, 0.6f));
clr.Set(0.1f, 0.2f, 0.3f, 0.4f);
clr += math::Color(0.2f, 0.2f, 0.2f, 0.2f);
EXPECT_TRUE(clr == math::Color(0.3f, 0.4f, 0.5f, 0.6f));
clr.Set(0.1f, 0.2f, 0.3f, 0.4f);
clr = clr - 0.1f;
EXPECT_TRUE(clr == math::Color(0.0f, 0.1f, 0.2f, 0.3f));
clr.Set(0.1f, 0.2f, 0.3f, 0.4f);
clr -= math::Color(0.1f, 0.1f, 0.1f, 0.1f);
EXPECT_TRUE(clr == math::Color(0.0f, 0.1f, 0.2f, 0.3f));
clr.Set(1.f, 1.f, 1.f, 1.f);
clr = clr / 1.6f;
EXPECT_TRUE(clr == math::Color(0.625f, 0.625f, 0.625f, 0.625f));
clr.Set(1.f, 1.f, 1.f, 1.f);
clr /= math::Color(1.f, 1.f, 1.f, 1.f);
EXPECT_TRUE(clr == math::Color(1.f, 1.f, 1.f, 1.f));
clr.Set(.1f, .2f, .3f, .4f);
clr = clr * .1f;
EXPECT_TRUE(clr == math::Color(0.01f, 0.02f, 0.03f, 0.04f));
clr.Set(.1f, .2f, .3f, .4f);
clr *= math::Color(0.1f, 0.1f, 0.1f, 0.1f);
EXPECT_TRUE(clr == math::Color(0.01f, 0.02f, 0.03f, 0.04f));
clr.SetFromYUV(0.5f, 0.2f, 0.8f);
EXPECT_TRUE(math::equal(0.00553f, clr.R(), 1e-3f));
EXPECT_TRUE(math::equal(0.0f, clr.G()));
EXPECT_TRUE(math::equal(0.9064f, clr.B(), 1e-3f));
EXPECT_TRUE(math::equal(0.04f, clr.A()));
EXPECT_TRUE(clr.YUV() == math::Vector3f(0.104985f, 0.95227f, 0.429305f));
clr = math::Color(1.0f, 0.0f, 0.5f, 1.0f) +
math::Color(0.1f, 0.3f, 0.4f, 1.0f);
EXPECT_TRUE(math::equal(0.00431373f, clr.R()));
EXPECT_TRUE(math::equal(0.3f, clr.G()));
EXPECT_TRUE(math::equal(0.9f, clr.B()));
EXPECT_TRUE(math::equal(1.0f, clr.A()));
clr = math::Color(1.0f, 0.0f, 0.5f, 1.0f) -
math::Color(0.1f, 0.3f, 0.4f, 1.0f);
EXPECT_TRUE(math::equal(0.9f, clr.R()));
EXPECT_TRUE(math::equal(0.0f, clr.G()));
EXPECT_TRUE(math::equal(0.1f, clr.B()));
EXPECT_TRUE(math::equal(0.0f, clr.A()));
clr = math::Color(0.5f, 0.2f, 0.4f, 0.6f) / 2.0f;
EXPECT_TRUE(math::equal(0.25f, clr.R()));
EXPECT_TRUE(math::equal(0.1f, clr.G()));
EXPECT_TRUE(math::equal(0.2f, clr.B()));
EXPECT_TRUE(math::equal(0.3f, clr.A()));
}
/////////////////////////////////////////////////
TEST(Color, MulOp)
{
math::Color clr(0.0f, 0.01f, 0.2f, 1.0f);
math::Color clr2(1.0f, 0.2f, 0.2f, 0.0f);
math::Color clr3 = clr * clr2;
EXPECT_FLOAT_EQ(clr3.R(), 0.0f);
EXPECT_FLOAT_EQ(clr3.G(), 0.002f);
EXPECT_FLOAT_EQ(clr3.B(), 0.04f);
EXPECT_FLOAT_EQ(clr3.A(), 0.0f);
}
/////////////////////////////////////////////////
TEST(Color, DivisonOp)
{
math::Color clr(0.0f, 0.01f, 0.2f, 1.0f);
math::Color clr2 = clr / 0.2f;
EXPECT_FLOAT_EQ(clr2.R(), 0.0f);
EXPECT_FLOAT_EQ(clr2.G(), 0.05f);
EXPECT_FLOAT_EQ(clr2.B(), 1.0f);
EXPECT_FLOAT_EQ(clr2.A(), 1.0f);
clr2 = clr / 2.0f;
EXPECT_FLOAT_EQ(clr2.R(), 0.0f);
EXPECT_FLOAT_EQ(clr2.G(), 0.005f);
EXPECT_FLOAT_EQ(clr2.B(), 0.1f);
EXPECT_FLOAT_EQ(clr2.A(), 0.5f);
clr2.Set(0.0f, 0.2f, 0.4f, 0.5f);
math::Color clr3 = clr / clr2;
EXPECT_FLOAT_EQ(clr3.R(), 0.0f);
EXPECT_FLOAT_EQ(clr3.G(), 0.05f);
EXPECT_FLOAT_EQ(clr3.B(), 0.5f);
EXPECT_FLOAT_EQ(clr3.A(), 1.0f);
clr.Set(0.0f, 0.0f, 0.0f, 0.0f);
clr2.Set(0.0f, 0.0f, 0.0f, 0.0f);
clr3 = clr / clr2;
EXPECT_FLOAT_EQ(clr3.R(), 0.0f);
EXPECT_FLOAT_EQ(clr3.G(), 0.0f);
EXPECT_FLOAT_EQ(clr3.B(), 0.0f);
EXPECT_FLOAT_EQ(clr3.A(), 0.0f);
}
/////////////////////////////////////////////////
TEST(Color, ConstAndSet)
{
const math::Color clr(0.1f, 0.2f, 0.3f, 0.4f);
EXPECT_FLOAT_EQ(clr.R(), 0.1f);
EXPECT_FLOAT_EQ(clr.G(), 0.2f);
EXPECT_FLOAT_EQ(clr.B(), 0.3f);
EXPECT_FLOAT_EQ(clr.A(), 0.4f);
math::Color clr2;
clr2.R(0.4f);
clr2.G(0.3f);
clr2.B(0.2f);
clr2.A(0.1f);
EXPECT_FLOAT_EQ(clr2.R(), 0.4f);
EXPECT_FLOAT_EQ(clr2.G(), 0.3f);
EXPECT_FLOAT_EQ(clr2.B(), 0.2f);
EXPECT_FLOAT_EQ(clr2.A(), 0.1f);
EXPECT_TRUE(clr2 != clr);
}
/////////////////////////////////////////////////
TEST(Color, OperatorStreamOut)
{
math::Color c(0.1f, 0.2f, 0.3f, 0.5f);
std::ostringstream stream;
stream << c;
EXPECT_EQ(stream.str(), "0.1 0.2 0.3 0.5");
}
/////////////////////////////////////////////////
TEST(Color, HSV)
{
math::Color clr;
math::Vector3f hsv = clr.HSV();
EXPECT_FLOAT_EQ(hsv.X(), -1.0f);
EXPECT_FLOAT_EQ(hsv.Y(), 0.0f);
EXPECT_FLOAT_EQ(hsv.Z(), 0.0f);
clr.Set(0.1f, 0.2f, 0.3f, 1.0f);
hsv = clr.HSV();
EXPECT_NEAR(hsv.X(), 3.5f, 1e-3);
EXPECT_NEAR(hsv.Y(), 0.666667f, 1e-3);
EXPECT_NEAR(hsv.Z(), 0.3f, 1e-3);
clr.Set(0.3f, 0.2f, 0.1f, 1.0f);
hsv = clr.HSV();
EXPECT_NEAR(hsv.X(), 0.5f, 1e-3);
EXPECT_NEAR(hsv.Y(), 0.666667f, 1e-3);
EXPECT_NEAR(hsv.Z(), 0.3f, 1e-3);
clr.SetFromHSV(60, 10, 5);
EXPECT_NEAR(clr.R(), 0.0196078f, 1e-3);
EXPECT_NEAR(clr.G(), 0.0196078f, 1e-3);
EXPECT_NEAR(clr.B(), 0.0f, 1e-3);
EXPECT_NEAR(clr.A(), 1.0, 1e-3);
clr.SetFromHSV(360.0f, 0.5f, 0.6f);
EXPECT_NEAR(clr.R(), 0.6f, 1e-3);
EXPECT_NEAR(clr.G(), 0.3f, 1e-3);
EXPECT_NEAR(clr.B(), 0.3f, 1e-3);
EXPECT_NEAR(clr.A(), 1.0, 1e-3);
}
/////////////////////////////////////////////////
int main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

105
ign-math/src/Filter_TEST.cc Normal file
View File

@ -0,0 +1,105 @@
/*
* Copyright (C) 2012-2014 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/Filter.hh"
using namespace ignition;
/////////////////////////////////////////////////
TEST(FilterTest, OnePole)
{
math::OnePole<double> filterA;
EXPECT_DOUBLE_EQ(filterA.Process(0.2), 0.0);
filterA.Fc(0.6, 1.4);
EXPECT_DOUBLE_EQ(filterA.Process(2.5), 2.3307710879153634);
math::OnePole<double> filterB(0.1, 0.2);
EXPECT_DOUBLE_EQ(filterB.Process(0.5), 0.47839304086811385);
filterB.Set(5.4);
EXPECT_DOUBLE_EQ(filterB.Value(), 5.4);
}
/////////////////////////////////////////////////
TEST(FilterTest, OnePoleQuaternion)
{
math::OnePoleQuaternion filterA;
EXPECT_EQ(filterA.Value(), math::Quaterniond(1, 0, 0, 0));
math::OnePoleQuaternion filterB(0.4, 1.4);
EXPECT_EQ(filterB.Value(), math::Quaterniond(1, 0, 0, 0));
EXPECT_EQ(filterA.Process(math::Quaterniond(0.1, 0.2, 0.3)),
math::Quaterniond(1, 0, 0, 0));
EXPECT_EQ(filterB.Process(math::Quaterniond(0.1, 0.2, 0.3)),
math::Quaterniond(0.98841, 0.0286272, 0.0885614, 0.119929));
}
/////////////////////////////////////////////////
TEST(FilterTest, OnePoleVector3)
{
math::OnePoleVector3 filterA;
EXPECT_EQ(filterA.Value(), math::Vector3d(0, 0, 0));
math::OnePoleVector3 filterB(1.2, 3.4);
EXPECT_EQ(filterB.Value(), math::Vector3d(0, 0, 0));
EXPECT_EQ(filterA.Process(math::Vector3d(0.1, 0.2, 0.3)),
math::Vector3d(0, 0, 0));
EXPECT_EQ(filterB.Process(math::Vector3d(0.1, 0.2, 0.3)),
math::Vector3d(0.089113, 0.178226, 0.267339));
}
/////////////////////////////////////////////////
TEST(FilterTest, Biquad)
{
math::BiQuad<double> filterA;
EXPECT_NEAR(filterA.Value(), 0.0, 1e-10);
EXPECT_NEAR(filterA.Process(1.1), 0.0, 1e-10);
filterA.Fc(0.3, 1.4);
EXPECT_DOUBLE_EQ(filterA.Process(1.2), 0.66924691484768517);
filterA.Fc(0.3, 1.4, 0.1);
EXPECT_DOUBLE_EQ(filterA.Process(10.25), 0.96057152402651302);
math::BiQuad<double> filterB(4.3, 10.6);
EXPECT_NEAR(filterB.Value(), 0.0, 1e-10);
EXPECT_DOUBLE_EQ(filterB.Process(0.1234), 0.072418159950486546);
filterB.Set(4.5);
EXPECT_DOUBLE_EQ(filterB.Value(), 4.5);
}
/////////////////////////////////////////////////
TEST(FilterTest, BiquadVector3)
{
math::BiQuadVector3 filterA;
EXPECT_EQ(filterA.Value(), math::Vector3d(0, 0, 0));
EXPECT_EQ(filterA.Process(math::Vector3d(1.1, 2.3, 3.4)),
math::Vector3d(0, 0, 0));
math::BiQuadVector3 filterB(6.5, 22.4);
EXPECT_EQ(filterB.Value(), math::Vector3d(0, 0, 0));
EXPECT_EQ(filterB.Process(math::Vector3d(0.1, 20.3, 33.45)),
math::Vector3d(0.031748, 6.44475, 10.6196));
}

348
ign-math/src/Frustum.cc Normal file
View File

@ -0,0 +1,348 @@
/*
* Copyright (C) 2015 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <cmath>
#include "ignition/math/Matrix4.hh"
#include "ignition/math/FrustumPrivate.hh"
#include "ignition/math/Frustum.hh"
using namespace ignition;
using namespace math;
/////////////////////////////////////////////////
Frustum::Frustum()
: dataPtr(new FrustumPrivate(0, 1, IGN_DTOR(45), 1, Pose3d::Zero))
{
}
/////////////////////////////////////////////////
Frustum::Frustum(const double _near,
const double _far,
const Angle &_fov,
const double _aspectRatio,
const Pose3d &_pose)
: dataPtr(new FrustumPrivate(_near, _far, _fov, _aspectRatio, _pose))
{
// Compute plane based on near distance, far distance, field of view,
// aspect ratio, and pose
this->ComputePlanes();
}
/////////////////////////////////////////////////
Frustum::~Frustum()
{
delete this->dataPtr;
this->dataPtr = NULL;
}
/////////////////////////////////////////////////
Frustum::Frustum(const Frustum &_p)
: dataPtr(new FrustumPrivate(_p.Near(), _p.Far(), _p.FOV(),
_p.AspectRatio(), _p.Pose()))
{
for (int i = 0; i < 6; ++i)
this->dataPtr->planes[i] = _p.dataPtr->planes[i];
}
/////////////////////////////////////////////////
Planed Frustum::Plane(const FrustumPlane _plane) const
{
return this->dataPtr->planes[_plane];
}
/////////////////////////////////////////////////
bool Frustum::Contains(const Box &_b) const
{
// This is a fast test used for culling.
// If the box is on the negative side of a plane, then the box is not
// visible.
int overlapping = 0;
for (auto const &plane : this->dataPtr->planes)
{
auto const sign = plane.Side(_b);
if (sign == Planed::NEGATIVE_SIDE)
return false;
else if (sign == Planed::BOTH_SIDE)
++overlapping;
}
// it is possible to be outside of frustum and overlapping multiple planes
if (overlapping >= 2)
{
// return true if any box point is inside the frustum
auto const &min = _b.Min();
auto const &max = _b.Max();
for (int p = 0; p < 8; ++p)
{
const double &x = (p & 4) ? min.X() : max.X();
const double &y = (p & 2) ? min.Y() : max.Y();
const double &z = (p & 1) ? min.Z() : max.Z();
if (this->Contains(Vector3d(x, y, z)))
return true;
}
// return true if any frustum point is inside the box
for (auto const &pt : this->dataPtr->points)
{
if (_b.Contains(pt))
return true;
}
// Return true if any edge of the frustum passes through the AABB
for (const auto &edge : this->dataPtr->edges)
{
// If the edge projected onto a world axis does not overlapp with the AABB
// then the edge could not be passing through the AABB.
if (edge.first.X() < min.X() && edge.second.X() < min.X())
{
// both frustum edge points are below AABB on x axis
continue;
}
else if (edge.first.X() > max.X() && edge.second.X() > max.X())
{
// both frustum edge points are above AABB on x axis
continue;
}
else if (edge.first.Y() < min.Y() && edge.second.Y() < min.Y())
{
// both frustum edge points are below AABB on y axis
continue;
}
else if (edge.first.Y() > max.Y() && edge.second.Y() > max.Y())
{
// both frustum edge points are above AABB on y axis
continue;
}
else if (edge.first.Z() < min.Z() && edge.second.Z() < min.Z())
{
// both frustum edge points are below AABB on z axis
continue;
}
else if (edge.first.Z() > max.Z() && edge.second.Z() > max.Z())
{
// both frustum edge points are above AABB on z axis
continue;
}
else
{
// TODO prove or disprove that Frustum must penetrate AABB???
return true;
}
}
return false;
}
return true;
}
/////////////////////////////////////////////////
bool Frustum::Contains(const Vector3d &_p) const
{
// If the point is on the negative side of a plane, then the point is not
// visible.
for (auto const &plane : this->dataPtr->planes)
{
if (plane.Side(_p) == Planed::NEGATIVE_SIDE)
return false;
}
return true;
}
/////////////////////////////////////////////////
double Frustum::Near() const
{
return this->dataPtr->near;
}
/////////////////////////////////////////////////
void Frustum::SetNear(const double _near)
{
this->dataPtr->near = _near;
this->ComputePlanes();
}
/////////////////////////////////////////////////
double Frustum::Far() const
{
return this->dataPtr->far;
}
/////////////////////////////////////////////////
void Frustum::SetFar(const double _far)
{
this->dataPtr->far = _far;
this->ComputePlanes();
}
/////////////////////////////////////////////////
Angle Frustum::FOV() const
{
return this->dataPtr->fov;
}
/////////////////////////////////////////////////
void Frustum::SetFOV(const Angle &_angle)
{
this->dataPtr->fov = _angle;
this->ComputePlanes();
}
/////////////////////////////////////////////////
Pose3d Frustum::Pose() const
{
return this->dataPtr->pose;
}
/////////////////////////////////////////////////
void Frustum::SetPose(const Pose3d &_pose)
{
this->dataPtr->pose = _pose;
this->ComputePlanes();
}
/////////////////////////////////////////////////
double Frustum::AspectRatio() const
{
return this->dataPtr->aspectRatio;
}
/////////////////////////////////////////////////
void Frustum::SetAspectRatio(const double _aspectRatio)
{
this->dataPtr->aspectRatio = _aspectRatio;
this->ComputePlanes();
}
/////////////////////////////////////////////////
void Frustum::ComputePlanes()
{
// Tangent of half the field of view.
double tanFOV2 = std::tan(this->dataPtr->fov() * 0.5);
// Width of near plane
double nearWidth = 2.0 * tanFOV2 * this->dataPtr->near;
// Height of near plane
double nearHeight = nearWidth / this->dataPtr->aspectRatio;
// Width of far plane
double farWidth = 2.0 * tanFOV2 * this->dataPtr->far;
// Height of far plane
double farHeight = farWidth / this->dataPtr->aspectRatio;
// Up, right, and forward unit vectors.
Vector3d forward = this->dataPtr->pose.Rot().RotateVector(Vector3d::UnitX);
Vector3d up = this->dataPtr->pose.Rot().RotateVector(Vector3d::UnitZ);
Vector3d right = this->dataPtr->pose.Rot().RotateVector(-Vector3d::UnitY);
// Near plane center
Vector3d nearCenter = this->dataPtr->pose.Pos() + forward *
this->dataPtr->near;
// Far plane center
Vector3d farCenter = this->dataPtr->pose.Pos() + forward *
this->dataPtr->far;
// These four variables are here for convenience.
Vector3d upNearHeight2 = up * (nearHeight * 0.5);
Vector3d rightNearWidth2 = right * (nearWidth * 0.5);
Vector3d upFarHeight2 = up * (farHeight * 0.5);
Vector3d rightFarWidth2 = right * (farWidth * 0.5);
// Compute the vertices of the near plane
Vector3d nearTopLeft = nearCenter + upNearHeight2 - rightNearWidth2;
Vector3d nearTopRight = nearCenter + upNearHeight2 + rightNearWidth2;
Vector3d nearBottomLeft = nearCenter - upNearHeight2 - rightNearWidth2;
Vector3d nearBottomRight = nearCenter - upNearHeight2 + rightNearWidth2;
// Compute the vertices of the far plane
Vector3d farTopLeft = farCenter + upFarHeight2 - rightFarWidth2;
Vector3d farTopRight = farCenter + upFarHeight2 + rightFarWidth2;
Vector3d farBottomLeft = farCenter - upFarHeight2 - rightFarWidth2;
Vector3d farBottomRight = farCenter - upFarHeight2 + rightFarWidth2;
// Save these vertices
this->dataPtr->points[0] = nearTopLeft;
this->dataPtr->points[1] = nearTopRight;
this->dataPtr->points[2] = nearBottomLeft;
this->dataPtr->points[3] = nearBottomRight;
this->dataPtr->points[4] = farTopLeft;
this->dataPtr->points[5] = farTopRight;
this->dataPtr->points[6] = farBottomLeft;
this->dataPtr->points[7] = farBottomRight;
// Save the edges
this->dataPtr->edges[0] = {nearTopLeft, nearTopRight};
this->dataPtr->edges[1] = {nearTopLeft, nearBottomLeft};
this->dataPtr->edges[2] = {nearTopLeft, farTopLeft};
this->dataPtr->edges[3] = {nearTopRight, nearBottomRight};
this->dataPtr->edges[4] = {nearTopRight, farTopRight};
this->dataPtr->edges[5] = {nearBottomLeft, nearBottomRight};
this->dataPtr->edges[6] = {nearBottomLeft, farBottomLeft};
this->dataPtr->edges[7] = {farTopLeft, farTopRight};
this->dataPtr->edges[8] = {farTopLeft, farBottomLeft};
this->dataPtr->edges[9] = {farTopRight, farBottomRight};
this->dataPtr->edges[10] = {farBottomLeft, farBottomRight};
this->dataPtr->edges[11] = {farBottomRight, nearBottomRight};
Vector3d leftCenter =
(farTopLeft + nearTopLeft + farBottomLeft + nearBottomLeft) / 4.0;
Vector3d rightCenter =
(farTopRight + nearTopRight + farBottomRight + nearBottomRight) / 4.0;
Vector3d topCenter =
(farTopRight + nearTopRight + farTopLeft + nearTopLeft) / 4.0;
Vector3d bottomCenter =
(farBottomRight + nearBottomRight + farBottomLeft + nearBottomLeft) / 4.0;
// Compute plane offsets
// Set the planes, where the first value is the plane normal and the
// second the plane offset
Vector3d norm = Vector3d::Normal(nearTopLeft, nearTopRight, nearBottomLeft);
this->dataPtr->planes[FRUSTUM_PLANE_NEAR].Set(norm, nearCenter.Dot(norm));
norm = Vector3d::Normal(farTopRight, farTopLeft, farBottomLeft);
this->dataPtr->planes[FRUSTUM_PLANE_FAR].Set(norm, farCenter.Dot(norm));
norm = Vector3d::Normal(farTopLeft, nearTopLeft, nearBottomLeft);
this->dataPtr->planes[FRUSTUM_PLANE_LEFT].Set(norm, leftCenter.Dot(norm));
norm = Vector3d::Normal(nearTopRight, farTopRight, farBottomRight);
this->dataPtr->planes[FRUSTUM_PLANE_RIGHT].Set(norm, rightCenter.Dot(norm));
norm = Vector3d::Normal(nearTopLeft, farTopLeft, nearTopRight);
this->dataPtr->planes[FRUSTUM_PLANE_TOP].Set(norm, topCenter.Dot(norm));
norm = Vector3d::Normal(nearBottomLeft, nearBottomRight, farBottomRight);
this->dataPtr->planes[FRUSTUM_PLANE_BOTTOM].Set(norm, bottomCenter.Dot(norm));
}
//////////////////////////////////////////////////
Frustum &Frustum::operator =(const Frustum &_f)
{
this->dataPtr->near = _f.dataPtr->near;
this->dataPtr->far = _f.dataPtr->far;
this->dataPtr->fov = _f.dataPtr->fov;
this->dataPtr->aspectRatio = _f.dataPtr->aspectRatio;
this->dataPtr->pose = _f.dataPtr->pose;
this->ComputePlanes();
return *this;
}

651
ign-math/src/Frustum_TEST.cc Normal file
View File

@ -0,0 +1,651 @@
/*
* Copyright (C) 2015 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <gtest/gtest.h>
#include "ignition/math/Helpers.hh"
#include "ignition/math/Frustum.hh"
using namespace ignition;
using namespace math;
/////////////////////////////////////////////////
TEST(FrustumTest, Constructor)
{
Frustum frustum;
EXPECT_EQ(frustum.Near(), 0);
EXPECT_EQ(frustum.Far(), 1);
EXPECT_EQ(frustum.FOV(), IGN_DTOR(45));
EXPECT_EQ(frustum.AspectRatio(), 1);
EXPECT_EQ(frustum.Pose(), Pose3d::Zero);
}
/////////////////////////////////////////////////
TEST(FrustumTest, CopyConstructor)
{
// Frustum pointing down the +x axis
Frustum frustum(
// Near distance
1,
// Far distance
10,
// Field of view
Angle(IGN_DTOR(45)),
// Aspect ratio
320.0/240.0,
// Pose
Pose3d(0, 0, 0, 0, 0, 0));
Frustum frustum2(frustum);
EXPECT_EQ(frustum.FOV(), frustum2.FOV());
EXPECT_EQ(frustum.Near(), frustum2.Near());
EXPECT_EQ(frustum.Far(), frustum2.Far());
EXPECT_EQ(frustum.AspectRatio(), frustum2.AspectRatio());
EXPECT_EQ(frustum.AspectRatio(), frustum2.AspectRatio());
EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_NEAR).Normal(),
frustum2.Plane(Frustum::FRUSTUM_PLANE_NEAR).Normal());
EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_FAR).Normal(),
frustum2.Plane(Frustum::FRUSTUM_PLANE_FAR).Normal());
EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_LEFT).Normal(),
frustum2.Plane(Frustum::FRUSTUM_PLANE_LEFT).Normal());
EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_RIGHT).Normal(),
frustum2.Plane(Frustum::FRUSTUM_PLANE_RIGHT).Normal());
EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_TOP).Normal(),
frustum2.Plane(Frustum::FRUSTUM_PLANE_TOP).Normal());
EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_BOTTOM).Normal(),
frustum2.Plane(Frustum::FRUSTUM_PLANE_BOTTOM).Normal());
}
/////////////////////////////////////////////////
TEST(FrustumTest, AssignmentOperator)
{
// Frustum pointing to the +X+Y diagonal
Frustum frustum(
// Near distance
1,
// Far distance
10,
// Field of view
Angle(IGN_DTOR(45)),
// Aspect ratio
320.0/240.0,
// Pose
Pose3d(0, 0, 0, 0, 0, IGN_DTOR(45)));
Frustum frustum2 = frustum;
EXPECT_EQ(frustum.FOV(), frustum2.FOV());
EXPECT_EQ(frustum.Near(), frustum2.Near());
EXPECT_EQ(frustum.Far(), frustum2.Far());
EXPECT_EQ(frustum.AspectRatio(), frustum2.AspectRatio());
EXPECT_EQ(frustum.AspectRatio(), frustum2.AspectRatio());
EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_NEAR).Normal(),
frustum2.Plane(Frustum::FRUSTUM_PLANE_NEAR).Normal());
EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_FAR).Normal(),
frustum2.Plane(Frustum::FRUSTUM_PLANE_FAR).Normal());
EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_LEFT).Normal(),
frustum2.Plane(Frustum::FRUSTUM_PLANE_LEFT).Normal());
EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_RIGHT).Normal(),
frustum2.Plane(Frustum::FRUSTUM_PLANE_RIGHT).Normal());
EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_TOP).Normal(),
frustum2.Plane(Frustum::FRUSTUM_PLANE_TOP).Normal());
EXPECT_EQ(frustum.Plane(Frustum::FRUSTUM_PLANE_BOTTOM).Normal(),
frustum2.Plane(Frustum::FRUSTUM_PLANE_BOTTOM).Normal());
}
/////////////////////////////////////////////////
TEST(FrustumTest, PyramidXAxisPos)
{
// Frustum pointing down the +x axis
Frustum frustum(
// Near distance
1,
// Far distance
10,
// Field of view
Angle(IGN_DTOR(45)),
// Aspect ratio
320.0/240.0,
// Pose
Pose3d(0, 0, 0, 0, 0, 0));
EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 0)));
EXPECT_TRUE(frustum.Contains(Vector3d(1, 0, 0)));
EXPECT_TRUE(frustum.Contains(Vector3d(2, 0, 0)));
EXPECT_TRUE(frustum.Contains(Vector3d(10, 0, 0)));
EXPECT_FALSE(frustum.Contains(Vector3d(10.1, 0, 0)));
EXPECT_TRUE(frustum.Contains(Box(Vector3d(1, 0, 0), Vector3d(5, 5, 5))));
EXPECT_FALSE(frustum.Contains(Box(Vector3d(-1, 0, 0), Vector3d(.1, .2, .3))));
}
/////////////////////////////////////////////////
TEST(FrustumTest, PyramidXAxisNeg)
{
// Frustum pointing down the -x axis
Frustum frustum(
// Near distance
1,
// Far distance
10,
// Field of view
Angle(IGN_DTOR(45)),
// Aspect ratio
320.0/240.0,
// Pose
Pose3d(0, 0, 0, 0, 0, IGN_PI));
EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 0)));
EXPECT_FALSE(frustum.Contains(Vector3d(-0.5, 0, 0)));
EXPECT_FALSE(frustum.Contains(Vector3d(-10.1, 0, 0)));
EXPECT_TRUE(frustum.Contains(Vector3d(-1, 0, 0)));
EXPECT_TRUE(frustum.Contains(Vector3d(-2, 0, 0)));
EXPECT_TRUE(frustum.Contains(Vector3d(-10, 0, 0)));
EXPECT_FALSE(frustum.Contains(Box(Vector3d(1, 0, 0), Vector3d(5, 5, 5))));
EXPECT_TRUE(frustum.Contains(Box(Vector3d(-1, 0, 0), Vector3d(.1, .2, .3))));
}
/////////////////////////////////////////////////
TEST(FrustumTest, PyramidYAxis)
{
// Frustum pointing down the +y axis
Frustum frustum(
// Near distance
.1,
// Far distance
5,
// Field of view
Angle(IGN_DTOR(45)),
// Aspect ratio
320.0/320.0,
// Pose
Pose3d(0, 0, 0, 0, 0, IGN_PI*0.5));
EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 0)));
EXPECT_FALSE(frustum.Contains(Vector3d(1, 0, 0)));
EXPECT_FALSE(frustum.Contains(Vector3d(.05, 0, 0)));
EXPECT_TRUE(frustum.Contains(Vector3d(0, .1, 0)));
EXPECT_TRUE(frustum.Contains(Vector3d(0, 1, 0)));
EXPECT_TRUE(frustum.Contains(Vector3d(0, 5, 0)));
EXPECT_TRUE(frustum.Contains(Box(Vector3d(0, 1, 0), Vector3d(5, 5, 5))));
EXPECT_FALSE(frustum.Contains(Box(Vector3d(0, -1, 0), Vector3d(.1, 0, .3))));
}
/////////////////////////////////////////////////
TEST(FrustumTest, PyramidZAxis)
{
// Frustum pointing down the -z axis
Frustum frustum(
// Near distance
1,
// Far distance
10,
// Field of view
Angle(IGN_DTOR(45)),
// Aspect ratio
320.0/320.0,
// Pose
Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0));
EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 0)));
EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, -0.9)));
EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, -10.5)));
EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 0.9)));
EXPECT_FALSE(frustum.Contains(Vector3d(0, 0, 10.5)));
EXPECT_TRUE(frustum.Contains(Vector3d(0, 0, -1.1)));
EXPECT_TRUE(frustum.Contains(Vector3d(0.5, 0.5, -5.5)));
EXPECT_TRUE(frustum.Contains(Vector3d(0, 0, -10)));
EXPECT_FALSE(frustum.Contains(Box(Vector3d(0, 0, 0), Vector3d(5, 5, 5))));
EXPECT_TRUE(frustum.Contains(Box(Vector3d(0, 0, -1), Vector3d(.1, 0, .3))));
}
/////////////////////////////////////////////////
TEST(FrustumTest, NearFar)
{
Frustum frustum(
// Near distance
1,
// Far distance
10,
// Field of view
Angle(IGN_DTOR(45)),
// Aspect ratio
320.0/320.0,
// Pose
Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0));
EXPECT_DOUBLE_EQ(frustum.Near(), 1.0);
EXPECT_DOUBLE_EQ(frustum.Far(), 10.0);
frustum.SetNear(-1.0);
frustum.SetFar(-10.0);
EXPECT_DOUBLE_EQ(frustum.Near(), -1.0);
EXPECT_DOUBLE_EQ(frustum.Far(), -10.0);
}
/////////////////////////////////////////////////
TEST(FrustumTest, FOV)
{
Frustum frustum(
// Near distance
1,
// Far distance
10,
// Field of view
Angle(IGN_DTOR(45)),
// Aspect ratio
320.0/320.0,
// Pose
Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0));
EXPECT_EQ(frustum.FOV(), math::Angle(IGN_DTOR(45)));
frustum.SetFOV(1.5707);
EXPECT_EQ(frustum.FOV(), math::Angle(1.5707));
}
/////////////////////////////////////////////////
TEST(FrustumTest, AspectRatio)
{
Frustum frustum(
// Near distance
1,
// Far distance
10,
// Field of view
Angle(IGN_DTOR(45)),
// Aspect ratio
320.0/320.0,
// Pose
Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0));
EXPECT_DOUBLE_EQ(frustum.AspectRatio(), 320.0/320.0);
frustum.SetAspectRatio(1.3434);
EXPECT_DOUBLE_EQ(frustum.AspectRatio(), 1.3434);
}
/////////////////////////////////////////////////
TEST(FrustumTest, Pose)
{
Frustum frustum(
// Near distance
1,
// Far distance
10,
// Field of view
Angle(IGN_DTOR(45)),
// Aspect ratio
320.0/320.0,
// Pose
Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0));
EXPECT_EQ(frustum.Pose(), Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0));
frustum.SetPose(Pose3d(1, 2, 3, IGN_PI, 0, 0));
EXPECT_EQ(frustum.Pose(), Pose3d(1, 2, 3, IGN_PI, 0, 0));
}
/////////////////////////////////////////////////
TEST(FrustumTest, PoseContains)
{
Frustum frustum(
// Near distance
1,
// Far distance
10,
// Field of view
Angle(IGN_DTOR(60)),
// Aspect ratio
1920.0/1080.0,
// Pose
Pose3d(0, -5, 0, 0, 0, IGN_PI*0.5));
// Test the near clip boundary
EXPECT_FALSE(frustum.Contains(Vector3d(0, -4.01, 0)));
EXPECT_TRUE(frustum.Contains(Vector3d(0, -4.0, 0)));
// Test a point between the near and far clip planes
EXPECT_TRUE(frustum.Contains(Vector3d(0, 1, 0)));
// Test the far clip boundary
EXPECT_TRUE(frustum.Contains(Vector3d(0, 5, 0)));
EXPECT_FALSE(frustum.Contains(Vector3d(0, 5.001, 0)));
// Use an offset for the test points. This makes the test more stable, and
// is also used to generate point outside the frustum.
double offset = 0.00001;
// Compute near clip points
Vector3d nearTopLeft(
-tan(IGN_DTOR(30)) + offset,
frustum.Pose().Pos().Y() + frustum.Near() + offset,
tan(IGN_DTOR(30)) / frustum.AspectRatio() - offset);
Vector3d nearTopLeftBad(
-tan(IGN_DTOR(30)) - offset,
frustum.Pose().Pos().Y() + frustum.Near() - offset,
tan(IGN_DTOR(30)) / frustum.AspectRatio() + offset);
Vector3d nearTopRight(
tan(IGN_DTOR(30)) - offset,
frustum.Pose().Pos().Y() + frustum.Near() + offset,
tan(IGN_DTOR(30)) / frustum.AspectRatio() - offset);
Vector3d nearTopRightBad(
tan(IGN_DTOR(30)) + offset,
frustum.Pose().Pos().Y() + frustum.Near() - offset,
tan(IGN_DTOR(30)) / frustum.AspectRatio() + offset);
Vector3d nearBottomLeft(
-tan(IGN_DTOR(30)) + offset,
frustum.Pose().Pos().Y() + frustum.Near() + offset,
-tan(IGN_DTOR(30)) / frustum.AspectRatio() + offset);
Vector3d nearBottomLeftBad(
-tan(IGN_DTOR(30)) - offset,
frustum.Pose().Pos().Y() + frustum.Near() - offset,
-tan(IGN_DTOR(30)) / frustum.AspectRatio() - offset);
Vector3d nearBottomRight(
tan(IGN_DTOR(30)) - offset,
frustum.Pose().Pos().Y() + frustum.Near() + offset,
-tan(IGN_DTOR(30)) / frustum.AspectRatio() + offset);
Vector3d nearBottomRightBad(
tan(IGN_DTOR(30)) + offset,
frustum.Pose().Pos().Y() + frustum.Near() - offset,
-tan(IGN_DTOR(30)) / frustum.AspectRatio() - offset);
// Test near clip corners
EXPECT_TRUE(frustum.Contains(nearTopLeft));
EXPECT_FALSE(frustum.Contains(nearTopLeftBad));
EXPECT_TRUE(frustum.Contains(nearTopRight));
EXPECT_FALSE(frustum.Contains(nearTopRightBad));
EXPECT_TRUE(frustum.Contains(nearBottomLeft));
EXPECT_FALSE(frustum.Contains(nearBottomLeftBad));
EXPECT_TRUE(frustum.Contains(nearBottomRight));
EXPECT_FALSE(frustum.Contains(nearBottomRightBad));
// Compute far clip points
Vector3d farTopLeft(
-tan(IGN_DTOR(30)) * frustum.Far() + offset,
frustum.Pose().Pos().Y() + frustum.Far() - offset,
(tan(IGN_DTOR(30)) * frustum.Far()) / frustum.AspectRatio() - offset);
Vector3d farTopLeftBad(
-tan(IGN_DTOR(30))*frustum.Far() - offset,
frustum.Pose().Pos().Y() + frustum.Far() + offset,
(tan(IGN_DTOR(30) * frustum.Far())) / frustum.AspectRatio() + offset);
Vector3d farTopRight(
tan(IGN_DTOR(30))*frustum.Far() - offset,
frustum.Pose().Pos().Y() + frustum.Far() - offset,
(tan(IGN_DTOR(30)) * frustum.Far()) / frustum.AspectRatio() - offset);
Vector3d farTopRightBad(
tan(IGN_DTOR(30))*frustum.Far() + offset,
frustum.Pose().Pos().Y() + frustum.Far() + offset,
(tan(IGN_DTOR(30)) * frustum.Far()) / frustum.AspectRatio() + offset);
Vector3d farBottomLeft(
-tan(IGN_DTOR(30))*frustum.Far() + offset,
frustum.Pose().Pos().Y() + frustum.Far() - offset,
(-tan(IGN_DTOR(30)) * frustum.Far()) / frustum.AspectRatio() + offset);
Vector3d farBottomLeftBad(
-tan(IGN_DTOR(30))*frustum.Far() - offset,
frustum.Pose().Pos().Y() + frustum.Far() + offset,
(-tan(IGN_DTOR(30)) * frustum.Far()) / frustum.AspectRatio() - offset);
Vector3d farBottomRight(
tan(IGN_DTOR(30))*frustum.Far() - offset,
frustum.Pose().Pos().Y() + frustum.Far() - offset,
(-tan(IGN_DTOR(30)) * frustum.Far()) / frustum.AspectRatio() + offset);
Vector3d farBottomRightBad(
tan(IGN_DTOR(30))*frustum.Far() + offset,
frustum.Pose().Pos().Y() + frustum.Far() + offset,
(-tan(IGN_DTOR(30)) * frustum.Far()) / frustum.AspectRatio() - offset);
// Test far clip corners
EXPECT_TRUE(frustum.Contains(farTopLeft));
EXPECT_FALSE(frustum.Contains(farTopLeftBad));
EXPECT_TRUE(frustum.Contains(farTopRight));
EXPECT_FALSE(frustum.Contains(farTopRightBad));
EXPECT_TRUE(frustum.Contains(farBottomLeft));
EXPECT_FALSE(frustum.Contains(farBottomLeftBad));
EXPECT_TRUE(frustum.Contains(farBottomRight));
EXPECT_FALSE(frustum.Contains(farBottomRightBad));
// Adjust to 45 degrees rotation
frustum.SetPose(Pose3d(1, 1, 0, 0, 0, -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)));
}
//////////////////////////////////////////////////
TEST(FrustumTest, ContainsAABBNoOverlap)
{
Frustum frustum;
frustum.SetNear(0.55);
frustum.SetFar(2.5);
frustum.SetFOV(1.05);
frustum.SetAspectRatio(1.8);
frustum.SetPose(Pose3d(0, 0, 2, 0, 0, 0));
// Boxes that don't overlapp any planes
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(1.45, -0.05, 1.95), Vector3d(1.55, 0.05, 2.05))));
EXPECT_FALSE(frustum.Contains(
Box(Vector3d(2.55, -0.05, 1.95), Vector3d(2.65, 0.05, 2.05))));
EXPECT_FALSE(frustum.Contains(
Box(Vector3d(0.35, -0.05, 1.95), Vector3d(0.45, 0.05, 2.05))));
EXPECT_FALSE(frustum.Contains(
Box(Vector3d(1.45, -0.05, 2.55), Vector3d(1.55, 0.05, 2.65))));
EXPECT_FALSE(frustum.Contains(
Box(Vector3d(1.45, -0.05, 1.35), Vector3d(1.55, 0.05, 1.45))));
EXPECT_FALSE(frustum.Contains(
Box(Vector3d(1.45, -1.05, 1.95), Vector3d(1.55, -0.95, 2.05))));
EXPECT_FALSE(frustum.Contains(
Box(Vector3d(1.45, 0.95, 1.95), Vector3d(1.55, 1.05, 2.05))));
}
//////////////////////////////////////////////////
TEST(FrustumTest, ContainsAABBOverlapOnePlane)
{
Frustum frustum;
frustum.SetNear(0.55);
frustum.SetFar(2.5);
frustum.SetFOV(1.05);
frustum.SetAspectRatio(1.8);
frustum.SetPose(Pose3d(0, 0, 2, 0, 0, 0));
// Boxes overlapping one plane
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(2.43, -0.05, 1.95), Vector3d(2.53, 0.05, 2.05))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(0.495, -0.05, 1.95), Vector3d(0.595, 0.05, 2.05))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(1.45, -0.05, 2.42), Vector3d(1.55, 0.05, 2.52))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(1.45, -0.05, 1.48), Vector3d(1.55, 0.05, 1.58))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(1.45, -0.9, 1.95), Vector3d(1.55, -0.8, 2.05))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(1.45, 0.8, 1.95), Vector3d(1.55, 0.9, 2.05))));
}
//////////////////////////////////////////////////
TEST(FrustumTest, ContainsAABBOverlapTwoPlanes)
{
Frustum frustum;
frustum.SetNear(0.55);
frustum.SetFar(2.5);
frustum.SetFOV(1.05);
frustum.SetAspectRatio(1.8);
frustum.SetPose(Pose3d(0, 0, 2, 0, 0, 0));
// Boxes overlapping two planes
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(2.42, -0.05, 2.7), Vector3d(2.52, 0.05, 2.8))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(2.42, -0.05, 1.2), Vector3d(2.52, 0.05, 1.3))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(2.42, -1.44, 1.95), Vector3d(2.52, -1.34, 2.05))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(2.42, 1.34, 1.95), Vector3d(2.52, 1.44, 2.05))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(0.495, -0.05, 2.1), Vector3d(0.595, 0.05, 2.2))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(0.495, -0.05, 1.8), Vector3d(0.595, 0.05, 1.9))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(0.495, 0.25, 1.95), Vector3d(0.595, 0.35, 2.05))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(0.495, -0.35, 1.95), Vector3d(0.595, -0.25, 2.05))));
EXPECT_FALSE(frustum.Contains(
Box(Vector3d(2.48, -0.05, 2.81), Vector3d(2.58, 0.05, 2.91))));
EXPECT_FALSE(frustum.Contains(
Box(Vector3d(2.48, -0.05, 1.09), Vector3d(2.58, 0.05, 1.19))));
EXPECT_FALSE(frustum.Contains(
Box(Vector3d(2.48, -1.55, 1.95), Vector3d(2.58, -1.45, 2.05))));
EXPECT_FALSE(frustum.Contains(
Box(Vector3d(2.48, 1.45, 1.95), Vector3d(2.58, 1.55, 2.05))));
}
//////////////////////////////////////////////////
TEST(FrustumTest, ContainsAABBOverlapThreePlanes)
{
Frustum frustum;
frustum.SetNear(0.55);
frustum.SetFar(2.5);
frustum.SetFOV(1.05);
frustum.SetAspectRatio(1.8);
frustum.SetPose(Pose3d(0, 0, 2, 0, 0, 0));
// Boxes overlapping three planes
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(0.495, 0.25, 2.1), Vector3d(0.595, 0.35, 2.2))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(0.495, 0.25, 1.8), Vector3d(0.595, 0.35, 1.9))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(0.495, -0.35, 2.1), Vector3d(0.595, -0.25, 2.2))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(0.495, -0.35, 1.8), Vector3d(0.595, -0.25, 1.9))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(2.42, 1.34, 2.7), Vector3d(2.52, 1.44, 2.8))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(2.42, 1.34, 1.2), Vector3d(2.52, 1.44, 1.3))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(2.42, -1.44, 2.7), Vector3d(2.52, -1.34, 2.8))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(2.42, -1.44, 1.2), Vector3d(2.52, -1.34, 1.3))));
EXPECT_FALSE(frustum.Contains(
Box(Vector3d(2.48, 1.45, 2.81), Vector3d(2.58, 1.55, 2.91))));
EXPECT_FALSE(frustum.Contains(
Box(Vector3d(2.48, -1.55, 2.81), Vector3d(2.58, -1.45, 2.91))));
EXPECT_FALSE(frustum.Contains(
Box(Vector3d(2.48, 1.45, 1.09), Vector3d(2.58, 1.55, 1.19))));
EXPECT_FALSE(frustum.Contains(
Box(Vector3d(2.48, -1.55, 1.09), Vector3d(2.58, -1.45, 1.19))));
}
//////////////////////////////////////////////////
TEST(FrustumTest, AABBContainsFrustum)
{
Frustum frustum;
frustum.SetNear(0.55);
frustum.SetFar(2.5);
frustum.SetFOV(1.05);
frustum.SetAspectRatio(1.8);
frustum.SetPose(Pose3d(0, 0, 2, 0, 0, 0));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(-100, -100, -100), Vector3d(100, 100, 100))));
}
//////////////////////////////////////////////////
TEST(FrustumTest, AABBFrustumEdgeOverlap)
{
// This test case has the top of an AABB overlap a frustum, but all the
// corners of AABB fall outside the frustum.
double ybounds = 10;
Frustum frustum;
frustum.SetNear(0.55);
frustum.SetFar(2.5);
frustum.SetFOV(1.05);
frustum.SetAspectRatio(1.8);
frustum.SetPose(Pose3d(0, 0, 2, 0, 0, 0));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(1, -ybounds, 0), Vector3d(2, ybounds, 2))));
}
//////////////////////////////////////////////////
TEST(FrustumTest, AABBBFWall)
{
// Frustum contains at a large but thin wall
Frustum frustum;
frustum.SetNear(0.55);
frustum.SetFar(2.5);
frustum.SetFOV(1.05);
frustum.SetAspectRatio(1.8);
frustum.SetPose(Pose3d(0, 0, 2, 0, 0, 0));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(1, -10, -10), Vector3d(2, 10, 10))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(-10, 1, -10), Vector3d(10, 1.1, 10))));
EXPECT_TRUE(frustum.Contains(
Box(Vector3d(-10, -10, 1.95), Vector3d(10, 10, 2.05))));
}

47
ign-math/src/Helpers.cc Normal file
View File

@ -0,0 +1,47 @@
/*
* Copyright (C) 2012-2014 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/Helpers.hh"
/////////////////////////////////////////////
ignition::math::PairOutput ignition::math::Pair(
const ignition::math::PairInput _a, const ignition::math::PairInput _b)
{
// 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));
}

434
ign-math/src/Helpers_TEST.cc Normal file
View File

@ -0,0 +1,434 @@
/*
* Copyright (C) 2012-2014 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/Rand.hh"
#include "ignition/math/Vector3.hh"
#include "ignition/math/Helpers.hh"
using namespace ignition;
/////////////////////////////////////////////////
// Test a few function in Helpers
TEST(HelpersTest, Helpers)
{
EXPECT_EQ(12345, math::parseInt("12345"));
EXPECT_EQ(-12345, math::parseInt("-12345"));
EXPECT_EQ(-12345, math::parseInt(" -12345"));
EXPECT_EQ(0, math::parseInt(" "));
EXPECT_EQ(math::NAN_I, math::parseInt(""));
EXPECT_EQ(math::NAN_I, math::parseInt("?"));
EXPECT_EQ(math::NAN_I, math::parseInt("23ab67"));
EXPECT_DOUBLE_EQ(12.345, math::parseFloat("12.345"));
EXPECT_DOUBLE_EQ(-12.345, math::parseFloat("-12.345"));
EXPECT_DOUBLE_EQ(-12.345, math::parseFloat(" -12.345"));
EXPECT_DOUBLE_EQ(0.0, math::parseFloat(" "));
EXPECT_TRUE(math::equal(123.45, math::parseFloat("1.2345e2"), 1e-2));
EXPECT_TRUE(math::equal(123.45, math::parseFloat("1.2345e+2"), 1e-2));
EXPECT_TRUE(math::equal(123.45, math::parseFloat("1.2345e+002"), 1e-2));
EXPECT_TRUE(math::equal(.012345, math::parseFloat("1.2345e-2"), 1e-2));
EXPECT_TRUE(math::equal(.012345, math::parseFloat("1.2345e-002"), 1e-2));
EXPECT_TRUE(math::equal(1.2345, math::parseFloat("1.2345e+"), 1e-2));
EXPECT_TRUE(math::equal(1.2345, math::parseFloat("1.2345e-"), 1e-2));
EXPECT_TRUE(math::lessOrNearEqual(1.0, 2.0, 1e-2));
EXPECT_TRUE(math::lessOrNearEqual(1.0, 1.0 - 9e-3, 1e-2));
EXPECT_FALSE(math::lessOrNearEqual(1.0, 1.0 - 1.1e-2, 1e-2));
EXPECT_TRUE(math::greaterOrNearEqual(1.0, 0.5, 1e-2));
EXPECT_TRUE(math::greaterOrNearEqual(1.0, 1.0 + 9e-3, 1e-2));
EXPECT_FALSE(math::greaterOrNearEqual(1.0, 1.0 + 1.1e-2, 1e-2));
EXPECT_DOUBLE_EQ(1.2345, math::parseFloat("1.2345e+0"));
EXPECT_TRUE(math::isnan(math::parseFloat("")));
EXPECT_TRUE(math::isnan(math::parseFloat("?")));
EXPECT_TRUE(math::isnan(math::parseFloat("23ab67")));
EXPECT_EQ(1u, math::roundUpPowerOfTwo(0));
EXPECT_EQ(1u, math::roundUpPowerOfTwo(1));
EXPECT_EQ(2u, math::roundUpPowerOfTwo(2));
EXPECT_EQ(2048u, math::roundUpPowerOfTwo(1025));
}
/////////////////////////////////////////////////
// Test Helpers::precision
TEST(HelpersTest, Precision)
{
EXPECT_DOUBLE_EQ(0, math::precision(0.0, 1));
EXPECT_DOUBLE_EQ(0.1, math::precision(0.1, 1));
EXPECT_DOUBLE_EQ(0.1, math::precision(0.14, 1));
EXPECT_DOUBLE_EQ(0.2, math::precision(0.15, 1));
EXPECT_DOUBLE_EQ(0.15, math::precision(0.15, 2));
EXPECT_DOUBLE_EQ(1, math::precision(1.4, 0));
EXPECT_EQ(0, math::precision(0, 0));
}
/////////////////////////////////////////////////
// Test Helpers::isPowerOfTwo
TEST(HelpersTest, PowerOfTwo)
{
EXPECT_FALSE(math::isPowerOfTwo(0));
EXPECT_FALSE(math::isPowerOfTwo(3));
EXPECT_TRUE(math::isPowerOfTwo(1));
EXPECT_TRUE(math::isPowerOfTwo(2));
EXPECT_TRUE(math::isPowerOfTwo(4));
}
// MSVC report errors on division by zero
#ifndef _MSC_VER
/////////////////////////////////////////////////
// Test Helpers::fixnan functions
TEST(HelpersTest, FixNaN)
{
EXPECT_DOUBLE_EQ(math::fixnan(1.0 / 0.0), 0.0);
EXPECT_DOUBLE_EQ(math::fixnan(-1.0 / 0.0), 0.0);
EXPECT_DOUBLE_EQ(math::fixnan(0.0 / 0.0), 0.0);
EXPECT_DOUBLE_EQ(math::fixnan(42.0), 42.0);
EXPECT_DOUBLE_EQ(math::fixnan(-42.0), -42.0);
EXPECT_FLOAT_EQ(math::fixnan(1.0f / 0.0f), 0.0f);
EXPECT_FLOAT_EQ(math::fixnan(-1.0f / 0.0f), 0.0f);
EXPECT_FLOAT_EQ(math::fixnan(0.0f / 0.0f), 0.0f);
EXPECT_FLOAT_EQ(math::fixnan(42.0f), 42.0f);
EXPECT_FLOAT_EQ(math::fixnan(-42.0f), -42.0f);
}
#endif
/////////////////////////////////////////////////
// Even test
TEST(HelpersTest, Even)
{
int i = 1;
signed s = 1;
signed int si = 1;
unsigned u = 1;
unsigned int ui = 1;
EXPECT_FALSE(math::isEven(i));
EXPECT_FALSE(math::isEven(s));
EXPECT_FALSE(math::isEven(si));
EXPECT_FALSE(math::isEven(u));
EXPECT_FALSE(math::isEven(ui));
i = -1;
s = -1;
si = -1;
EXPECT_FALSE(math::isEven(i));
EXPECT_FALSE(math::isEven(s));
EXPECT_FALSE(math::isEven(si));
i = 4;
s = 4;
si = 4;
u = 4;
ui = 4;
EXPECT_TRUE(math::isEven(i));
EXPECT_TRUE(math::isEven(s));
EXPECT_TRUE(math::isEven(si));
EXPECT_TRUE(math::isEven(u));
EXPECT_TRUE(math::isEven(ui));
i = -2;
s = -2;
si = -2;
EXPECT_TRUE(math::isEven(i));
EXPECT_TRUE(math::isEven(s));
EXPECT_TRUE(math::isEven(si));
i = 0;
s = 0;
si = 0;
u = 0;
ui = 0;
EXPECT_TRUE(math::isEven(i));
EXPECT_TRUE(math::isEven(s));
EXPECT_TRUE(math::isEven(si));
EXPECT_TRUE(math::isEven(u));
EXPECT_TRUE(math::isEven(ui));
}
/////////////////////////////////////////////////
// Odd test
TEST(HelpersTest, Odd)
{
int i = 1;
signed s = 1;
signed int si = 1;
unsigned u = 1;
unsigned int ui = 1;
EXPECT_TRUE(math::isOdd(i));
EXPECT_TRUE(math::isOdd(s));
EXPECT_TRUE(math::isOdd(si));
EXPECT_TRUE(math::isOdd(u));
EXPECT_TRUE(math::isOdd(ui));
i = -1;
s = -1;
si = -1;
EXPECT_TRUE(math::isOdd(i));
EXPECT_TRUE(math::isOdd(s));
EXPECT_TRUE(math::isOdd(si));
i = 4;
s = 4;
si = 4;
u = 4;
ui = 4;
EXPECT_FALSE(math::isOdd(i));
EXPECT_FALSE(math::isOdd(s));
EXPECT_FALSE(math::isOdd(si));
EXPECT_FALSE(math::isOdd(u));
EXPECT_FALSE(math::isOdd(ui));
i = -2;
s = -2;
si = -2;
EXPECT_FALSE(math::isOdd(i));
EXPECT_FALSE(math::isOdd(s));
EXPECT_FALSE(math::isOdd(si));
i = 0;
s = 0;
si = 0;
u = 0;
ui = 0;
EXPECT_FALSE(math::isOdd(i));
EXPECT_FALSE(math::isOdd(s));
EXPECT_FALSE(math::isOdd(si));
EXPECT_FALSE(math::isOdd(u));
EXPECT_FALSE(math::isOdd(ui));
}
/////////////////////////////////////////////////
TEST(HelpersTest, Sort)
{
{
int a = 2;
int b = -1;
math::sort2(a, b);
EXPECT_LE(a, b);
}
{
int a = 0;
int b = 1;
math::sort2(a, b);
EXPECT_LE(a, b);
}
{
int a = 2;
int b = -1;
int c = 0;
math::sort3(a, b, c);
EXPECT_LE(a, b);
EXPECT_LE(b, c);
}
{
unsigned int a = 2;
unsigned int b = 1;
math::sort2(a, b);
EXPECT_LE(a, b);
}
{
unsigned int a = 2;
unsigned int b = 1;
unsigned int c = 0;
math::sort3(a, b, c);
EXPECT_LE(a, b);
EXPECT_LE(b, c);
}
{
unsigned int a = 0;
unsigned int b = 1;
unsigned int c = 2;
math::sort3(a, b, c);
EXPECT_LE(a, b);
EXPECT_LE(b, c);
}
{
float a = 2.1f;
float b = -1.1e-1f;
math::sort2(a, b);
EXPECT_LE(a, b);
}
{
float a = 34.5f;
float b = -1.34f;
float c = 0.194f;
math::sort3(a, b, c);
EXPECT_LE(a, b);
EXPECT_LE(b, c);
}
{
double a = 2.1;
double b = -1.1e-1;
math::sort2(a, b);
EXPECT_LE(a, b);
}
{
double a = 34.5;
double b = -1.34;
double c = 0.194;
math::sort3(a, b, c);
EXPECT_LE(a, b);
EXPECT_LE(b, c);
}
}
/////////////////////////////////////////////////
TEST(HelpersTest, Volume)
{
EXPECT_DOUBLE_EQ(IGN_SPHERE_VOLUME(1.0), 4.0*IGN_PI*std::pow(1, 3)/3.0);
EXPECT_DOUBLE_EQ(IGN_SPHERE_VOLUME(0.1), 4.0*IGN_PI*std::pow(.1, 3)/3.0);
EXPECT_DOUBLE_EQ(IGN_SPHERE_VOLUME(-1.1), 4.0*IGN_PI*std::pow(-1.1, 3)/3.0);
EXPECT_DOUBLE_EQ(IGN_CYLINDER_VOLUME(0.5, 2.0), 2 * IGN_PI * std::pow(.5, 2));
EXPECT_DOUBLE_EQ(IGN_CYLINDER_VOLUME(1, -1), -1 * IGN_PI * std::pow(1, 2));
EXPECT_DOUBLE_EQ(IGN_BOX_VOLUME(1, 2, 3), 1 * 2 * 3);
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 = math::MAX_UI16;
math::PairInput maxB = math::MAX_UI16;
#else
math::PairInput maxA = math::MAX_UI32;
math::PairInput maxB = math::MAX_UI32;
#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, math::MAX_UI32);
#else
EXPECT_EQ(maxKey, math::MAX_UI64);
#endif
std::tie(maxC, maxD) = math::Unpair(maxKey);
EXPECT_EQ(maxC, maxA);
EXPECT_EQ(maxD, maxB);
#ifdef _MSC_VER
math::PairInput minA = math::MIN_UI16;
math::PairInput minB = math::MIN_UI16;
#else
math::PairInput minA = math::MIN_UI32;
math::PairInput minB = math::MIN_UI32;
#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, math::MIN_UI32);
#else
EXPECT_EQ(minKey, math::MIN_UI64);
#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 = math::MIN_UI16; a < math::MAX_UI16 - 500;
a += static_cast<uint16_t>(math::Rand::IntUniform(100, 500)))
{
for (uint16_t b = math::MIN_UI16; b < math::MAX_UI16 - 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 = math::MAX_UI32-5000; a < math::MAX_UI32; a++)
{
for (math::PairInput b = math::MAX_UI32-5000; b < math::MAX_UI32; 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
}
}

25
ign-math/src/IndexException.cc Normal file
View File

@ -0,0 +1,25 @@
/*
* 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/IndexException.hh>
using namespace ignition;
using namespace math;
IndexException::IndexException() : std::runtime_error("Invalid index")
{}

527
ign-math/src/Inertial_TEST.cc Normal file
View File

@ -0,0 +1,527 @@
/*
* 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;
/////////////////////////////////////////////////
/// \brief Compare quaternions, but allow rotations of PI about any axis.
void CompareModuloPi(const math::Quaterniond &_q1,
const math::Quaterniond &_q2,
const double _tol = 1e-6)
{
const auto rotErrorEuler = (_q1.Inverse() * _q2).Euler();
EXPECT_NEAR(sin(rotErrorEuler.X()), 0.0, _tol);
EXPECT_NEAR(sin(rotErrorEuler.Y()), 0.0, _tol);
EXPECT_NEAR(sin(rotErrorEuler.Z()), 0.0, _tol);
}
/////////////////////////////////////////////////
// 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
// that has the same base frame MOI but no pose rotation
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
CompareModuloPi(m2.PrincipalAxesOffset(), pose.Rot());
}
}
/////////////////////////////////////////////////
// Base frame MOI should be invariant
void SetRotation(const double _mass,
const math::Vector3d &_ixxyyzz,
const math::Vector3d &_ixyxzyz,
const bool _unique = true)
{
const math::MassMatrix3d m(_mass, _ixxyyzz, _ixyxzyz);
EXPECT_TRUE(m.IsPositive());
EXPECT_TRUE(m.IsValid());
math::Pose3d pose(math::Vector3d::Zero, math::Quaterniond::Identity);
const math::Inertiald inertialRef(m, pose);
const auto moi = inertialRef.MOI();
const std::vector<math::Quaterniond> rotations = {
math::Quaterniond::Identity,
math::Quaterniond(IGN_PI, 0, 0),
math::Quaterniond(0, IGN_PI, 0),
math::Quaterniond(0, 0, IGN_PI),
math::Quaterniond(IGN_PI_2, 0, 0),
math::Quaterniond(0, IGN_PI_2, 0),
math::Quaterniond(0, 0, IGN_PI_2),
math::Quaterniond(IGN_PI_4, 0, 0),
math::Quaterniond(0, IGN_PI_4, 0),
math::Quaterniond(0, 0, IGN_PI_4),
math::Quaterniond(IGN_PI/6, 0, 0),
math::Quaterniond(0, IGN_PI/6, 0),
math::Quaterniond(0, 0, IGN_PI/6),
math::Quaterniond(0.1, 0.2, 0.3),
math::Quaterniond(-0.1, 0.2, -0.3),
math::Quaterniond(0.4, 0.2, 0.5),
math::Quaterniond(-0.1, 0.7, -0.7)};
for (const auto rot : rotations)
{
{
auto inertial = inertialRef;
const double tol = -1e-6;
EXPECT_TRUE(inertial.SetMassMatrixRotation(rot, tol));
EXPECT_EQ(moi, inertial.MOI());
if (_unique)
{
CompareModuloPi(rot, inertial.MassMatrix().PrincipalAxesOffset(tol));
}
EXPECT_TRUE(inertial.SetInertialRotation(rot));
EXPECT_EQ(rot, inertial.Pose().Rot());
EXPECT_EQ(moi, inertial.MOI());
}
{
auto inertial = inertialRef;
EXPECT_TRUE(inertial.SetInertialRotation(rot));
EXPECT_EQ(rot, inertial.Pose().Rot());
EXPECT_EQ(moi, inertial.MOI());
const double tol = -1e-6;
EXPECT_TRUE(inertial.SetMassMatrixRotation(rot, tol));
EXPECT_EQ(moi, inertial.MOI());
if (_unique)
{
CompareModuloPi(rot, inertial.MassMatrix().PrincipalAxesOffset(tol));
}
}
}
}
/////////////////////////////////////////////////
TEST(Inertiald_Test, SetRotationUniqueDiagonal)
{
SetRotation(12, math::Vector3d(2, 3, 4), math::Vector3d::Zero);
SetRotation(12, math::Vector3d(3, 2, 4), math::Vector3d::Zero);
SetRotation(12, math::Vector3d(2, 4, 3), math::Vector3d::Zero);
SetRotation(12, math::Vector3d(3, 4, 2), math::Vector3d::Zero);
SetRotation(12, math::Vector3d(4, 2, 3), math::Vector3d::Zero);
SetRotation(12, math::Vector3d(4, 3, 2), math::Vector3d::Zero);
}
/////////////////////////////////////////////////
TEST(Inertiald_Test, SetRotationUniqueNondiagonal)
{
SetRotation(12, math::Vector3d(2, 3, 4), math::Vector3d(0.3, 0.2, 0.1));
}
/////////////////////////////////////////////////
TEST(Inertiald_Test, SetRotationNonuniqueDiagonal)
{
SetRotation(12, math::Vector3d(2, 2, 2), math::Vector3d::Zero, false);
SetRotation(12, math::Vector3d(2, 2, 3), math::Vector3d::Zero, false);
SetRotation(12, math::Vector3d(2, 3, 2), math::Vector3d::Zero, false);
SetRotation(12, math::Vector3d(3, 2, 2), math::Vector3d::Zero, false);
SetRotation(12, math::Vector3d(2, 3, 3), math::Vector3d::Zero, false);
SetRotation(12, math::Vector3d(3, 2, 3), math::Vector3d::Zero, false);
SetRotation(12, math::Vector3d(3, 3, 2), math::Vector3d::Zero, false);
}
/////////////////////////////////////////////////
TEST(Inertiald_Test, SetRotationNonuniqueNondiagonal)
{
SetRotation(12, math::Vector3d(4, 4, 3), math::Vector3d(-1, 0, 0), false);
SetRotation(12, math::Vector3d(4, 3, 4), math::Vector3d(0, -1, 0), false);
SetRotation(12, math::Vector3d(3, 4, 4), math::Vector3d(0, 0, -1), false);
SetRotation(12, math::Vector3d(4, 4, 5), math::Vector3d(-1, 0, 0), false);
SetRotation(12, math::Vector3d(5, 4, 4), math::Vector3d(0, 0, -1), false);
SetRotation(12, math::Vector3d(5.5, 4.125, 4.375),
0.25*math::Vector3d(-sqrt(3), 3.0, -sqrt(3)/2), false);
SetRotation(12, math::Vector3d(4.125, 5.5, 4.375),
0.25*math::Vector3d(-sqrt(3), -sqrt(3)/2, 3.0), false);
}
/////////////////////////////////////////////////
// test for diagonalizing MassMatrix
// verify MOI is conserved
// and that off-diagonal terms are zero
void Diagonalize(
const double _mass,
const math::Vector3d &_ixxyyzz,
const math::Vector3d &_ixyxzyz)
{
const math::MassMatrix3d m(_mass, _ixxyyzz, _ixyxzyz);
EXPECT_TRUE(m.IsPositive());
EXPECT_TRUE(m.IsValid());
math::Pose3d pose(math::Vector3d::Zero, math::Quaterniond::Identity);
math::Inertiald inertial(m, pose);
const auto moi = inertial.MOI();
EXPECT_TRUE(inertial.SetMassMatrixRotation(math::Quaterniond::Identity));
EXPECT_EQ(moi, inertial.MOI());
EXPECT_EQ(inertial.MassMatrix().OffDiagonalMoments(), math::Vector3d::Zero);
// try again with negative tolerance
EXPECT_TRUE(
inertial.SetMassMatrixRotation(math::Quaterniond::Identity, -1e-6));
EXPECT_EQ(moi, inertial.MOI());
EXPECT_EQ(inertial.MassMatrix().OffDiagonalMoments(), math::Vector3d::Zero);
}
/////////////////////////////////////////////////
TEST(Inertiald_Test, Diagonalize)
{
Diagonalize(12, math::Vector3d(2, 3, 4), math::Vector3d::Zero);
Diagonalize(12, math::Vector3d(3, 2, 4), math::Vector3d::Zero);
Diagonalize(12, math::Vector3d(2, 4, 3), math::Vector3d::Zero);
Diagonalize(12, math::Vector3d(3, 4, 2), math::Vector3d::Zero);
Diagonalize(12, math::Vector3d(4, 2, 3), math::Vector3d::Zero);
Diagonalize(12, math::Vector3d(4, 3, 2), math::Vector3d::Zero);
Diagonalize(12, math::Vector3d(2, 3, 4), math::Vector3d(0.3, 0.2, 0.1));
Diagonalize(12, math::Vector3d(2, 2, 2), math::Vector3d::Zero);
Diagonalize(12, math::Vector3d(2, 2, 3), math::Vector3d::Zero);
Diagonalize(12, math::Vector3d(2, 3, 2), math::Vector3d::Zero);
Diagonalize(12, math::Vector3d(3, 2, 2), math::Vector3d::Zero);
Diagonalize(12, math::Vector3d(2, 3, 3), math::Vector3d::Zero);
Diagonalize(12, math::Vector3d(3, 2, 3), math::Vector3d::Zero);
Diagonalize(12, math::Vector3d(3, 3, 2), math::Vector3d::Zero);
Diagonalize(12, math::Vector3d(4, 4, 3), math::Vector3d(-1, 0, 0));
Diagonalize(12, math::Vector3d(4, 3, 4), math::Vector3d(0, -1, 0));
Diagonalize(12, math::Vector3d(3, 4, 4), math::Vector3d(0, 0, -1));
Diagonalize(12, math::Vector3d(4, 4, 5), math::Vector3d(-1, 0, 0));
Diagonalize(12, math::Vector3d(5, 4, 4), math::Vector3d(0, 0, -1));
Diagonalize(12, math::Vector3d(5.5, 4.125, 4.375),
0.25*math::Vector3d(-sqrt(3), 3.0, -sqrt(3)/2));
Diagonalize(12, math::Vector3d(4.125, 5.5, 4.375),
0.25*math::Vector3d(-sqrt(3), -sqrt(3)/2, 3.0));
}
/////////////////////////////////////////////////
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());
}
}

Some files were not shown because too many files have changed in this diff Show More