From 5e2f7bc712a2e29b0234cde50c53f3285f56ecb9 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Fri, 6 Feb 2026 15:55:03 +0700 Subject: [PATCH] delete tf3 --- src/Libraries/tf3/.gitignore | 18 - src/Libraries/tf3/CHANGELOG.rst | 524 ------ src/Libraries/tf3/CMakeLists.txt | 208 --- src/Libraries/tf3/README.md | 13 - .../tf3/examples/simple_tf3_example.cpp | 91 - .../tf3/include/tf3/LinearMath/Matrix3x3.h | 696 ------- .../tf3/include/tf3/LinearMath/MinMax.h | 72 - .../tf3/include/tf3/LinearMath/QuadWord.h | 183 -- .../tf3/include/tf3/LinearMath/Quaternion.h | 477 ----- .../tf3/include/tf3/LinearMath/Scalar.h | 417 ----- .../tf3/include/tf3/LinearMath/Transform.h | 305 --- .../tf3/include/tf3/LinearMath/Vector3.h | 731 -------- src/Libraries/tf3/include/tf3/TF3Error.h | 11 - src/Libraries/tf3/include/tf3/buffer_core.h | 432 ----- src/Libraries/tf3/include/tf3/compat.h | 31 - src/Libraries/tf3/include/tf3/convert.h | 131 -- src/Libraries/tf3/include/tf3/exceptions.h | 110 -- src/Libraries/tf3/include/tf3/impl/convert.h | 90 - src/Libraries/tf3/include/tf3/impl/utils.h | 142 -- src/Libraries/tf3/include/tf3/macros.h | 13 - .../tf3/include/tf3/message_traits.h | 12 - src/Libraries/tf3/include/tf3/time.h | 152 -- src/Libraries/tf3/include/tf3/time_cache.h | 155 -- .../tf3/include/tf3/transform_datatypes.h | 84 - .../tf3/include/tf3/transform_storage.h | 80 - src/Libraries/tf3/include/tf3/utils.h | 66 - src/Libraries/tf3/index.rst | 15 - src/Libraries/tf3/mainpage.dox | 36 - src/Libraries/tf3/package.xml | 25 - src/Libraries/tf3/src/buffer_core.cpp | 1645 ----------------- src/Libraries/tf3/src/cache.cpp | 338 ---- src/Libraries/tf3/src/static_cache.cpp | 81 - src/Libraries/tf3/test/cache_unittest.cpp | 414 ----- src/Libraries/tf3/test/simple_tf3_core.cpp | 320 ---- src/Libraries/tf3/test/speed_test.cpp | 252 --- src/Libraries/tf3/test/static_cache_test.cpp | 101 - .../tf3/test/test_transform_datatypes.cpp | 77 - 37 files changed, 8548 deletions(-) delete mode 100644 src/Libraries/tf3/.gitignore delete mode 100644 src/Libraries/tf3/CHANGELOG.rst delete mode 100644 src/Libraries/tf3/CMakeLists.txt delete mode 100644 src/Libraries/tf3/README.md delete mode 100644 src/Libraries/tf3/examples/simple_tf3_example.cpp delete mode 100644 src/Libraries/tf3/include/tf3/LinearMath/Matrix3x3.h delete mode 100644 src/Libraries/tf3/include/tf3/LinearMath/MinMax.h delete mode 100644 src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h delete mode 100644 src/Libraries/tf3/include/tf3/LinearMath/Quaternion.h delete mode 100644 src/Libraries/tf3/include/tf3/LinearMath/Scalar.h delete mode 100644 src/Libraries/tf3/include/tf3/LinearMath/Transform.h delete mode 100644 src/Libraries/tf3/include/tf3/LinearMath/Vector3.h delete mode 100644 src/Libraries/tf3/include/tf3/TF3Error.h delete mode 100644 src/Libraries/tf3/include/tf3/buffer_core.h delete mode 100644 src/Libraries/tf3/include/tf3/compat.h delete mode 100644 src/Libraries/tf3/include/tf3/convert.h delete mode 100644 src/Libraries/tf3/include/tf3/exceptions.h delete mode 100644 src/Libraries/tf3/include/tf3/impl/convert.h delete mode 100644 src/Libraries/tf3/include/tf3/impl/utils.h delete mode 100644 src/Libraries/tf3/include/tf3/macros.h delete mode 100644 src/Libraries/tf3/include/tf3/message_traits.h delete mode 100644 src/Libraries/tf3/include/tf3/time.h delete mode 100644 src/Libraries/tf3/include/tf3/time_cache.h delete mode 100644 src/Libraries/tf3/include/tf3/transform_datatypes.h delete mode 100644 src/Libraries/tf3/include/tf3/transform_storage.h delete mode 100644 src/Libraries/tf3/include/tf3/utils.h delete mode 100644 src/Libraries/tf3/index.rst delete mode 100644 src/Libraries/tf3/mainpage.dox delete mode 100644 src/Libraries/tf3/package.xml delete mode 100644 src/Libraries/tf3/src/buffer_core.cpp delete mode 100644 src/Libraries/tf3/src/cache.cpp delete mode 100644 src/Libraries/tf3/src/static_cache.cpp delete mode 100644 src/Libraries/tf3/test/cache_unittest.cpp delete mode 100644 src/Libraries/tf3/test/simple_tf3_core.cpp delete mode 100644 src/Libraries/tf3/test/speed_test.cpp delete mode 100644 src/Libraries/tf3/test/static_cache_test.cpp delete mode 100644 src/Libraries/tf3/test/test_transform_datatypes.cpp diff --git a/src/Libraries/tf3/.gitignore b/src/Libraries/tf3/.gitignore deleted file mode 100644 index b098b7a..0000000 --- a/src/Libraries/tf3/.gitignore +++ /dev/null @@ -1,18 +0,0 @@ -# .gitignore for a C++ project using CMake -/build/ -*.o -*.obj -*.exe -*.out -*.log - -# IDE specific files -.vscode/ -.idea/ -*.user - -# CMake temp files -CMakeCache.txt -CMakeFiles/ -cmake_install.cmake -Makefile diff --git a/src/Libraries/tf3/CHANGELOG.rst b/src/Libraries/tf3/CHANGELOG.rst deleted file mode 100644 index 139c4eb..0000000 --- a/src/Libraries/tf3/CHANGELOG.rst +++ /dev/null @@ -1,524 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package tf2 -^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.10 (2025-05-02) -------------------- -* Fix race conditions in MessageFilter (`#539 `_) -* Contributors: Robert Haschke - -0.7.9 (2025-04-25) ------------------- - -0.7.8 (2025-04-10) ------------------- -* Longer char array for null termination needed (`#514 `_) -* Fixed error message when fixed_frame is not found (`#559 `_) -* Add missing #include to buffer_core.cpp (`#558 `_) -* Contributors: Lucas Walter, Martin Pecka, vslashg - -0.7.7 (2023-10-13) ------------------- -* fix extra comma that gives annoying build warnings with -Wall and -Wpedantic with g++-9 and assuming most other compilers (`#550 `_) -* Add parent frame to warning logs (`#533 `_) -* Contributors: Jack Zender, Stephan - -0.7.6 (2022-10-11) ------------------- -* Fix dead loop in message filter (`#532 `_) -* Restore time difference order so future extrapolation exceptions don't show non-sensical negative seconds into the future (`#522 `_) -* Contributors: Feng Zhaolin, Lucas Walter - -0.7.5 (2020-09-01) ------------------- -* restore buffer sizes -* Contributors: Tully Foote - -0.7.4 (2020-09-01) ------------------- -* Fix potential buffer overrun of snprintf (`#479 `_) -* Contributors: Atsushi Watanabe - -0.7.3 (2020-08-25) ------------------- -* Use snprintf instead of stringstream to increase performance of lookupTransform() in error cases. -* Do not waste time constructing error string if nobody is interested in it in canTransform(). (`#469 `_) -* Output time difference of extrapolation exceptions (`#477 `_) -* Cherry-picking various commits from Melodic (`#471 `_) - * Revert "rework Eigen functions namespace hack" (`#436 `_) - * Fixed warnings in message_filter.h (`#434 `_) - the variables are not used in function body and caused -Wunused-parameter to trigger with -Wall - * Fix ambiguous call for tf2::convert on MSVC (`#444 `_) - * rework ambiguous call on MSVC. -* Contributors: Lucas Walter, Martin Pecka, Robert Haschke - -0.7.2 (2020-06-08) ------------------- - -0.7.1 (2020-05-13) ------------------- -* Fix to improper ring_45 test, where 'anchor' frame for both inverse and normal transform was frame 'b' instead of frame 'a', thus creating a problem -* Don't insert a TF frame is one of the same timestamp already exists, instead just overwrite it. -* [Noetic] Add tf2::Stamped::operator=() to fix warnings downstream (`#453 `_) - * Add tf2::Stamped::operator=() -* [noetic] cherry-pick Windows fixes from melodic-devel (`#450 `_) - * [Windows][melodic-devel] Fix install locations (`#442 `_) - * fixed install locations of tf2 - * [windows][melodic] more portable fixes. (`#443 `_) - * more portable fixes. -* Contributors: Patrick Beeson, Robert Haschke, Sean Yen, Shane Loretz - -0.7.0 (2020-03-09) ------------------- -* Bump CMake version to avoid CMP0048 warning (`#445 `_) - Signed-off-by: Shane Loretz -* Fix compile error missing ros/ros.h (`#400 `_) - * ros/ros.h -> ros/time.h - tf2 package depends on rostime - * tf2_bullet doesn't need ros.h - Signed-off-by: Shane Loretz - * tf2_eigen doesn't need ros/ros.h - Signed-off-by: Shane Loretz -* Merge pull request `#367 `_ from kejxu/add_tf2_namespace_to_avoid_name_collision - rework Eigen functions namespace hack -* separate transform function declarations into transform_functions.h -* use ROS_DEPRECATED macro for portability (`#362 `_) - * use ROS_DEPRECATED for better portability - * change ROS_DEPRECATED position (`#5 `_) -* Remove `signals` from find_package(Boost COMPONENTS ...). - tf2 is using signals2, which is not the same library. - Additionally, signals2 has always been header only, and header only - libraries must not be listed in find_package. - Boost 1.69 removed the old signals library entirely, so the otherwise - useless `COMPONENTS signals` actually breaks the build. -* Remove legacy inclusion in CMakeLists of tf2. -* Contributors: James Xu, Maarten de Vries, Marco Tranzatto, Shane Loretz, Tully Foote - -0.6.5 (2018-11-16) ------------------- - -0.6.4 (2018-11-06) ------------------- -* Resolved pedantic warnings -* fix issue `#315 `_ -* fixed nan interpoaltion issue -* Contributors: Keller Fabian Rudolf (CC-AD/EYC3), Kuang Fangjun, Martin Ganeff - -0.6.3 (2018-07-09) ------------------- -* preserve constness of const argument to avoid warnings (`#307 `_) -* Change comment style for unused doxygen (`#297 `_) -* Contributors: Jacob Perron, Tully Foote - -0.6.2 (2018-05-02) ------------------- - -0.6.1 (2018-03-21) ------------------- -* Replaced deprecated console_bridge macro calls (tests) -* Contributors: Johannes Meyer, Tully Foote - -0.6.0 (2018-03-21) ------------------- -* Replaced deprecated log macro calls -* Contributors: Tim Rakowski, Tully Foote - -0.5.17 (2018-01-01) -------------------- -* Merge pull request `#278 `_ from ros/chain_as_vec_test2 - Clean up results of _chainAsVector -* Simple test to check BufferCore::_chainAsVector. - Unit tests for walk and chain passing now. -* Merge pull request `#267 `_ from at-wat/speedup-timecache-for-large-buffer - Speed-up TimeCache search for large cache time. -* Merge pull request `#265 `_ from vsherrod/interpolation_fix - Corrected time output on interpolation function. -* Add time_interval option to tf2 speed-test. -* Merge pull request `#269 `_ from ros/frames_as_yaml - allFrameAsYaml consistently outputting a dict -* resolve https://github.com/ros/geometry/pull/153 at the source instead of needing the workaround. -* Speed-up TimeCache search for large cache time. -* Modified tests for correct time in interpolation to existing tests. -* Corrected time output on interpolation function. - Added unit test to check for this. -* Contributors: Atsushi Watanabe, Miguel Prada, Tully Foote, Vallan Sherrod - -0.5.16 (2017-07-14) -------------------- -* remove explicit templating to standardize on overloading. But provide backwards compatibility with deprecation. -* Merge pull request `#144 `_ from clearpathrobotics/dead_lock_fix - Solve a bug that causes a deadlock in MessageFilter -* Resolve 2 places where the error_msg would not be propogated. - Fixes `#198 `_ -* Remove generate_rand_vectors() from a number of tests. (`#227 `_) -* fixing include directory order to support overlays (`#231 `_) -* replaced dependencies on tf2_msgs_gencpp by exported dependencies -* Document the lifetime of the returned reference for getFrameId getTimestamp -* relax normalization tolerance. `#196 `_ was too strict for some use cases. (`#220 `_) -* Solve a bug that causes a deadlock in MessageFilter -* Contributors: Adel Fakih, Chris Lalancette, Christopher Wecht, Tully Foote, dhood - -0.5.15 (2017-01-24) -------------------- - -0.5.14 (2017-01-16) -------------------- -* fixes `#194 `_ check for quaternion normalization before inserting into storage (`#196 `_) - * check for quaternion normalization before inserting into storage - * Add test to check for transform failure on invalid quaternion input -* updating getAngleShortestPath() (`#187 `_) -* Move internal cache functions into a namespace - Fixes https://github.com/ros/geometry2/issues/175 -* Link properly to convert.h -* Landing page for tf2 describing the conversion interface -* Fix comment on BufferCore::MAX_GRAPH_DEPTH. -* Contributors: Jackie Kay, Phil Osteen, Tully Foote, alex, gavanderhoorn - -0.5.13 (2016-03-04) -------------------- - -0.5.12 (2015-08-05) -------------------- -* add utilities to get yaw, pitch, roll and identity transform -* provide more conversions between types - The previous conversion always assumed that it was converting a - non-message type to a non-message type. Now, one, both or none - can be a message or a non-message. -* Contributors: Vincent Rabaud - -0.5.11 (2015-04-22) -------------------- - -0.5.10 (2015-04-21) -------------------- -* move lct_cache into function local memoryfor `#92 `_ -* Clean up range checking. Re: `#92 `_ -* Fixed chainToVector -* release lock before possibly invoking user callbacks. Fixes `#91 `_ -* Contributors: Jackie Kay, Tully Foote - -0.5.9 (2015-03-25) ------------------- -* fixing edge case where two no frame id lookups matched in getLatestCommonTime -* Contributors: Tully Foote - -0.5.8 (2015-03-17) ------------------- -* change from default argument to overload to avoid linking issue `#84 `_ -* remove useless Makefile files -* Remove unused assignments in max/min functions -* change _allFramesAsDot() -> _allFramesAsDot(double current_time) -* Contributors: Jon Binney, Kei Okada, Tully Foote, Vincent Rabaud - -0.5.7 (2014-12-23) ------------------- - -0.5.6 (2014-09-18) ------------------- - -0.5.5 (2014-06-23) ------------------- -* convert to use console bridge from upstream debian package https://github.com/ros/rosdistro/issues/4633 -* Fix format string -* Contributors: Austin, Tully Foote - -0.5.4 (2014-05-07) ------------------- -* switch to boost signals2 following `ros/ros_comm#267 `_, blocking `ros/geometry#23 `_ -* Contributors: Tully Foote - -0.5.3 (2014-02-21) ------------------- - -0.5.2 (2014-02-20) ------------------- - -0.5.1 (2014-02-14) ------------------- - -0.5.0 (2014-02-14) ------------------- - -0.4.10 (2013-12-26) -------------------- -* updated error message. fixes `#38 `_ -* tf2: add missing console bridge include directories (fix `#48 `_) -* Fix const correctness of tf2::Vector3 rotate() method - The method does not modify the class thus should be const. - This has already been fixed in Bullet itself. -* Contributors: Dirk Thomas, Timo Rohling, Tully Foote - -0.4.9 (2013-11-06) ------------------- - -0.4.8 (2013-11-06) ------------------- -* moving python documentation to tf2_ros from tf2 to follow the code -* removing legacy rospy dependency. implementation removed in 0.4.0 fixes `#27 `_ - -0.4.7 (2013-08-28) ------------------- -* switching to use allFramesAsStringNoLock inside of getLatestCommonTime and walkToParent and locking in public API _getLatestCommonTime instead re `#23 `_ -* Fixes a crash in tf's view_frames related to dot code generation in allFramesAsDot - -0.4.6 (2013-08-28) ------------------- -* cleaner fix for `#19 `_ -* fix pointer initialization. Fixes `#19 `_ -* fixes `#18 `_ for hydro -* package.xml: corrected typo in description - -0.4.5 (2013-07-11) ------------------- -* adding _chainAsVector method for https://github.com/ros/geometry/issues/18 -* adding _allFramesAsDot for backwards compatability https://github.com/ros/geometry/issues/18 - -0.4.4 (2013-07-09) ------------------- -* making repo use CATKIN_ENABLE_TESTING correctly and switching rostest to be a test_depend with that change. -* tf2: Fixes a warning on OS X, but generally safer - Replaces the use of pointers with shared_ptrs, - this allows the polymorphism and makes it so that - the compiler doesn't yell at us about calling - delete on a class with a public non-virtual - destructor. -* tf2: Fixes compiler warnings on OS X - This exploited a gcc specific extension and is not - C++ standard compliant. There used to be a "fix" - for OS X which no longer applies. I think it is ok - to use this as an int instead of a double, but - another way to fix it would be to use a define. -* tf2: Fixes linkedit errors on OS X - -0.4.3 (2013-07-05) ------------------- - -0.4.2 (2013-07-05) ------------------- -* adding getCacheLength() to parallel old tf API -* removing legacy static const variable MAX_EXTRAPOLATION_DISTANCE copied from tf unnecessesarily - -0.4.1 (2013-07-05) ------------------- -* adding old style callback notifications to BufferCore to enable backwards compatability of message filters -* exposing dedicated thread logic in BufferCore and checking in Buffer -* more methods to expose, and check for empty cache before getting latest timestamp -* adding methods to enable backwards compatability for passing through to tf::Transformer - -0.4.0 (2013-06-27) ------------------- -* splitting rospy dependency into tf2_py so tf2 is pure c++ library. -* switching to console_bridge from rosconsole -* moving convert methods back into tf2 because it does not have any ros dependencies beyond robot::Time which is already a dependency of tf2 -* Cleaning up unnecessary dependency on roscpp -* Cleaning up packaging of tf2 including: - removing unused nodehandle - fixing overmatch on search and replace - cleaning up a few dependencies and linking - removing old backup of package.xml - making diff minimally different from tf version of library -* suppressing bullet LinearMath copy inside of tf2, so it will not collide, and should not be used externally. -* Restoring test packages and bullet packages. - reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented - reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ -* fixing includes in unit tests -* Make PythonLibs find_package python2 specific - On systems with python 3 installed and default, find_package(PythonLibs) will find the python 3 paths and libraries. However, the c++ include structure seems to be different in python 3 and tf2 uses includes that are no longer present or deprecated. - Until the includes are made to be python 3 compliant, we should specify that the version of python found must be python 2. - -0.3.6 (2013-03-03) ------------------- - -0.3.5 (2013-02-15 14:46) ------------------------- -* 0.3.4 -> 0.3.5 - -0.3.4 (2013-02-15 13:14) ------------------------- -* 0.3.3 -> 0.3.4 -* moving LinearMath includes to include/tf2 - -0.3.3 (2013-02-15 11:30) ------------------------- -* 0.3.2 -> 0.3.3 -* fixing include installation of tf2 - -0.3.2 (2013-02-15 00:42) ------------------------- -* 0.3.1 -> 0.3.2 -* fixed missing include export & tf2_ros dependecy - -0.3.1 (2013-02-14) ------------------- -* 0.3.0 -> 0.3.1 -* fixing PYTHON installation directory - -0.3.0 (2013-02-13) ------------------- -* switching to version 0.3.0 -* adding setup.py to tf2 package -* fixed tf2 exposing python functionality -* removed line that was killing tf2_ros.so -* fixing catkin message dependencies -* removing packages with missing deps -* adding missing package.xml -* adding missing package.xml -* adding missing package.xml -* catkinizing geometry-experimental -* removing bullet headers from use in header files -* removing bullet headers from use in header files -* merging my recent changes -* setting child_frame_id overlooked in revision 6a0eec022be0 which fixed failing tests -* allFramesAsString public and internal methods seperated. Public method is locked, private method is not -* fixing another scoped lock -* fixing one scoped lock -* fixing test compilation -* merge -* Error message fix, ros-pkg5085 -* Check if target equals to source before validation -* When target_frame == source_frame, just returns an identity transform. -* adding addition ros header includes for strictness -* Fixed optimized lookups with compound transforms -* Fixed problem in tf2 optimized branch. Quaternion multiplication order was incorrect -* fix compilation on 32-bit -* Josh fix: Final inverse transform composition (missed multiplying the sourcd->top vector by the target->top inverse orientation). b44877d2b054 -* Josh change: fix first/last time case. 46bf33868e0d -* fix transform accumulation to parent -* fix parent lookup, now works on the real pr2's tree -* move the message filter to tf2_ros -* tf2::MessageFilter + tests. Still need to change it around to pass in a callback queue, since we're being triggered directly from the tf2 buffer -* Don't add the request if the transform is already available. Add some new tests -* working transformable callbacks with a simple (incomplete) test case -* first pass at a transformable callback api, not tested yet -* add interpolation cases -* fix getLatestCommonTime -- no longer returns the latest of any of the times -* Some more optimization -- allow findClosest to inline -* another minor speedup -* Minorly speed up canTransform by not requiring the full data lookup, and only looking up the parent -* Add explicit operator= so that we can see the time in it on a profile graph. Also some minor cleanup -* minor cleanup -* add 3 more cases to the speed test -* Remove use of btTransform at all from transform accumulation, since the conversion to/from is unnecessary, expensive, and can introduce floating point error -* Don't use btTransform as an intermediate when accumulating transforms, as constructing them takes quite a bit of time -* Completely remove lookupLists(). canTransform() now uses the same walking code as lookupTransform(). Also fixed a bug in the static transform publisher test -* Genericise the walk-to-top-parent code in lookupTransform so that it will be able to be used by canTransform as well (minus the cost of actually computing the transform) -* remove id lookup that wasn't doing anything -* Some more optimization: - * Reduce # of TransformStorage copies made in TimeCache::getData() - * Remove use of lookupLists from getLatestCommonTime -* lookupTransform() no longer uses lookupLists unless it's called with Time(0). Removes lots of object construction/destruction due to removal of pushing back on the lists -* Remove CompactFrameID in favor of a typedef -* these mode checks are no longer necessary -* Fix crash when testing extrapolation on the forward transforms -* Update cache unit tests to work with the changes TransformStorage. - Also make sure that BT_USE_DOUBLE_PRECISION is set for tf2. -* remove exposure of time_cache.h from buffer_core.h -* Removed the mutex from TimeCache, as it's unnecessary (BufferCore needs to have its own mutex locked anyway), and this speeds things up by about 20% - Also fixed a number of thread-safety problems -* Optimize test_extrapolation a bit, 25% speedup of lookupTransform -* use a hash map for looking up frame numbers, speeds up lookupTransform by ~8% -* Cache vectors used for looking up transforms. Speeds up lookupTransform by another 10% -* speed up lookupTransform by another 25% -* speed up lookupTransform by another 2x. also reduces the memory footprint of the cache significantly -* sped up lookupTransform by another 2x -* First add of a simple speed test - Sped up lookupTransform 2x -* roscpp dependency explicit, instead of relying on implicit -* static transform tested and working -* tests passing and all throw catches removed too\! -* validating frame_ids up front for lookup exceptions -* working with single base class vector -* tests passing for static storage -* making method private for clarity -* static cache implementation and test -* cleaning up API doc typos -* sphinx docs for Buffer -* new dox mainpage -* update tf2 manifest -* commenting out twist -* Changed cache_time to cache_time_ to follow C++ style guide, also initialized it to actually get things to work -* no more rand in cache tests -* Changing tf2_py.cpp to use underscores instead of camelCase -* removing all old converter functions from transform_datatypes.h -* removing last references to transform_datatypes.h in tf2 -* transform conversions internalized -* removing unused datatypes -* copying bullet transform headers into tf2 and breaking bullet dependency -* merge -* removing dependency on tf -* removing include of old tf from tf2 -* update doc -* merge -* kdl unittest passing -* Spaces instead of tabs in YAML grrrr -* Adding quotes for parent -* canTransform advanced ported -* Hopefully fixing YAML syntax -* new version of view_frames in new tf2_tools package -* testing new argument validation and catching bug -* Python support for debugging -* merge -* adding validation of frame_ids in queries with warnings and exceptions where appropriate -* Exposing ability to get frames as a string -* A compiling version of YAML debugging interface for BufferCore -* placeholder for tf debug -* fixing tf:: to tf2:: ns issues and stripping slashes on set in tf2 for backwards compatiabily -* Adding a python version of the BufferClient -* moving test to new package -* merging -* working unit test for BufferCore::lookupTransform -* removing unused method test and converting NO_PARENT test to new API -* Adding some comments -* Moving the python bindings for tf2 to the tf2 package from the tf2_py package -* buffercore tests upgraded -* porting tf_unittest while running incrmentally instead of block copy -* BufferCore::clear ported forward -* successfully changed lookupTransform advanced to new version -* switching to new implementation of lookupTransform tests still passing -* compiling lookupTransform new version -* removing tf_prefix from BufferCore. BuferCore is independent of any frame_ids. tf_prefix should be implemented at the ROS API level. -* initializing tf_prefix -* adding missing initialization -* suppressing warnings -* more tests ported -* removing tests for apis not ported forward -* setTransform tests ported -* old tests in new package passing due to backwards dependency. now for the fun, port all 1500 lines :-) -* setTransform working in new framework as well as old -* porting more methods -* more compatability -* bringing in helper functions for buffer_core from tf.h/cpp -* rethrowing to new exceptions -* converting Storage to robot_geometry_msgs::TransformStamped -* removing deprecated useage -* cleaning up includes -* moving all implementations into cpp file -* switching test to new class from old one -* Compiling version of the buffer client -* moving listener to tf_cpp -* removing listener, it should be in another package -* most of listener -* add cantransform implementation -* removing deprecated API usage -* initial import of listener header -* move implementation into library -* 2 tests of buffer -* moving executables back into bin -* compiling again with new design -* rename tfcore to buffercore -* almost compiling version of template code -* compiling tf2_core simple test -* add test to start compiling -* copying in tf_unittest for tf_core testing template -* prototype of tf2_core implemented using old tf. -* first version of template functions -* remove timeouts -* properly naming tf2_core.h from tf_core.h -* working cache test with tf2 lib -* first unit test passing, not yet ported -* tf_core api -* tf2 v2 -* aborting port -* moving across time cache tf and datatypes headers -* copying exceptions from tf -* switching to tf2 from tf_core diff --git a/src/Libraries/tf3/CMakeLists.txt b/src/Libraries/tf3/CMakeLists.txt deleted file mode 100644 index 450e89c..0000000 --- a/src/Libraries/tf3/CMakeLists.txt +++ /dev/null @@ -1,208 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(tf3 VERSION 1.0.0 LANGUAGES CXX) - -if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL) - set(BUILDING_WITH_CATKIN TRUE) - message(STATUS "Building tf3 with Catkin") - -else() - set(BUILDING_WITH_CATKIN FALSE) - message(STATUS "Building tf3 with Standalone CMake") -endif() - -# C++ Standard - must be set before find_package -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -# Find dependencies -find_package(console_bridge REQUIRED) -find_package(Boost REQUIRED COMPONENTS system thread) - -if (NOT BUILDING_WITH_CATKIN) - - # Enable Position Independent Code - set(CMAKE_POSITION_INDEPENDENT_CODE ON) - - # Cấu hình RPATH để tránh cycle trong runtime search path - set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE) - set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) - set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}") - -else() - -# ======================================================== -# Catkin specific configuration -# ======================================================== - find_package(catkin REQUIRED) - - catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - DEPENDS console_bridge Boost - ) - - include_directories( - include - ${catkin_INCLUDE_DIRS} - ${console_bridge_INCLUDE_DIRS} - ) -endif() - -# Libraries -add_library(${PROJECT_NAME} SHARED - src/cache.cpp - src/buffer_core.cpp - src/static_cache.cpp -) - -if(BUILDING_WITH_CATKIN) - add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - - target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ - ) - - target_link_libraries(${PROJECT_NAME} - PUBLIC ${catkin_LIBRARIES} - PRIVATE Boost::boost Boost::system Boost::thread - PRIVATE console_bridge::console_bridge - ) - -else() - - target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ - ) - - target_link_libraries(${PROJECT_NAME} - PUBLIC - Boost::boost Boost::system Boost::thread - console_bridge::console_bridge - ) - - set_target_properties(${PROJECT_NAME} PROPERTIES - LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} - BUILD_RPATH "${CMAKE_BINARY_DIR}" - INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" - ) - -endif() - -if(BUILDING_WITH_CATKIN) - ## Mark libraries for installation - ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html - install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} - ) - - ## Mark cpp header files for installation - install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE - ) - -else() - - install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME}-targets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - ) - - # Export targets - install(EXPORT ${PROJECT_NAME}-targets - FILE ${PROJECT_NAME}-targets.cmake - NAMESPACE ${PROJECT_NAME}:: - DESTINATION lib/cmake/${PROJECT_NAME} - ) - - ## Mark cpp header files for installation - install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE - ) - - # Print configuration info - message(STATUS "=================================") - message(STATUS "Project: ${PROJECT_NAME}") - message(STATUS "Version: ${PROJECT_VERSION}") - message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") - message(STATUS "Dependencies: console_bridge, Boost") - message(STATUS "=================================") -endif() - -# ======================================================== -# Example executables -# ======================================================== -if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/examples/simple_tf3_example.cpp) - add_executable(${PROJECT_NAME}_example examples/simple_tf3_example.cpp) - target_include_directories(${PROJECT_NAME}_example PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) - target_link_libraries(${PROJECT_NAME}_example PRIVATE - ${PROJECT_NAME} - pthread - console_bridge::console_bridge - ) -endif() - -# ======================================================== -# Test executables -# ======================================================== -option(BUILD_TESTING "Build tests" OFF) -if(BUILD_TESTING) - enable_testing() - - if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/cache_unittest.cpp) - add_executable(${PROJECT_NAME}_cache_test test/cache_unittest.cpp) - target_link_libraries(${PROJECT_NAME}_cache_test PRIVATE - ${PROJECT_NAME} - console_bridge::console_bridge - ) - add_test(NAME ${PROJECT_NAME}_cache_test COMMAND ${PROJECT_NAME}_cache_test) - endif() - - if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/static_cache_test.cpp) - add_executable(${PROJECT_NAME}_static_cache_test test/static_cache_test.cpp) - target_link_libraries(${PROJECT_NAME}_static_cache_test PRIVATE - ${PROJECT_NAME} - console_bridge::console_bridge - ) - add_test(NAME ${PROJECT_NAME}_static_cache_test COMMAND ${PROJECT_NAME}_static_cache_test) - endif() - - if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/simple_tf3_core.cpp) - add_executable(${PROJECT_NAME}_simple_test test/simple_tf3_core.cpp) - target_link_libraries(${PROJECT_NAME}_simple_test PRIVATE - ${PROJECT_NAME} - console_bridge::console_bridge - ) - add_test(NAME ${PROJECT_NAME}_simple_test COMMAND ${PROJECT_NAME}_simple_test) - endif() - - if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/speed_test.cpp) - add_executable(${PROJECT_NAME}_speed_test test/speed_test.cpp) - target_link_libraries(${PROJECT_NAME}_speed_test PRIVATE - ${PROJECT_NAME} - console_bridge::console_bridge - ) - add_test(NAME ${PROJECT_NAME}_speed_test COMMAND ${PROJECT_NAME}_speed_test) - endif() - - if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/test_transform_datatypes.cpp) - add_executable(${PROJECT_NAME}_transform_datatypes_test test/test_transform_datatypes.cpp) - target_link_libraries(${PROJECT_NAME}_transform_datatypes_test PRIVATE - ${PROJECT_NAME} - console_bridge::console_bridge - ) - add_test(NAME ${PROJECT_NAME}_transform_datatypes_test COMMAND ${PROJECT_NAME}_transform_datatypes_test) - endif() -endif() diff --git a/src/Libraries/tf3/README.md b/src/Libraries/tf3/README.md deleted file mode 100644 index 2ca5db2..0000000 --- a/src/Libraries/tf3/README.md +++ /dev/null @@ -1,13 +0,0 @@ -# Requirements: -- Ubuntu 20.04.6 LTS / 22.04 LTS - -# Build tf3 project: -- Build and install tf3 library: - ``` - cd tf3 - mkdir -p build - cd build - cmake .. -DBUILD_SHARED_LIBS=ON -DCMAKE_POSITION_INDEPENDENT_CODE=ON -DCMAKE_INSTALL_PREFIX=/usr/local - make -j$(nproc) - make install - ``` \ No newline at end of file diff --git a/src/Libraries/tf3/examples/simple_tf3_example.cpp b/src/Libraries/tf3/examples/simple_tf3_example.cpp deleted file mode 100644 index a599d28..0000000 --- a/src/Libraries/tf3/examples/simple_tf3_example.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include -#include -#include - -#include "tf3/buffer_core.h" -#include "tf3/compat.h" -#include "tf3/LinearMath/Transform.h" - -using namespace tf3; - -static void printTransform(const TransformStampedMsg & t) -{ - std::cout << "Transform: [" << t.header.frame_id << "] -> [" << t.child_frame_id << "]\n"; - std::cout << " stamp: " << t.header.stamp.toSec() << "\n"; - std::cout << " translation: (" << t.transform.translation.x << ", " << t.transform.translation.y << ", " << t.transform.translation.z << ")\n"; - std::cout << " rotation: (" << t.transform.rotation.x << ", " << t.transform.rotation.y << ", " << t.transform.rotation.z << ", " << t.transform.rotation.w << ")\n"; -} - -int main() -{ - // Create a buffer with 10 seconds cache - BufferCore buffer(tf3::Duration(10)); - - // Create a transform from "world" -> "robot" - TransformStampedMsg world_to_robot; - world_to_robot.header.stamp = tf3::Time::fromSec(1.0); - world_to_robot.header.frame_id = "world"; - world_to_robot.child_frame_id = "robot"; - world_to_robot.transform.translation.x = 1.0; - world_to_robot.transform.translation.y = 2.0; - world_to_robot.transform.translation.z = 0.0; - // identity rotation - world_to_robot.transform.rotation.x = 0.0; - world_to_robot.transform.rotation.y = 0.0; - world_to_robot.transform.rotation.z = 0.0; - world_to_robot.transform.rotation.w = 1.0; - - // Insert the transform into the buffer (this simulates broadcasting) - buffer.setTransform(world_to_robot, "example_authority", false); - - std::cout << "Inserted world->robot transform:\n"; - printTransform(world_to_robot); - - // Create another transform robot -> sensor - TransformStampedMsg robot_to_sensor; - robot_to_sensor.header.stamp = tf3::Time::fromSec(1.0); - robot_to_sensor.header.frame_id = "robot"; - robot_to_sensor.child_frame_id = "sensor"; - robot_to_sensor.transform.translation.x = 0.1; - robot_to_sensor.transform.translation.y = 0.0; - robot_to_sensor.transform.translation.z = 0.2; - robot_to_sensor.transform.rotation.x = 0.0; - robot_to_sensor.transform.rotation.y = 0.0; - robot_to_sensor.transform.rotation.z = 0.0; - robot_to_sensor.transform.rotation.w = 1.0; - - buffer.setTransform(robot_to_sensor, "example_authority", false); - std::cout << "Inserted robot->sensor transform:\n"; - printTransform(robot_to_sensor); - - // Query: get transform from sensor -> world (composed path) - try - { - // lookupTransform(target_frame, source_frame, time) - // returns the transform that maps a point in `source_frame` into `target_frame`. - TransformStampedMsg sensor_to_world = buffer.lookupTransform("world", "sensor", tf3::Time()); - std::cout << "Lookup: sensor -> world (composed):\n"; - printTransform(sensor_to_world); - } - catch (const std::exception & ex) - { - std::cerr << "lookupTransform failed: " << ex.what() << std::endl; - } - - // Simulate broadcasting updates over time (update robot position) - for (int i = 0; i < 3; ++i) - { - double tsec = 2.0 + i * 1.0; - world_to_robot.header.stamp = tf3::Time::fromSec(tsec); - world_to_robot.transform.translation.x += 0.5; // robot moves in x - buffer.setTransform(world_to_robot, "example_authority", false); - std::cout << "Updated world->robot at t=" << tsec << "\n"; - // lookup at latest - TransformStampedMsg latest = buffer.lookupTransform("world", "sensor", tf3::Time()); - std::cout << "Latest composed transform (sensor->world):\n"; - printTransform(latest); - std::this_thread::sleep_for(std::chrono::milliseconds(200)); - } - - return 0; -} diff --git a/src/Libraries/tf3/include/tf3/LinearMath/Matrix3x3.h b/src/Libraries/tf3/include/tf3/LinearMath/Matrix3x3.h deleted file mode 100644 index d8a3044..0000000 --- a/src/Libraries/tf3/include/tf3/LinearMath/Matrix3x3.h +++ /dev/null @@ -1,696 +0,0 @@ -/* -Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef TF3_MATRIX3x3_H -#define TF3_MATRIX3x3_H - -#include "Vector3.h" -#include "Quaternion.h" - -#include "../macros.h" - -namespace tf3 -{ - - -#define Matrix3x3Data Matrix3x3DoubleData - - -/**@brief The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Quaternion, Transform and Vector3. -* Make sure to only include a pure orthogonal matrix without scaling. */ -class Matrix3x3 { - - ///Data storage for the matrix, each vector is a row of the matrix - Vector3 m_el[3]; - -public: - /** @brief No initializaion constructor */ - Matrix3x3 () {} - - // explicit Matrix3x3(const tf3Scalar *m) { setFromOpenGLSubMatrix(m); } - - /**@brief Constructor from Quaternion */ - explicit Matrix3x3(const Quaternion& q) { setRotation(q); } - /* - template - Matrix3x3(const tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& roll) - { - setEulerYPR(yaw, pitch, roll); - } - */ - /** @brief Constructor with row major formatting */ - Matrix3x3(const tf3Scalar& xx, const tf3Scalar& xy, const tf3Scalar& xz, - const tf3Scalar& yx, const tf3Scalar& yy, const tf3Scalar& yz, - const tf3Scalar& zx, const tf3Scalar& zy, const tf3Scalar& zz) - { - setValue(xx, xy, xz, - yx, yy, yz, - zx, zy, zz); - } - /** @brief Copy constructor */ - TF3SIMD_FORCE_INLINE Matrix3x3 (const Matrix3x3& other) - { - m_el[0] = other.m_el[0]; - m_el[1] = other.m_el[1]; - m_el[2] = other.m_el[2]; - } - - - /** @brief Assignment Operator */ - TF3SIMD_FORCE_INLINE Matrix3x3& operator=(const Matrix3x3& other) - { - m_el[0] = other.m_el[0]; - m_el[1] = other.m_el[1]; - m_el[2] = other.m_el[2]; - return *this; - } - - - /** @brief Get a column of the matrix as a vector - * @param i Column number 0 indexed */ - TF3SIMD_FORCE_INLINE Vector3 getColumn(int i) const - { - return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]); - } - - - /** @brief Get a row of the matrix as a vector - * @param i Row number 0 indexed */ - TF3SIMD_FORCE_INLINE const Vector3& getRow(int i) const - { - tf3FullAssert(0 <= i && i < 3); - return m_el[i]; - } - - /** @brief Get a mutable reference to a row of the matrix as a vector - * @param i Row number 0 indexed */ - TF3SIMD_FORCE_INLINE Vector3& operator[](int i) - { - tf3FullAssert(0 <= i && i < 3); - return m_el[i]; - } - - /** @brief Get a const reference to a row of the matrix as a vector - * @param i Row number 0 indexed */ - TF3SIMD_FORCE_INLINE const Vector3& operator[](int i) const - { - tf3FullAssert(0 <= i && i < 3); - return m_el[i]; - } - - /** @brief Multiply by the target matrix on the right - * @param m Rotation matrix to be applied - * Equivilant to this = this * m */ - Matrix3x3& operator*=(const Matrix3x3& m); - - /** @brief Set from a carray of tf3Scalars - * @param m A pointer to the beginning of an array of 9 tf3Scalars */ - void setFromOpenGLSubMatrix(const tf3Scalar *m) - { - m_el[0].setValue(m[0],m[4],m[8]); - m_el[1].setValue(m[1],m[5],m[9]); - m_el[2].setValue(m[2],m[6],m[10]); - - } - /** @brief Set the values of the matrix explicitly (row major) - * @param xx Top left - * @param xy Top Middle - * @param xz Top Right - * @param yx Middle Left - * @param yy Middle Middle - * @param yz Middle Right - * @param zx Bottom Left - * @param zy Bottom Middle - * @param zz Bottom Right*/ - void setValue(const tf3Scalar& xx, const tf3Scalar& xy, const tf3Scalar& xz, - const tf3Scalar& yx, const tf3Scalar& yy, const tf3Scalar& yz, - const tf3Scalar& zx, const tf3Scalar& zy, const tf3Scalar& zz) - { - m_el[0].setValue(xx,xy,xz); - m_el[1].setValue(yx,yy,yz); - m_el[2].setValue(zx,zy,zz); - } - - /** @brief Set the matrix from a quaternion - * @param q The Quaternion to match */ - void setRotation(const Quaternion& q) - { - tf3Scalar d = q.length2(); - tf3FullAssert(d != tf3Scalar(0.0)); - tf3Scalar s = tf3Scalar(2.0) / d; - tf3Scalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; - tf3Scalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; - tf3Scalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; - tf3Scalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; - setValue(tf3Scalar(1.0) - (yy + zz), xy - wz, xz + wy, - xy + wz, tf3Scalar(1.0) - (xx + zz), yz - wx, - xz - wy, yz + wx, tf3Scalar(1.0) - (xx + yy)); - } - - - /** @brief Set the matrix from euler angles using YPR around ZYX respectively - * @param yaw Yaw about Z axis - * @param pitch Pitch about Y axis - * @param roll Roll about X axis - */ - ROS_DEPRECATED void setEulerZYX(const tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& roll) - { - setEulerYPR(yaw, pitch, roll); - } - - /** @brief Set the matrix from euler angles YPR around ZYX axes - * @param eulerZ Yaw aboud Z axis - * @param eulerY Pitch around Y axis - * @param eulerX Roll about X axis - * - * These angles are used to produce a rotation matrix. The euler - * angles are applied in ZYX order. I.e a vector is first rotated - * about X then Y and then Z - **/ - void setEulerYPR(tf3Scalar eulerZ, tf3Scalar eulerY,tf3Scalar eulerX) { - tf3Scalar ci ( tf3Cos(eulerX)); - tf3Scalar cj ( tf3Cos(eulerY)); - tf3Scalar ch ( tf3Cos(eulerZ)); - tf3Scalar si ( tf3Sin(eulerX)); - tf3Scalar sj ( tf3Sin(eulerY)); - tf3Scalar sh ( tf3Sin(eulerZ)); - tf3Scalar cc = ci * ch; - tf3Scalar cs = ci * sh; - tf3Scalar sc = si * ch; - tf3Scalar ss = si * sh; - - setValue(cj * ch, sj * sc - cs, sj * cc + ss, - cj * sh, sj * ss + cc, sj * cs - sc, - -sj, cj * si, cj * ci); - } - - /** @brief Set the matrix using RPY about XYZ fixed axes - * @param roll Roll about X axis - * @param pitch Pitch around Y axis - * @param yaw Yaw aboud Z axis - * - **/ - void setRPY(tf3Scalar roll, tf3Scalar pitch,tf3Scalar yaw) { - setEulerYPR(yaw, pitch, roll); - } - - /**@brief Set the matrix to the identity */ - void setIdentity() - { - setValue(tf3Scalar(1.0), tf3Scalar(0.0), tf3Scalar(0.0), - tf3Scalar(0.0), tf3Scalar(1.0), tf3Scalar(0.0), - tf3Scalar(0.0), tf3Scalar(0.0), tf3Scalar(1.0)); - } - - static const Matrix3x3& getIdentity() - { - static const Matrix3x3 identityMatrix(tf3Scalar(1.0), tf3Scalar(0.0), tf3Scalar(0.0), - tf3Scalar(0.0), tf3Scalar(1.0), tf3Scalar(0.0), - tf3Scalar(0.0), tf3Scalar(0.0), tf3Scalar(1.0)); - return identityMatrix; - } - - /**@brief Fill the values of the matrix into a 9 element array - * @param m The array to be filled */ - void getOpenGLSubMatrix(tf3Scalar *m) const - { - m[0] = tf3Scalar(m_el[0].x()); - m[1] = tf3Scalar(m_el[1].x()); - m[2] = tf3Scalar(m_el[2].x()); - m[3] = tf3Scalar(0.0); - m[4] = tf3Scalar(m_el[0].y()); - m[5] = tf3Scalar(m_el[1].y()); - m[6] = tf3Scalar(m_el[2].y()); - m[7] = tf3Scalar(0.0); - m[8] = tf3Scalar(m_el[0].z()); - m[9] = tf3Scalar(m_el[1].z()); - m[10] = tf3Scalar(m_el[2].z()); - m[11] = tf3Scalar(0.0); - } - - /**@brief Get the matrix represented as a quaternion - * @param q The quaternion which will be set */ - void getRotation(Quaternion& q) const - { - tf3Scalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); - tf3Scalar temp[4]; - - if (trace > tf3Scalar(0.0)) - { - tf3Scalar s = tf3Sqrt(trace + tf3Scalar(1.0)); - temp[3]=(s * tf3Scalar(0.5)); - s = tf3Scalar(0.5) / s; - - temp[0]=((m_el[2].y() - m_el[1].z()) * s); - temp[1]=((m_el[0].z() - m_el[2].x()) * s); - temp[2]=((m_el[1].x() - m_el[0].y()) * s); - } - else - { - int i = m_el[0].x() < m_el[1].y() ? - (m_el[1].y() < m_el[2].z() ? 2 : 1) : - (m_el[0].x() < m_el[2].z() ? 2 : 0); - int j = (i + 1) % 3; - int k = (i + 2) % 3; - - tf3Scalar s = tf3Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tf3Scalar(1.0)); - temp[i] = s * tf3Scalar(0.5); - s = tf3Scalar(0.5) / s; - - temp[3] = (m_el[k][j] - m_el[j][k]) * s; - temp[j] = (m_el[j][i] + m_el[i][j]) * s; - temp[k] = (m_el[k][i] + m_el[i][k]) * s; - } - q.setValue(temp[0],temp[1],temp[2],temp[3]); - } - - /**@brief Get the matrix represented as euler angles around ZYX - * @param yaw Yaw around Z axis - * @param pitch Pitch around Y axis - * @param roll around X axis - * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ - ROS_DEPRECATED void getEulerZYX(tf3Scalar& yaw, tf3Scalar& pitch, tf3Scalar& roll, unsigned int solution_number = 1) const - { - getEulerYPR(yaw, pitch, roll, solution_number); - }; - - - /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR - * @param yaw Yaw around Z axis - * @param pitch Pitch around Y axis - * @param roll around X axis */ - void getEulerYPR(tf3Scalar& yaw, tf3Scalar& pitch, tf3Scalar& roll, unsigned int solution_number = 1) const - { - struct Euler - { - tf3Scalar yaw; - tf3Scalar pitch; - tf3Scalar roll; - }; - - Euler euler_out; - Euler euler_out2; //second solution - //get the pointer to the raw data - - // Check that pitch is not at a singularity - // Check that pitch is not at a singularity - if (tf3Fabs(m_el[2].x()) >= 1) - { - euler_out.yaw = 0; - euler_out2.yaw = 0; - - // From difference of angles formula - tf3Scalar delta = tf3Atan2(m_el[2].y(),m_el[2].z()); - if (m_el[2].x() < 0) //gimbal locked down - { - euler_out.pitch = TF3SIMD_PI / tf3Scalar(2.0); - euler_out2.pitch = TF3SIMD_PI / tf3Scalar(2.0); - euler_out.roll = delta; - euler_out2.roll = delta; - } - else // gimbal locked up - { - euler_out.pitch = -TF3SIMD_PI / tf3Scalar(2.0); - euler_out2.pitch = -TF3SIMD_PI / tf3Scalar(2.0); - euler_out.roll = delta; - euler_out2.roll = delta; - } - } - else - { - euler_out.pitch = - tf3Asin(m_el[2].x()); - euler_out2.pitch = TF3SIMD_PI - euler_out.pitch; - - euler_out.roll = tf3Atan2(m_el[2].y()/tf3Cos(euler_out.pitch), - m_el[2].z()/tf3Cos(euler_out.pitch)); - euler_out2.roll = tf3Atan2(m_el[2].y()/tf3Cos(euler_out2.pitch), - m_el[2].z()/tf3Cos(euler_out2.pitch)); - - euler_out.yaw = tf3Atan2(m_el[1].x()/tf3Cos(euler_out.pitch), - m_el[0].x()/tf3Cos(euler_out.pitch)); - euler_out2.yaw = tf3Atan2(m_el[1].x()/tf3Cos(euler_out2.pitch), - m_el[0].x()/tf3Cos(euler_out2.pitch)); - } - - if (solution_number == 1) - { - yaw = euler_out.yaw; - pitch = euler_out.pitch; - roll = euler_out.roll; - } - else - { - yaw = euler_out2.yaw; - pitch = euler_out2.pitch; - roll = euler_out2.roll; - } - } - - /**@brief Get the matrix represented as roll pitch and yaw about fixed axes XYZ - * @param roll around X axis - * @param pitch Pitch around Y axis - * @param yaw Yaw around Z axis - * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ - void getRPY(tf3Scalar& roll, tf3Scalar& pitch, tf3Scalar& yaw, unsigned int solution_number = 1) const - { - getEulerYPR(yaw, pitch, roll, solution_number); - } - - /**@brief Create a scaled copy of the matrix - * @param s Scaling vector The elements of the vector will scale each column */ - - Matrix3x3 scaled(const Vector3& s) const - { - return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), - m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(), - m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); - } - - /**@brief Return the determinant of the matrix */ - tf3Scalar determinant() const; - /**@brief Return the adjoint of the matrix */ - Matrix3x3 adjoint() const; - /**@brief Return the matrix with all values non negative */ - Matrix3x3 absolute() const; - /**@brief Return the transpose of the matrix */ - Matrix3x3 transpose() const; - /**@brief Return the inverse of the matrix */ - Matrix3x3 inverse() const; - - Matrix3x3 transposeTimes(const Matrix3x3& m) const; - Matrix3x3 timesTranspose(const Matrix3x3& m) const; - - TF3SIMD_FORCE_INLINE tf3Scalar tdotx(const Vector3& v) const - { - return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z(); - } - TF3SIMD_FORCE_INLINE tf3Scalar tdoty(const Vector3& v) const - { - return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z(); - } - TF3SIMD_FORCE_INLINE tf3Scalar tdotz(const Vector3& v) const - { - return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); - } - - - /**@brief diagonalizes this matrix by the Jacobi method. - * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original - * coordinate system, i.e., old_this = rot * new_this * rot^T. - * @param threshold See iteration - * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied - * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. - * - * Note that this matrix is assumed to be symmetric. - */ - void diagonalize(Matrix3x3& rot, tf3Scalar threshold, int maxSteps) - { - rot.setIdentity(); - for (int step = maxSteps; step > 0; step--) - { - // find off-diagonal element [p][q] with largest magnitude - int p = 0; - int q = 1; - int r = 2; - tf3Scalar max = tf3Fabs(m_el[0][1]); - tf3Scalar v = tf3Fabs(m_el[0][2]); - if (v > max) - { - q = 2; - r = 1; - max = v; - } - v = tf3Fabs(m_el[1][2]); - if (v > max) - { - p = 1; - q = 2; - r = 0; - max = v; - } - - tf3Scalar t = threshold * (tf3Fabs(m_el[0][0]) + tf3Fabs(m_el[1][1]) + tf3Fabs(m_el[2][2])); - if (max <= t) - { - if (max <= TF3SIMD_EPSILON * t) - { - return; - } - step = 1; - } - - // compute Jacobi rotation J which leads to a zero for element [p][q] - tf3Scalar mpq = m_el[p][q]; - tf3Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq); - tf3Scalar theta2 = theta * theta; - tf3Scalar cos; - tf3Scalar sin; - if (theta2 * theta2 < tf3Scalar(10 / TF3SIMD_EPSILON)) - { - t = (theta >= 0) ? 1 / (theta + tf3Sqrt(1 + theta2)) - : 1 / (theta - tf3Sqrt(1 + theta2)); - cos = 1 / tf3Sqrt(1 + t * t); - sin = cos * t; - } - else - { - // approximation for large theta-value, i.e., a nearly diagonal matrix - t = 1 / (theta * (2 + tf3Scalar(0.5) / theta2)); - cos = 1 - tf3Scalar(0.5) * t * t; - sin = cos * t; - } - - // apply rotation to matrix (this = J^T * this * J) - m_el[p][q] = m_el[q][p] = 0; - m_el[p][p] -= t * mpq; - m_el[q][q] += t * mpq; - tf3Scalar mrp = m_el[r][p]; - tf3Scalar mrq = m_el[r][q]; - m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq; - m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp; - - // apply rotation to rot (rot = rot * J) - for (int i = 0; i < 3; i++) - { - Vector3& row = rot[i]; - mrp = row[p]; - mrq = row[q]; - row[p] = cos * mrp - sin * mrq; - row[q] = cos * mrq + sin * mrp; - } - } - } - - - - - /**@brief Calculate the matrix cofactor - * @param r1 The first row to use for calculating the cofactor - * @param c1 The first column to use for calculating the cofactor - * @param r1 The second row to use for calculating the cofactor - * @param c1 The second column to use for calculating the cofactor - * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details - */ - tf3Scalar cofac(int r1, int c1, int r2, int c2) const - { - return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; - } - - void serialize(struct Matrix3x3Data& dataOut) const; - - void serializeFloat(struct Matrix3x3FloatData& dataOut) const; - - void deSerialize(const struct Matrix3x3Data& dataIn); - - void deSerializeFloat(const struct Matrix3x3FloatData& dataIn); - - void deSerializeDouble(const struct Matrix3x3DoubleData& dataIn); - -}; - - -TF3SIMD_FORCE_INLINE Matrix3x3& -Matrix3x3::operator*=(const Matrix3x3& m) -{ - setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), - m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), - m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); - return *this; -} - -TF3SIMD_FORCE_INLINE tf3Scalar -Matrix3x3::determinant() const -{ - return tf3Triple((*this)[0], (*this)[1], (*this)[2]); -} - - -TF3SIMD_FORCE_INLINE Matrix3x3 -Matrix3x3::absolute() const -{ - return Matrix3x3( - tf3Fabs(m_el[0].x()), tf3Fabs(m_el[0].y()), tf3Fabs(m_el[0].z()), - tf3Fabs(m_el[1].x()), tf3Fabs(m_el[1].y()), tf3Fabs(m_el[1].z()), - tf3Fabs(m_el[2].x()), tf3Fabs(m_el[2].y()), tf3Fabs(m_el[2].z())); -} - -TF3SIMD_FORCE_INLINE Matrix3x3 -Matrix3x3::transpose() const -{ - return Matrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), - m_el[0].y(), m_el[1].y(), m_el[2].y(), - m_el[0].z(), m_el[1].z(), m_el[2].z()); -} - -TF3SIMD_FORCE_INLINE Matrix3x3 -Matrix3x3::adjoint() const -{ - return Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2), - cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0), - cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1)); -} - -TF3SIMD_FORCE_INLINE Matrix3x3 -Matrix3x3::inverse() const -{ - Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); - tf3Scalar det = (*this)[0].dot(co); - tf3FullAssert(det != tf3Scalar(0.0)); - tf3Scalar s = tf3Scalar(1.0) / det; - return Matrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, - co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, - co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); -} - -TF3SIMD_FORCE_INLINE Matrix3x3 -Matrix3x3::transposeTimes(const Matrix3x3& m) const -{ - return Matrix3x3( - m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), - m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), - m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), - m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), - m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), - m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), - m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), - m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), - m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z()); -} - -TF3SIMD_FORCE_INLINE Matrix3x3 -Matrix3x3::timesTranspose(const Matrix3x3& m) const -{ - return Matrix3x3( - m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]), - m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]), - m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2])); - -} - -TF3SIMD_FORCE_INLINE Vector3 -operator*(const Matrix3x3& m, const Vector3& v) -{ - return Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); -} - - -TF3SIMD_FORCE_INLINE Vector3 -operator*(const Vector3& v, const Matrix3x3& m) -{ - return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); -} - -TF3SIMD_FORCE_INLINE Matrix3x3 -operator*(const Matrix3x3& m1, const Matrix3x3& m2) -{ - return Matrix3x3( - m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]), - m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]), - m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2])); -} - -/* -TF3SIMD_FORCE_INLINE Matrix3x3 tf3MultTransposeLeft(const Matrix3x3& m1, const Matrix3x3& m2) { -return Matrix3x3( -m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], -m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1], -m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2], -m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0], -m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1], -m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2], -m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0], -m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], -m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); -} -*/ - -/**@brief Equality operator between two matrices -* It will test all elements are equal. */ -TF3SIMD_FORCE_INLINE bool operator==(const Matrix3x3& m1, const Matrix3x3& m2) -{ - return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && - m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] && - m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] ); -} - -///for serialization -struct Matrix3x3FloatData -{ - Vector3FloatData m_el[3]; -}; - -///for serialization -struct Matrix3x3DoubleData -{ - Vector3DoubleData m_el[3]; -}; - - - - -TF3SIMD_FORCE_INLINE void Matrix3x3::serialize(struct Matrix3x3Data& dataOut) const -{ - for (int i=0;i<3;i++) - m_el[i].serialize(dataOut.m_el[i]); -} - -TF3SIMD_FORCE_INLINE void Matrix3x3::serializeFloat(struct Matrix3x3FloatData& dataOut) const -{ - for (int i=0;i<3;i++) - m_el[i].serializeFloat(dataOut.m_el[i]); -} - - -TF3SIMD_FORCE_INLINE void Matrix3x3::deSerialize(const struct Matrix3x3Data& dataIn) -{ - for (int i=0;i<3;i++) - m_el[i].deSerialize(dataIn.m_el[i]); -} - -TF3SIMD_FORCE_INLINE void Matrix3x3::deSerializeFloat(const struct Matrix3x3FloatData& dataIn) -{ - for (int i=0;i<3;i++) - m_el[i].deSerializeFloat(dataIn.m_el[i]); -} - -TF3SIMD_FORCE_INLINE void Matrix3x3::deSerializeDouble(const struct Matrix3x3DoubleData& dataIn) -{ - for (int i=0;i<3;i++) - m_el[i].deSerializeDouble(dataIn.m_el[i]); -} - -} -#endif //TF3_MATRIX3x3_H - diff --git a/src/Libraries/tf3/include/tf3/LinearMath/MinMax.h b/src/Libraries/tf3/include/tf3/LinearMath/MinMax.h deleted file mode 100644 index 801e054..0000000 --- a/src/Libraries/tf3/include/tf3/LinearMath/MinMax.h +++ /dev/null @@ -1,72 +0,0 @@ -/* -Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef TF3_GEN_MINMAX_H -#define TF3_GEN_MINMAX_H - -#include "Scalar.h" - - -template -TF3SIMD_FORCE_INLINE const T& tf3Min(const T& a, const T& b) -{ - return a < b ? a : b ; -} - -template -TF3SIMD_FORCE_INLINE const T& tf3Max(const T& a, const T& b) -{ - return a > b ? a : b; -} - -template -TF3SIMD_FORCE_INLINE const T& tf3GEN_clamped(const T& a, const T& lb, const T& ub) -{ - return a < lb ? lb : (ub < a ? ub : a); -} - -template -TF3SIMD_FORCE_INLINE void tf3SetMin(T& a, const T& b) -{ - if (b < a) - { - a = b; - } -} - -template -TF3SIMD_FORCE_INLINE void tf3SetMax(T& a, const T& b) -{ - if (a < b) - { - a = b; - } -} - -template -TF3SIMD_FORCE_INLINE void tf3GEN_clamp(T& a, const T& lb, const T& ub) -{ - if (a < lb) - { - a = lb; - } - else if (ub < a) - { - a = ub; - } -} - -#endif diff --git a/src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h b/src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h deleted file mode 100644 index 871696e..0000000 --- a/src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h +++ /dev/null @@ -1,183 +0,0 @@ -/* -Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef TF3SIMD_QUADWORD_H -#define TF3SIMD_QUADWORD_H - -#include "Scalar.h" -#include "MinMax.h" - - -#if defined (__CELLOS_LV2) && defined (__SPU__) -#include -#endif - -namespace tf3 -{ -/**@brief The QuadWord class is base class for Vector3 and Quaternion. - * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. - */ -#ifndef USE_LIBSPE2 -ATTRIBUTE_ALIGNED16(class) QuadWord -#else -class QuadWord -#endif -{ -protected: - -#if defined (__SPU__) && defined (__CELLOS_LV2__) - union { - vec_float4 mVec128; - tf3Scalar m_floats[4]; - }; -public: - vec_float4 get128() const - { - return mVec128; - } -protected: -#else //__CELLOS_LV2__ __SPU__ - tf3Scalar m_floats[4]; -#endif //__CELLOS_LV2__ __SPU__ - - public: - - - /**@brief Return the x value */ - TF3SIMD_FORCE_INLINE const tf3Scalar& getX() const { return m_floats[0]; } - /**@brief Return the y value */ - TF3SIMD_FORCE_INLINE const tf3Scalar& getY() const { return m_floats[1]; } - /**@brief Return the z value */ - TF3SIMD_FORCE_INLINE const tf3Scalar& getZ() const { return m_floats[2]; } - /**@brief Set the x value */ - TF3SIMD_FORCE_INLINE void setX(tf3Scalar x) { m_floats[0] = x;}; - /**@brief Set the y value */ - TF3SIMD_FORCE_INLINE void setY(tf3Scalar y) { m_floats[1] = y;}; - /**@brief Set the z value */ - TF3SIMD_FORCE_INLINE void setZ(tf3Scalar z) { m_floats[2] = z;}; - /**@brief Set the w value */ - TF3SIMD_FORCE_INLINE void setW(tf3Scalar w) { m_floats[3] = w;}; - /**@brief Return the x value */ - TF3SIMD_FORCE_INLINE const tf3Scalar& x() const { return m_floats[0]; } - /**@brief Return the y value */ - TF3SIMD_FORCE_INLINE const tf3Scalar& y() const { return m_floats[1]; } - /**@brief Return the z value */ - TF3SIMD_FORCE_INLINE const tf3Scalar& z() const { return m_floats[2]; } - /**@brief Return the w value */ - TF3SIMD_FORCE_INLINE const tf3Scalar& w() const { return m_floats[3]; } - - //TF3SIMD_FORCE_INLINE tf3Scalar& operator[](int i) { return (&m_floats[0])[i]; } - //TF3SIMD_FORCE_INLINE const tf3Scalar& operator[](int i) const { return (&m_floats[0])[i]; } - ///operator tf3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. - TF3SIMD_FORCE_INLINE operator tf3Scalar *() { return &m_floats[0]; } - TF3SIMD_FORCE_INLINE operator const tf3Scalar *() const { return &m_floats[0]; } - - TF3SIMD_FORCE_INLINE bool operator==(const QuadWord& other) const - { - return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); - } - - TF3SIMD_FORCE_INLINE bool operator!=(const QuadWord& other) const - { - return !(*this == other); - } - - /**@brief Set x,y,z and zero w - * @param x Value of x - * @param y Value of y - * @param z Value of z - */ - TF3SIMD_FORCE_INLINE void setValue(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z) - { - m_floats[0]=x; - m_floats[1]=y; - m_floats[2]=z; - m_floats[3] = 0.f; - } - -/* void getValue(tf3Scalar *m) const - { - m[0] = m_floats[0]; - m[1] = m_floats[1]; - m[2] = m_floats[2]; - } -*/ -/**@brief Set the values - * @param x Value of x - * @param y Value of y - * @param z Value of z - * @param w Value of w - */ - TF3SIMD_FORCE_INLINE void setValue(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z,const tf3Scalar& w) - { - m_floats[0]=x; - m_floats[1]=y; - m_floats[2]=z; - m_floats[3]=w; - } - /**@brief No initialization constructor */ - TF3SIMD_FORCE_INLINE QuadWord() - // :m_floats[0](tf3Scalar(0.)),m_floats[1](tf3Scalar(0.)),m_floats[2](tf3Scalar(0.)),m_floats[3](tf3Scalar(0.)) - { - } - - /**@brief Three argument constructor (zeros w) - * @param x Value of x - * @param y Value of y - * @param z Value of z - */ - TF3SIMD_FORCE_INLINE QuadWord(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z) - { - m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f; - } - -/**@brief Initializing constructor - * @param x Value of x - * @param y Value of y - * @param z Value of z - * @param w Value of w - */ - TF3SIMD_FORCE_INLINE QuadWord(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z,const tf3Scalar& w) - { - m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w; - } - - /**@brief Set each element to the max of the current values and the values of another QuadWord - * @param other The other QuadWord to compare with - */ - TF3SIMD_FORCE_INLINE void setMax(const QuadWord& other) - { - tf3SetMax(m_floats[0], other.m_floats[0]); - tf3SetMax(m_floats[1], other.m_floats[1]); - tf3SetMax(m_floats[2], other.m_floats[2]); - tf3SetMax(m_floats[3], other.m_floats[3]); - } - /**@brief Set each element to the min of the current values and the values of another QuadWord - * @param other The other QuadWord to compare with - */ - TF3SIMD_FORCE_INLINE void setMin(const QuadWord& other) - { - tf3SetMin(m_floats[0], other.m_floats[0]); - tf3SetMin(m_floats[1], other.m_floats[1]); - tf3SetMin(m_floats[2], other.m_floats[2]); - tf3SetMin(m_floats[3], other.m_floats[3]); - } - - - -}; - -} -#endif //TF3SIMD_QUADWORD_H diff --git a/src/Libraries/tf3/include/tf3/LinearMath/Quaternion.h b/src/Libraries/tf3/include/tf3/LinearMath/Quaternion.h deleted file mode 100644 index 44ec992..0000000 --- a/src/Libraries/tf3/include/tf3/LinearMath/Quaternion.h +++ /dev/null @@ -1,477 +0,0 @@ -/* -Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef TF3_QUATERNION_H_ -#define TF3_QUATERNION_H_ - - -#include "Vector3.h" -#include "QuadWord.h" - -#include "../macros.h" - -namespace tf3 -{ - -/**@brief The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x3, Vector3 and Transform. */ -class Quaternion : public QuadWord { -public: - /**@brief No initialization constructor */ - Quaternion() {} - - // template - // explicit Quaternion(const tf3Scalar *v) : Tuple4(v) {} - /**@brief Constructor from scalars */ - Quaternion(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z, const tf3Scalar& w) - : QuadWord(x, y, z, w) - {} - /**@brief Axis angle Constructor - * @param axis The axis which the rotation is around - * @param angle The magnitude of the rotation around the angle (Radians) */ - Quaternion(const Vector3& axis, const tf3Scalar& angle) - { - setRotation(axis, angle); - } - /**@brief Constructor from Euler angles - * @param yaw Angle around Y unless TF3_EULER_DEFAULT_ZYX defined then Z - * @param pitch Angle around X unless TF3_EULER_DEFAULT_ZYX defined then Y - * @param roll Angle around Z unless TF3_EULER_DEFAULT_ZYX defined then X */ - ROS_DEPRECATED Quaternion(const tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& roll) - { -#ifndef TF3_EULER_DEFAULT_ZYX - setEuler(yaw, pitch, roll); -#else - setRPY(roll, pitch, yaw); -#endif - } - /**@brief Set the rotation using axis angle notation - * @param axis The axis around which to rotate - * @param angle The magnitude of the rotation in Radians */ - void setRotation(const Vector3& axis, const tf3Scalar& angle) - { - tf3Scalar d = axis.length(); - tf3Assert(d != tf3Scalar(0.0)); - tf3Scalar s = tf3Sin(angle * tf3Scalar(0.5)) / d; - setValue(axis.x() * s, axis.y() * s, axis.z() * s, - tf3Cos(angle * tf3Scalar(0.5))); - } - /**@brief Set the quaternion using Euler angles - * @param yaw Angle around Y - * @param pitch Angle around X - * @param roll Angle around Z */ - void setEuler(const tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& roll) - { - tf3Scalar halfYaw = tf3Scalar(yaw) * tf3Scalar(0.5); - tf3Scalar halfPitch = tf3Scalar(pitch) * tf3Scalar(0.5); - tf3Scalar halfRoll = tf3Scalar(roll) * tf3Scalar(0.5); - tf3Scalar cosYaw = tf3Cos(halfYaw); - tf3Scalar sinYaw = tf3Sin(halfYaw); - tf3Scalar cosPitch = tf3Cos(halfPitch); - tf3Scalar sinPitch = tf3Sin(halfPitch); - tf3Scalar cosRoll = tf3Cos(halfRoll); - tf3Scalar sinRoll = tf3Sin(halfRoll); - setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, - cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, - sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, - cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); - } - /**@brief Set the quaternion using fixed axis RPY - * @param roll Angle around X - * @param pitch Angle around Y - * @param yaw Angle around Z*/ - void setRPY(const tf3Scalar& roll, const tf3Scalar& pitch, const tf3Scalar& yaw) - { - tf3Scalar halfYaw = tf3Scalar(yaw) * tf3Scalar(0.5); - tf3Scalar halfPitch = tf3Scalar(pitch) * tf3Scalar(0.5); - tf3Scalar halfRoll = tf3Scalar(roll) * tf3Scalar(0.5); - tf3Scalar cosYaw = tf3Cos(halfYaw); - tf3Scalar sinYaw = tf3Sin(halfYaw); - tf3Scalar cosPitch = tf3Cos(halfPitch); - tf3Scalar sinPitch = tf3Sin(halfPitch); - tf3Scalar cosRoll = tf3Cos(halfRoll); - tf3Scalar sinRoll = tf3Sin(halfRoll); - setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x - cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y - cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z - cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx - } - /**@brief Set the quaternion using euler angles - * @param yaw Angle around Z - * @param pitch Angle around Y - * @param roll Angle around X */ - ROS_DEPRECATED void setEulerZYX(const tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& roll) - { - setRPY(roll, pitch, yaw); - } - /**@brief Add two quaternions - * @param q The quaternion to add to this one */ - TF3SIMD_FORCE_INLINE Quaternion& operator+=(const Quaternion& q) - { - m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3]; - return *this; - } - - /**@brief Sutf3ract out a quaternion - * @param q The quaternion to sutf3ract from this one */ - Quaternion& operator-=(const Quaternion& q) - { - m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3]; - return *this; - } - - /**@brief Scale this quaternion - * @param s The scalar to scale by */ - Quaternion& operator*=(const tf3Scalar& s) - { - m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s; - return *this; - } - - /**@brief Multiply this quaternion by q on the right - * @param q The other quaternion - * Equivilant to this = this * q */ - Quaternion& operator*=(const Quaternion& q) - { - setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(), - m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(), - m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(), - m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z()); - return *this; - } - /**@brief Return the dot product between this quaternion and another - * @param q The other quaternion */ - tf3Scalar dot(const Quaternion& q) const - { - return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3]; - } - - /**@brief Return the length squared of the quaternion */ - tf3Scalar length2() const - { - return dot(*this); - } - - /**@brief Return the length of the quaternion */ - tf3Scalar length() const - { - return tf3Sqrt(length2()); - } - - /**@brief Normalize the quaternion - * Such that x^2 + y^2 + z^2 +w^2 = 1 */ - Quaternion& normalize() - { - return *this /= length(); - } - - /**@brief Return a scaled version of this quaternion - * @param s The scale factor */ - TF3SIMD_FORCE_INLINE Quaternion - operator*(const tf3Scalar& s) const - { - return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s); - } - - - /**@brief Return an inversely scaled versionof this quaternion - * @param s The inverse scale factor */ - Quaternion operator/(const tf3Scalar& s) const - { - tf3Assert(s != tf3Scalar(0.0)); - return *this * (tf3Scalar(1.0) / s); - } - - /**@brief Inversely scale this quaternion - * @param s The scale factor */ - Quaternion& operator/=(const tf3Scalar& s) - { - tf3Assert(s != tf3Scalar(0.0)); - return *this *= tf3Scalar(1.0) / s; - } - - /**@brief Return a normalized version of this quaternion */ - Quaternion normalized() const - { - return *this / length(); - } - /**@brief Return the ***half*** angle between this quaternion and the other - * @param q The other quaternion */ - tf3Scalar angle(const Quaternion& q) const - { - tf3Scalar s = tf3Sqrt(length2() * q.length2()); - tf3Assert(s != tf3Scalar(0.0)); - return tf3Acos(dot(q) / s); - } - /**@brief Return the angle between this quaternion and the other along the shortest path - * @param q The other quaternion */ - tf3Scalar angleShortestPath(const Quaternion& q) const - { - tf3Scalar s = tf3Sqrt(length2() * q.length2()); - tf3Assert(s != tf3Scalar(0.0)); - if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp - return tf3Acos(dot(-q) / s) * tf3Scalar(2.0); - else - return tf3Acos(dot(q) / s) * tf3Scalar(2.0); - } - /**@brief Return the angle [0, 2Pi] of rotation represented by this quaternion */ - tf3Scalar getAngle() const - { - tf3Scalar s = tf3Scalar(2.) * tf3Acos(m_floats[3]); - return s; - } - - /**@brief Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path */ - tf3Scalar getAngleShortestPath() const - { - tf3Scalar s; - if (m_floats[3] >= 0) - s = tf3Scalar(2.) * tf3Acos(m_floats[3]); - else - s = tf3Scalar(2.) * tf3Acos(-m_floats[3]); - - return s; - } - - /**@brief Return the axis of the rotation represented by this quaternion */ - Vector3 getAxis() const - { - tf3Scalar s_squared = tf3Scalar(1.) - tf3Pow(m_floats[3], tf3Scalar(2.)); - if (s_squared < tf3Scalar(10.) * TF3SIMD_EPSILON) //Check for divide by zero - return Vector3(1.0, 0.0, 0.0); // Arbitrary - tf3Scalar s = tf3Sqrt(s_squared); - return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s); - } - - /**@brief Return the inverse of this quaternion */ - Quaternion inverse() const - { - return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]); - } - - /**@brief Return the sum of this quaternion and the other - * @param q2 The other quaternion */ - TF3SIMD_FORCE_INLINE Quaternion - operator+(const Quaternion& q2) const - { - const Quaternion& q1 = *this; - return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]); - } - - /**@brief Return the difference between this quaternion and the other - * @param q2 The other quaternion */ - TF3SIMD_FORCE_INLINE Quaternion - operator-(const Quaternion& q2) const - { - const Quaternion& q1 = *this; - return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]); - } - - /**@brief Return the negative of this quaternion - * This simply negates each element */ - TF3SIMD_FORCE_INLINE Quaternion operator-() const - { - const Quaternion& q2 = *this; - return Quaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]); - } - /**@todo document this and it's use */ - TF3SIMD_FORCE_INLINE Quaternion farthest( const Quaternion& qd) const - { - Quaternion diff,sum; - diff = *this - qd; - sum = *this + qd; - if( diff.dot(diff) > sum.dot(sum) ) - return qd; - return (-qd); - } - - /**@todo document this and it's use */ - TF3SIMD_FORCE_INLINE Quaternion nearest( const Quaternion& qd) const - { - Quaternion diff,sum; - diff = *this - qd; - sum = *this + qd; - if( diff.dot(diff) < sum.dot(sum) ) - return qd; - return (-qd); - } - - - /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion - * @param q The other quaternion to interpolate with - * @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. - * Slerp interpolates assuming constant velocity. */ - Quaternion slerp(const Quaternion& q, const tf3Scalar& t) const - { - tf3Scalar theta = angleShortestPath(q) / tf3Scalar(2.0); - if (theta != tf3Scalar(0.0)) - { - tf3Scalar d = tf3Scalar(1.0) / tf3Sin(theta); - tf3Scalar s0 = tf3Sin((tf3Scalar(1.0) - t) * theta); - tf3Scalar s1 = tf3Sin(t * theta); - if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp - return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d, - (m_floats[1] * s0 + -q.y() * s1) * d, - (m_floats[2] * s0 + -q.z() * s1) * d, - (m_floats[3] * s0 + -q.m_floats[3] * s1) * d); - else - return Quaternion((m_floats[0] * s0 + q.x() * s1) * d, - (m_floats[1] * s0 + q.y() * s1) * d, - (m_floats[2] * s0 + q.z() * s1) * d, - (m_floats[3] * s0 + q.m_floats[3] * s1) * d); - - } - else - { - return *this; - } - } - - static const Quaternion& getIdentity() - { - static const Quaternion identityQuat(tf3Scalar(0.),tf3Scalar(0.),tf3Scalar(0.),tf3Scalar(1.)); - return identityQuat; - } - - TF3SIMD_FORCE_INLINE const tf3Scalar& getW() const { return m_floats[3]; } - - -}; - - -/**@brief Return the negative of a quaternion */ -TF3SIMD_FORCE_INLINE Quaternion -operator-(const Quaternion& q) -{ - return Quaternion(-q.x(), -q.y(), -q.z(), -q.w()); -} - - - -/**@brief Return the product of two quaternions */ -TF3SIMD_FORCE_INLINE Quaternion -operator*(const Quaternion& q1, const Quaternion& q2) { - return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(), - q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(), - q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(), - q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); -} - -TF3SIMD_FORCE_INLINE Quaternion -operator*(const Quaternion& q, const Vector3& w) -{ - return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(), - q.w() * w.y() + q.z() * w.x() - q.x() * w.z(), - q.w() * w.z() + q.x() * w.y() - q.y() * w.x(), - -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); -} - -TF3SIMD_FORCE_INLINE Quaternion -operator*(const Vector3& w, const Quaternion& q) -{ - return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(), - w.y() * q.w() + w.z() * q.x() - w.x() * q.z(), - w.z() * q.w() + w.x() * q.y() - w.y() * q.x(), - -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); -} - -/**@brief Calculate the dot product between two quaternions */ -TF3SIMD_FORCE_INLINE tf3Scalar -dot(const Quaternion& q1, const Quaternion& q2) -{ - return q1.dot(q2); -} - - -/**@brief Return the length of a quaternion */ -TF3SIMD_FORCE_INLINE tf3Scalar -length(const Quaternion& q) -{ - return q.length(); -} - -/**@brief Return the ***half*** angle between two quaternions*/ -TF3SIMD_FORCE_INLINE tf3Scalar -angle(const Quaternion& q1, const Quaternion& q2) -{ - return q1.angle(q2); -} - -/**@brief Return the shortest angle between two quaternions*/ -TF3SIMD_FORCE_INLINE tf3Scalar -angleShortestPath(const Quaternion& q1, const Quaternion& q2) -{ - return q1.angleShortestPath(q2); -} - -/**@brief Return the inverse of a quaternion*/ -TF3SIMD_FORCE_INLINE Quaternion -inverse(const Quaternion& q) -{ - return q.inverse(); -} - -/**@brief Return the result of spherical linear interpolation betwen two quaternions - * @param q1 The first quaternion - * @param q2 The second quaternion - * @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2 - * Slerp assumes constant velocity between positions. */ -TF3SIMD_FORCE_INLINE Quaternion -slerp(const Quaternion& q1, const Quaternion& q2, const tf3Scalar& t) -{ - return q1.slerp(q2, t); -} - -TF3SIMD_FORCE_INLINE Vector3 -quatRotate(const Quaternion& rotation, const Vector3& v) -{ - Quaternion q = rotation * v; - q *= rotation.inverse(); - return Vector3(q.getX(),q.getY(),q.getZ()); -} - -TF3SIMD_FORCE_INLINE Quaternion -shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized -{ - Vector3 c = v0.cross(v1); - tf3Scalar d = v0.dot(v1); - - if (d < -1.0 + TF3SIMD_EPSILON) - { - Vector3 n,unused; - tf3PlaneSpace1(v0,n,unused); - return Quaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0 - } - - tf3Scalar s = tf3Sqrt((1.0f + d) * 2.0f); - tf3Scalar rs = 1.0f / s; - - return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f); -} - -TF3SIMD_FORCE_INLINE Quaternion -shortestArcQuatNormalize2(Vector3& v0,Vector3& v1) -{ - v0.normalize(); - v1.normalize(); - return shortestArcQuat(v0,v1); -} - -} -#endif - - - - diff --git a/src/Libraries/tf3/include/tf3/LinearMath/Scalar.h b/src/Libraries/tf3/include/tf3/LinearMath/Scalar.h deleted file mode 100644 index c7a0f2b..0000000 --- a/src/Libraries/tf3/include/tf3/LinearMath/Scalar.h +++ /dev/null @@ -1,417 +0,0 @@ -/* -Copyright (c) 2003-2009 Erwin Coumans http://bullet.googlecode.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef TF3_SCALAR_H -#define TF3_SCALAR_H - -#ifdef TF3_MANAGED_CODE -//Aligned data types not supported in managed code -#pragma unmanaged -#endif - - -#include -#include //size_t for MSVC 6.0 -#include -#include -#include - -#if defined(DEBUG) || defined (_DEBUG) -#define TF3_DEBUG -#endif - - -#ifdef _WIN32 - - #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) - - #define TF3SIMD_FORCE_INLINE inline - #define ATTRIBUTE_ALIGNED16(a) a - #define ATTRIBUTE_ALIGNED64(a) a - #define ATTRIBUTE_ALIGNED128(a) a - #else - //#define TF3_HAS_ALIGNED_ALLOCATOR - #pragma warning(disable : 4324) // disable padding warning -// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. -// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines -// #pragma warning(disable:4786) // Disable the "debug name too long" warning - - #define TF3SIMD_FORCE_INLINE __forceinline - #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a - #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a - #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a - #ifdef _XBOX - #define TF3_USE_VMX128 - - #include - #define TF3_HAVE_NATIVE_FSEL - #define tf3Fsel(a,b,c) __fsel((a),(b),(c)) - #else - - - #endif//_XBOX - - #endif //__MINGW32__ - - #include -#ifdef TF3_DEBUG - #define tf3Assert assert -#else - #define tf3Assert(x) -#endif - //tf3FullAssert is optional, slows down a lot - #define tf3FullAssert(x) - - #define tf3Likely(_c) _c - #define tf3Unlikely(_c) _c - -#else - -#if defined (__CELLOS_LV2__) - #define TF3SIMD_FORCE_INLINE inline - #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #ifndef assert - #include - #endif -#ifdef TF3_DEBUG - #define tf3Assert assert -#else - #define tf3Assert(x) -#endif - //tf3FullAssert is optional, slows down a lot - #define tf3FullAssert(x) - - #define tf3Likely(_c) _c - #define tf3Unlikely(_c) _c - -#else - -#ifdef USE_LIBSPE2 - - #define TF3SIMD_FORCE_INLINE __inline - #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #ifndef assert - #include - #endif -#ifdef TF3_DEBUG - #define tf3Assert assert -#else - #define tf3Assert(x) -#endif - //tf3FullAssert is optional, slows down a lot - #define tf3FullAssert(x) - - - #define tf3Likely(_c) __builtin_expect((_c), 1) - #define tf3Unlikely(_c) __builtin_expect((_c), 0) - - -#else - //non-windows systems - - #define TF3SIMD_FORCE_INLINE inline - ///@todo: check out alignment methods for other platforms/compilers - ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #define ATTRIBUTE_ALIGNED16(a) a - #define ATTRIBUTE_ALIGNED64(a) a - #define ATTRIBUTE_ALIGNED128(a) a - #ifndef assert - #include - #endif - -#if defined(DEBUG) || defined (_DEBUG) - #define tf3Assert assert -#else - #define tf3Assert(x) -#endif - - //tf3FullAssert is optional, slows down a lot - #define tf3FullAssert(x) - #define tf3Likely(_c) _c - #define tf3Unlikely(_c) _c - -#endif // LIBSPE2 - -#endif //__CELLOS_LV2__ -#endif - - -///The tf3Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision. -typedef double tf3Scalar; -//this number could be bigger in double precision -#define TF3_LARGE_FLOAT 1e30 - - -#define TF3_DECLARE_ALIGNED_ALLOCATOR() \ - TF3SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return tf3AlignedAlloc(sizeInBytes,16); } \ - TF3SIMD_FORCE_INLINE void operator delete(void* ptr) { tf3AlignedFree(ptr); } \ - TF3SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \ - TF3SIMD_FORCE_INLINE void operator delete(void*, void*) { } \ - TF3SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return tf3AlignedAlloc(sizeInBytes,16); } \ - TF3SIMD_FORCE_INLINE void operator delete[](void* ptr) { tf3AlignedFree(ptr); } \ - TF3SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \ - TF3SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \ - - - - -TF3SIMD_FORCE_INLINE tf3Scalar tf3Sqrt(tf3Scalar x) { return sqrt(x); } -TF3SIMD_FORCE_INLINE tf3Scalar tf3Fabs(tf3Scalar x) { return fabs(x); } -TF3SIMD_FORCE_INLINE tf3Scalar tf3Cos(tf3Scalar x) { return cos(x); } -TF3SIMD_FORCE_INLINE tf3Scalar tf3Sin(tf3Scalar x) { return sin(x); } -TF3SIMD_FORCE_INLINE tf3Scalar tf3Tan(tf3Scalar x) { return tan(x); } -TF3SIMD_FORCE_INLINE tf3Scalar tf3Acos(tf3Scalar x) { if (xtf3Scalar(1)) x=tf3Scalar(1); return acos(x); } -TF3SIMD_FORCE_INLINE tf3Scalar tf3Asin(tf3Scalar x) { if (xtf3Scalar(1)) x=tf3Scalar(1); return asin(x); } -TF3SIMD_FORCE_INLINE tf3Scalar tf3Atan(tf3Scalar x) { return atan(x); } -TF3SIMD_FORCE_INLINE tf3Scalar tf3Atan2(tf3Scalar x, tf3Scalar y) { return atan2(x, y); } -TF3SIMD_FORCE_INLINE tf3Scalar tf3Exp(tf3Scalar x) { return exp(x); } -TF3SIMD_FORCE_INLINE tf3Scalar tf3Log(tf3Scalar x) { return log(x); } -TF3SIMD_FORCE_INLINE tf3Scalar tf3Pow(tf3Scalar x,tf3Scalar y) { return pow(x,y); } -TF3SIMD_FORCE_INLINE tf3Scalar tf3Fmod(tf3Scalar x,tf3Scalar y) { return fmod(x,y); } - - -#define TF3SIMD_2_PI tf3Scalar(6.283185307179586232) -#define TF3SIMD_PI (TF3SIMD_2_PI * tf3Scalar(0.5)) -#define TF3SIMD_HALF_PI (TF3SIMD_2_PI * tf3Scalar(0.25)) -#define TF3SIMD_RADS_PER_DEG (TF3SIMD_2_PI / tf3Scalar(360.0)) -#define TF3SIMD_DEGS_PER_RAD (tf3Scalar(360.0) / TF3SIMD_2_PI) -#define TF3SIMDSQRT12 tf3Scalar(0.7071067811865475244008443621048490) - -#define tf3RecipSqrt(x) ((tf3Scalar)(tf3Scalar(1.0)/tf3Sqrt(tf3Scalar(x)))) /* reciprocal square root */ - - -#define TF3SIMD_EPSILON DBL_EPSILON -#define TF3SIMD_INFINITY DBL_MAX - -TF3SIMD_FORCE_INLINE tf3Scalar tf3Atan2Fast(tf3Scalar y, tf3Scalar x) -{ - tf3Scalar coeff_1 = TF3SIMD_PI / 4.0f; - tf3Scalar coeff_2 = 3.0f * coeff_1; - tf3Scalar abs_y = tf3Fabs(y); - tf3Scalar angle; - if (x >= 0.0f) { - tf3Scalar r = (x - abs_y) / (x + abs_y); - angle = coeff_1 - coeff_1 * r; - } else { - tf3Scalar r = (x + abs_y) / (abs_y - x); - angle = coeff_2 - coeff_1 * r; - } - return (y < 0.0f) ? -angle : angle; -} - -TF3SIMD_FORCE_INLINE bool tf3FuzzyZero(tf3Scalar x) { return tf3Fabs(x) < TF3SIMD_EPSILON; } - -TF3SIMD_FORCE_INLINE bool tf3Equal(tf3Scalar a, tf3Scalar eps) { - return (((a) <= eps) && !((a) < -eps)); -} -TF3SIMD_FORCE_INLINE bool tf3GreaterEqual (tf3Scalar a, tf3Scalar eps) { - return (!((a) <= eps)); -} - - -TF3SIMD_FORCE_INLINE int tf3IsNegative(tf3Scalar x) { - return x < tf3Scalar(0.0) ? 1 : 0; -} - -TF3SIMD_FORCE_INLINE tf3Scalar tf3Radians(tf3Scalar x) { return x * TF3SIMD_RADS_PER_DEG; } -TF3SIMD_FORCE_INLINE tf3Scalar tf3Degrees(tf3Scalar x) { return x * TF3SIMD_DEGS_PER_RAD; } - -#define TF3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name - -#ifndef tf3Fsel -TF3SIMD_FORCE_INLINE tf3Scalar tf3Fsel(tf3Scalar a, tf3Scalar b, tf3Scalar c) -{ - return a >= 0 ? b : c; -} -#endif -#define tf3Fsels(a,b,c) (tf3Scalar)tf3Fsel(a,b,c) - - -TF3SIMD_FORCE_INLINE bool tf3MachineIsLittleEndian() -{ - long int i = 1; - const char *p = (const char *) &i; - if (p[0] == 1) // Lowest address contains the least significant byte - return true; - else - return false; -} - - - -///tf3Select avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360 -///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html -TF3SIMD_FORCE_INLINE unsigned tf3Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) -{ - // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero - // Rely on positive value or'ed with its negative having sign bit on - // and zero value or'ed with its negative (which is still zero) having sign bit off - // Use arithmetic shift right, shifting the sign bit through all 32 bits - unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); - unsigned testEqz = ~testNz; - return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); -} -TF3SIMD_FORCE_INLINE int tf3Select(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero) -{ - unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); - unsigned testEqz = ~testNz; - return static_cast((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); -} -TF3SIMD_FORCE_INLINE float tf3Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) -{ -#ifdef TF3_HAVE_NATIVE_FSEL - return (float)tf3Fsel((tf3Scalar)condition - tf3Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); -#else - return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; -#endif -} - -template TF3SIMD_FORCE_INLINE void tf3Swap(T& a, T& b) -{ - T tmp = a; - a = b; - b = tmp; -} - - -//PCK: endian swapping functions -TF3SIMD_FORCE_INLINE unsigned tf3SwapEndian(unsigned val) -{ - return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24)); -} - -TF3SIMD_FORCE_INLINE unsigned short tf3SwapEndian(unsigned short val) -{ - return static_cast(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8)); -} - -TF3SIMD_FORCE_INLINE unsigned tf3SwapEndian(int val) -{ - return tf3SwapEndian((unsigned)val); -} - -TF3SIMD_FORCE_INLINE unsigned short tf3SwapEndian(short val) -{ - return tf3SwapEndian((unsigned short) val); -} - -///tf3SwapFloat uses using char pointers to swap the endianness -////tf3SwapFloat/tf3SwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values -///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. -///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. -///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. -///so instead of returning a float/double, we return integer/long long integer -TF3SIMD_FORCE_INLINE unsigned int tf3SwapEndianFloat(float d) -{ - unsigned int a = 0; - unsigned char *dst = (unsigned char *)&a; - unsigned char *src = (unsigned char *)&d; - - dst[0] = src[3]; - dst[1] = src[2]; - dst[2] = src[1]; - dst[3] = src[0]; - return a; -} - -// unswap using char pointers -TF3SIMD_FORCE_INLINE float tf3UnswapEndianFloat(unsigned int a) -{ - float d = 0.0f; - unsigned char *src = (unsigned char *)&a; - unsigned char *dst = (unsigned char *)&d; - - dst[0] = src[3]; - dst[1] = src[2]; - dst[2] = src[1]; - dst[3] = src[0]; - - return d; -} - - -// swap using char pointers -TF3SIMD_FORCE_INLINE void tf3SwapEndianDouble(double d, unsigned char* dst) -{ - unsigned char *src = (unsigned char *)&d; - - dst[0] = src[7]; - dst[1] = src[6]; - dst[2] = src[5]; - dst[3] = src[4]; - dst[4] = src[3]; - dst[5] = src[2]; - dst[6] = src[1]; - dst[7] = src[0]; - -} - -// unswap using char pointers -TF3SIMD_FORCE_INLINE double tf3UnswapEndianDouble(const unsigned char *src) -{ - double d = 0.0; - unsigned char *dst = (unsigned char *)&d; - - dst[0] = src[7]; - dst[1] = src[6]; - dst[2] = src[5]; - dst[3] = src[4]; - dst[4] = src[3]; - dst[5] = src[2]; - dst[6] = src[1]; - dst[7] = src[0]; - - return d; -} - -// returns normalized value in range [-TF3SIMD_PI, TF3SIMD_PI] -TF3SIMD_FORCE_INLINE tf3Scalar tf3NormalizeAngle(tf3Scalar angleInRadians) -{ - angleInRadians = tf3Fmod(angleInRadians, TF3SIMD_2_PI); - if(angleInRadians < -TF3SIMD_PI) - { - return angleInRadians + TF3SIMD_2_PI; - } - else if(angleInRadians > TF3SIMD_PI) - { - return angleInRadians - TF3SIMD_2_PI; - } - else - { - return angleInRadians; - } -} - -///rudimentary class to provide type info -struct tf3TypedObject -{ - tf3TypedObject(int objectType) - :m_objectType(objectType) - { - } - int m_objectType; - inline int getObjectType() const - { - return m_objectType; - } -}; -#endif //TF3SIMD___SCALAR_H diff --git a/src/Libraries/tf3/include/tf3/LinearMath/Transform.h b/src/Libraries/tf3/include/tf3/LinearMath/Transform.h deleted file mode 100644 index ef676f3..0000000 --- a/src/Libraries/tf3/include/tf3/LinearMath/Transform.h +++ /dev/null @@ -1,305 +0,0 @@ -/* -Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef tf3_Transform_H -#define tf3_Transform_H - - -#include "Matrix3x3.h" - - -namespace tf3 -{ - -#define TransformData TransformDoubleData - - -/**@brief The Transform class supports rigid transforms with only translation and rotation and no scaling/shear. - *It can be used in combination with Vector3, Quaternion and Matrix3x3 linear algebra classes. */ -class Transform { - - ///Storage for the rotation - Matrix3x3 m_basis; - ///Storage for the translation - Vector3 m_origin; - -public: - - /**@brief No initialization constructor */ - Transform() {} - /**@brief Constructor from Quaternion (optional Vector3 ) - * @param q Rotation from quaternion - * @param c Translation from Vector (default 0,0,0) */ - explicit TF3SIMD_FORCE_INLINE Transform(const Quaternion& q, - const Vector3& c = Vector3(tf3Scalar(0), tf3Scalar(0), tf3Scalar(0))) - : m_basis(q), - m_origin(c) - {} - - /**@brief Constructor from Matrix3x3 (optional Vector3) - * @param b Rotation from Matrix - * @param c Translation from Vector default (0,0,0)*/ - explicit TF3SIMD_FORCE_INLINE Transform(const Matrix3x3& b, - const Vector3& c = Vector3(tf3Scalar(0), tf3Scalar(0), tf3Scalar(0))) - : m_basis(b), - m_origin(c) - {} - /**@brief Copy constructor */ - TF3SIMD_FORCE_INLINE Transform (const Transform& other) - : m_basis(other.m_basis), - m_origin(other.m_origin) - { - } - /**@brief Assignment Operator */ - TF3SIMD_FORCE_INLINE Transform& operator=(const Transform& other) - { - m_basis = other.m_basis; - m_origin = other.m_origin; - return *this; - } - - /**@brief Set the current transform as the value of the product of two transforms - * @param t1 Transform 1 - * @param t2 Transform 2 - * This = Transform1 * Transform2 */ - TF3SIMD_FORCE_INLINE void mult(const Transform& t1, const Transform& t2) { - m_basis = t1.m_basis * t2.m_basis; - m_origin = t1(t2.m_origin); - } - -/* void multInverseLeft(const Transform& t1, const Transform& t2) { - Vector3 v = t2.m_origin - t1.m_origin; - m_basis = tf3MultTransposeLeft(t1.m_basis, t2.m_basis); - m_origin = v * t1.m_basis; - } - */ - -/**@brief Return the transform of the vector */ - TF3SIMD_FORCE_INLINE Vector3 operator()(const Vector3& x) const - { - return Vector3(m_basis[0].dot(x) + m_origin.x(), - m_basis[1].dot(x) + m_origin.y(), - m_basis[2].dot(x) + m_origin.z()); - } - - /**@brief Return the transform of the vector */ - TF3SIMD_FORCE_INLINE Vector3 operator*(const Vector3& x) const - { - return (*this)(x); - } - - /**@brief Return the transform of the Quaternion */ - TF3SIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q) const - { - return getRotation() * q; - } - - /**@brief Return the basis matrix for the rotation */ - TF3SIMD_FORCE_INLINE Matrix3x3& getBasis() { return m_basis; } - /**@brief Return the basis matrix for the rotation */ - TF3SIMD_FORCE_INLINE const Matrix3x3& getBasis() const { return m_basis; } - - /**@brief Return the origin vector translation */ - TF3SIMD_FORCE_INLINE Vector3& getOrigin() { return m_origin; } - /**@brief Return the origin vector translation */ - TF3SIMD_FORCE_INLINE const Vector3& getOrigin() const { return m_origin; } - - /**@brief Return a quaternion representing the rotation */ - Quaternion getRotation() const { - Quaternion q; - m_basis.getRotation(q); - return q; - } - - - /**@brief Set from an array - * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ - void setFromOpenGLMatrix(const tf3Scalar *m) - { - m_basis.setFromOpenGLSubMatrix(m); - m_origin.setValue(m[12],m[13],m[14]); - } - - /**@brief Fill an array representation - * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ - void getOpenGLMatrix(tf3Scalar *m) const - { - m_basis.getOpenGLSubMatrix(m); - m[12] = m_origin.x(); - m[13] = m_origin.y(); - m[14] = m_origin.z(); - m[15] = tf3Scalar(1.0); - } - - /**@brief Set the translational element - * @param origin The vector to set the translation to */ - TF3SIMD_FORCE_INLINE void setOrigin(const Vector3& origin) - { - m_origin = origin; - } - - TF3SIMD_FORCE_INLINE Vector3 invXform(const Vector3& inVec) const; - - - /**@brief Set the rotational element by Matrix3x3 */ - TF3SIMD_FORCE_INLINE void setBasis(const Matrix3x3& basis) - { - m_basis = basis; - } - - /**@brief Set the rotational element by Quaternion */ - TF3SIMD_FORCE_INLINE void setRotation(const Quaternion& q) - { - m_basis.setRotation(q); - } - - - /**@brief Set this transformation to the identity */ - void setIdentity() - { - m_basis.setIdentity(); - m_origin.setValue(tf3Scalar(0.0), tf3Scalar(0.0), tf3Scalar(0.0)); - } - - /**@brief Multiply this Transform by another(this = this * another) - * @param t The other transform */ - Transform& operator*=(const Transform& t) - { - m_origin += m_basis * t.m_origin; - m_basis *= t.m_basis; - return *this; - } - - /**@brief Return the inverse of this transform */ - Transform inverse() const - { - Matrix3x3 inv = m_basis.transpose(); - return Transform(inv, inv * -m_origin); - } - - /**@brief Return the inverse of this transform times the other transform - * @param t The other transform - * return this.inverse() * the other */ - Transform inverseTimes(const Transform& t) const; - - /**@brief Return the product of this transform and the other */ - Transform operator*(const Transform& t) const; - - /**@brief Return an identity transform */ - static const Transform& getIdentity() - { - static const Transform identityTransform(Matrix3x3::getIdentity()); - return identityTransform; - } - - void serialize(struct TransformData& dataOut) const; - - void serializeFloat(struct TransformFloatData& dataOut) const; - - void deSerialize(const struct TransformData& dataIn); - - void deSerializeDouble(const struct TransformDoubleData& dataIn); - - void deSerializeFloat(const struct TransformFloatData& dataIn); - -}; - - -TF3SIMD_FORCE_INLINE Vector3 -Transform::invXform(const Vector3& inVec) const -{ - Vector3 v = inVec - m_origin; - return (m_basis.transpose() * v); -} - -TF3SIMD_FORCE_INLINE Transform -Transform::inverseTimes(const Transform& t) const -{ - Vector3 v = t.getOrigin() - m_origin; - return Transform(m_basis.transposeTimes(t.m_basis), - v * m_basis); -} - -TF3SIMD_FORCE_INLINE Transform -Transform::operator*(const Transform& t) const -{ - return Transform(m_basis * t.m_basis, - (*this)(t.m_origin)); -} - -/**@brief Test if two transforms have all elements equal */ -TF3SIMD_FORCE_INLINE bool operator==(const Transform& t1, const Transform& t2) -{ - return ( t1.getBasis() == t2.getBasis() && - t1.getOrigin() == t2.getOrigin() ); -} - - -///for serialization -struct TransformFloatData -{ - Matrix3x3FloatData m_basis; - Vector3FloatData m_origin; -}; - -struct TransformDoubleData -{ - Matrix3x3DoubleData m_basis; - Vector3DoubleData m_origin; -}; - - - -TF3SIMD_FORCE_INLINE void Transform::serialize(TransformData& dataOut) const -{ - m_basis.serialize(dataOut.m_basis); - m_origin.serialize(dataOut.m_origin); -} - -TF3SIMD_FORCE_INLINE void Transform::serializeFloat(TransformFloatData& dataOut) const -{ - m_basis.serializeFloat(dataOut.m_basis); - m_origin.serializeFloat(dataOut.m_origin); -} - - -TF3SIMD_FORCE_INLINE void Transform::deSerialize(const TransformData& dataIn) -{ - m_basis.deSerialize(dataIn.m_basis); - m_origin.deSerialize(dataIn.m_origin); -} - -TF3SIMD_FORCE_INLINE void Transform::deSerializeFloat(const TransformFloatData& dataIn) -{ - m_basis.deSerializeFloat(dataIn.m_basis); - m_origin.deSerializeFloat(dataIn.m_origin); -} - -TF3SIMD_FORCE_INLINE void Transform::deSerializeDouble(const TransformDoubleData& dataIn) -{ - m_basis.deSerializeDouble(dataIn.m_basis); - m_origin.deSerializeDouble(dataIn.m_origin); -} - -} - -#endif - - - - - - diff --git a/src/Libraries/tf3/include/tf3/LinearMath/Vector3.h b/src/Libraries/tf3/include/tf3/LinearMath/Vector3.h deleted file mode 100644 index 42416fc..0000000 --- a/src/Libraries/tf3/include/tf3/LinearMath/Vector3.h +++ /dev/null @@ -1,731 +0,0 @@ -/* -Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef TF3_VECTOR3_H -#define TF3_VECTOR3_H - - -#include "Scalar.h" -#include "MinMax.h" - -namespace tf3 -{ - -#define Vector3Data Vector3DoubleData -#define Vector3DataName "Vector3DoubleData" - - - - -/**@brief tf3::Vector3 can be used to represent 3D points and vectors. - * It has an un-used w component to suit 16-byte alignment when tf3::Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user - * Ideally, this class should be replaced by a platform optimized TF3SIMD version that keeps the data in registers - */ -ATTRIBUTE_ALIGNED16(class) Vector3 -{ -public: - -#if defined (__SPU__) && defined (__CELLOS_LV2__) - tf3Scalar m_floats[4]; -public: - TF3SIMD_FORCE_INLINE const vec_float4& get128() const - { - return *((const vec_float4*)&m_floats[0]); - } -public: -#else //__CELLOS_LV2__ __SPU__ -#ifdef TF3_USE_SSE // _WIN32 - union { - __m128 mVec128; - tf3Scalar m_floats[4]; - }; - TF3SIMD_FORCE_INLINE __m128 get128() const - { - return mVec128; - } - TF3SIMD_FORCE_INLINE void set128(__m128 v128) - { - mVec128 = v128; - } -#else - tf3Scalar m_floats[4]; -#endif -#endif //__CELLOS_LV2__ __SPU__ - - public: - - /**@brief No initialization constructor */ - TF3SIMD_FORCE_INLINE Vector3() {} - - - - /**@brief Constructor from scalars - * @param x X value - * @param y Y value - * @param z Z value - */ - TF3SIMD_FORCE_INLINE Vector3(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z) - { - m_floats[0] = x; - m_floats[1] = y; - m_floats[2] = z; - m_floats[3] = tf3Scalar(0.); - } - -/**@brief Add a vector to this one - * @param The vector to add to this one */ - TF3SIMD_FORCE_INLINE Vector3& operator+=(const Vector3& v) - { - - m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2]; - return *this; - } - - - /**@brief Sutf3ract a vector from this one - * @param The vector to sutf3ract */ - TF3SIMD_FORCE_INLINE Vector3& operator-=(const Vector3& v) - { - m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2]; - return *this; - } - /**@brief Scale the vector - * @param s Scale factor */ - TF3SIMD_FORCE_INLINE Vector3& operator*=(const tf3Scalar& s) - { - m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s; - return *this; - } - - /**@brief Inversely scale the vector - * @param s Scale factor to divide by */ - TF3SIMD_FORCE_INLINE Vector3& operator/=(const tf3Scalar& s) - { - tf3FullAssert(s != tf3Scalar(0.0)); - return *this *= tf3Scalar(1.0) / s; - } - - /**@brief Return the dot product - * @param v The other vector in the dot product */ - TF3SIMD_FORCE_INLINE tf3Scalar dot(const Vector3& v) const - { - return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2]; - } - - /**@brief Return the length of the vector squared */ - TF3SIMD_FORCE_INLINE tf3Scalar length2() const - { - return dot(*this); - } - - /**@brief Return the length of the vector */ - TF3SIMD_FORCE_INLINE tf3Scalar length() const - { - return tf3Sqrt(length2()); - } - - /**@brief Return the distance squared between the ends of this and another vector - * This is symantically treating the vector like a point */ - TF3SIMD_FORCE_INLINE tf3Scalar distance2(const Vector3& v) const; - - /**@brief Return the distance between the ends of this and another vector - * This is symantically treating the vector like a point */ - TF3SIMD_FORCE_INLINE tf3Scalar distance(const Vector3& v) const; - - /**@brief Normalize this vector - * x^2 + y^2 + z^2 = 1 */ - TF3SIMD_FORCE_INLINE Vector3& normalize() - { - return *this /= length(); - } - - /**@brief Return a normalized version of this vector */ - TF3SIMD_FORCE_INLINE Vector3 normalized() const; - - /**@brief Rotate this vector - * @param wAxis The axis to rotate about - * @param angle The angle to rotate by */ - TF3SIMD_FORCE_INLINE Vector3 rotate( const Vector3& wAxis, const tf3Scalar angle ) const; - - /**@brief Return the angle between this and another vector - * @param v The other vector */ - TF3SIMD_FORCE_INLINE tf3Scalar angle(const Vector3& v) const - { - tf3Scalar s = tf3Sqrt(length2() * v.length2()); - tf3FullAssert(s != tf3Scalar(0.0)); - return tf3Acos(dot(v) / s); - } - /**@brief Return a vector will the absolute values of each element */ - TF3SIMD_FORCE_INLINE Vector3 absolute() const - { - return Vector3( - tf3Fabs(m_floats[0]), - tf3Fabs(m_floats[1]), - tf3Fabs(m_floats[2])); - } - /**@brief Return the cross product between this and another vector - * @param v The other vector */ - TF3SIMD_FORCE_INLINE Vector3 cross(const Vector3& v) const - { - return Vector3( - m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1], - m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2], - m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]); - } - - TF3SIMD_FORCE_INLINE tf3Scalar triple(const Vector3& v1, const Vector3& v2) const - { - return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + - m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + - m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]); - } - - /**@brief Return the axis with the smallest value - * Note return values are 0,1,2 for x, y, or z */ - TF3SIMD_FORCE_INLINE int minAxis() const - { - return m_floats[0] < m_floats[1] ? (m_floats[0] return this, t=1 => return other) */ - TF3SIMD_FORCE_INLINE Vector3 lerp(const Vector3& v, const tf3Scalar& t) const - { - return Vector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t, - m_floats[1] + (v.m_floats[1] - m_floats[1]) * t, - m_floats[2] + (v.m_floats[2] -m_floats[2]) * t); - } - - /**@brief Elementwise multiply this vector by the other - * @param v The other vector */ - TF3SIMD_FORCE_INLINE Vector3& operator*=(const Vector3& v) - { - m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2]; - return *this; - } - - /**@brief Return the x value */ - TF3SIMD_FORCE_INLINE const tf3Scalar& getX() const { return m_floats[0]; } - /**@brief Return the y value */ - TF3SIMD_FORCE_INLINE const tf3Scalar& getY() const { return m_floats[1]; } - /**@brief Return the z value */ - TF3SIMD_FORCE_INLINE const tf3Scalar& getZ() const { return m_floats[2]; } - /**@brief Set the x value */ - TF3SIMD_FORCE_INLINE void setX(tf3Scalar x) { m_floats[0] = x;}; - /**@brief Set the y value */ - TF3SIMD_FORCE_INLINE void setY(tf3Scalar y) { m_floats[1] = y;}; - /**@brief Set the z value */ - TF3SIMD_FORCE_INLINE void setZ(tf3Scalar z) {m_floats[2] = z;}; - /**@brief Set the w value */ - TF3SIMD_FORCE_INLINE void setW(tf3Scalar w) { m_floats[3] = w;}; - /**@brief Return the x value */ - TF3SIMD_FORCE_INLINE const tf3Scalar& x() const { return m_floats[0]; } - /**@brief Return the y value */ - TF3SIMD_FORCE_INLINE const tf3Scalar& y() const { return m_floats[1]; } - /**@brief Return the z value */ - TF3SIMD_FORCE_INLINE const tf3Scalar& z() const { return m_floats[2]; } - /**@brief Return the w value */ - TF3SIMD_FORCE_INLINE const tf3Scalar& w() const { return m_floats[3]; } - - //TF3SIMD_FORCE_INLINE tf3Scalar& operator[](int i) { return (&m_floats[0])[i]; } - //TF3SIMD_FORCE_INLINE const tf3Scalar& operator[](int i) const { return (&m_floats[0])[i]; } - ///operator tf3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. - TF3SIMD_FORCE_INLINE operator tf3Scalar *() { return &m_floats[0]; } - TF3SIMD_FORCE_INLINE operator const tf3Scalar *() const { return &m_floats[0]; } - - TF3SIMD_FORCE_INLINE bool operator==(const Vector3& other) const - { - return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); - } - - TF3SIMD_FORCE_INLINE bool operator!=(const Vector3& other) const - { - return !(*this == other); - } - - /**@brief Set each element to the max of the current values and the values of another Vector3 - * @param other The other Vector3 to compare with - */ - TF3SIMD_FORCE_INLINE void setMax(const Vector3& other) - { - tf3SetMax(m_floats[0], other.m_floats[0]); - tf3SetMax(m_floats[1], other.m_floats[1]); - tf3SetMax(m_floats[2], other.m_floats[2]); - tf3SetMax(m_floats[3], other.w()); - } - /**@brief Set each element to the min of the current values and the values of another Vector3 - * @param other The other Vector3 to compare with - */ - TF3SIMD_FORCE_INLINE void setMin(const Vector3& other) - { - tf3SetMin(m_floats[0], other.m_floats[0]); - tf3SetMin(m_floats[1], other.m_floats[1]); - tf3SetMin(m_floats[2], other.m_floats[2]); - tf3SetMin(m_floats[3], other.w()); - } - - TF3SIMD_FORCE_INLINE void setValue(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z) - { - m_floats[0]=x; - m_floats[1]=y; - m_floats[2]=z; - m_floats[3] = tf3Scalar(0.); - } - - void getSkewSymmetricMatrix(Vector3* v0,Vector3* v1,Vector3* v2) const - { - v0->setValue(0. ,-z() ,y()); - v1->setValue(z() ,0. ,-x()); - v2->setValue(-y() ,x() ,0.); - } - - void setZero() - { - setValue(tf3Scalar(0.),tf3Scalar(0.),tf3Scalar(0.)); - } - - TF3SIMD_FORCE_INLINE bool isZero() const - { - return m_floats[0] == tf3Scalar(0) && m_floats[1] == tf3Scalar(0) && m_floats[2] == tf3Scalar(0); - } - - TF3SIMD_FORCE_INLINE bool fuzzyZero() const - { - return length2() < TF3SIMD_EPSILON; - } - - TF3SIMD_FORCE_INLINE void serialize(struct Vector3Data& dataOut) const; - - TF3SIMD_FORCE_INLINE void deSerialize(const struct Vector3Data& dataIn); - - TF3SIMD_FORCE_INLINE void serializeFloat(struct Vector3FloatData& dataOut) const; - - TF3SIMD_FORCE_INLINE void deSerializeFloat(const struct Vector3FloatData& dataIn); - - TF3SIMD_FORCE_INLINE void serializeDouble(struct Vector3DoubleData& dataOut) const; - - TF3SIMD_FORCE_INLINE void deSerializeDouble(const struct Vector3DoubleData& dataIn); - -}; - -/**@brief Return the sum of two vectors (Point symantics)*/ -TF3SIMD_FORCE_INLINE Vector3 -operator+(const Vector3& v1, const Vector3& v2) -{ - return Vector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]); -} - -/**@brief Return the elementwise product of two vectors */ -TF3SIMD_FORCE_INLINE Vector3 -operator*(const Vector3& v1, const Vector3& v2) -{ - return Vector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]); -} - -/**@brief Return the difference between two vectors */ -TF3SIMD_FORCE_INLINE Vector3 -operator-(const Vector3& v1, const Vector3& v2) -{ - return Vector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]); -} -/**@brief Return the negative of the vector */ -TF3SIMD_FORCE_INLINE Vector3 -operator-(const Vector3& v) -{ - return Vector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]); -} - -/**@brief Return the vector scaled by s */ -TF3SIMD_FORCE_INLINE Vector3 -operator*(const Vector3& v, const tf3Scalar& s) -{ - return Vector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s); -} - -/**@brief Return the vector scaled by s */ -TF3SIMD_FORCE_INLINE Vector3 -operator*(const tf3Scalar& s, const Vector3& v) -{ - return v * s; -} - -/**@brief Return the vector inversely scaled by s */ -TF3SIMD_FORCE_INLINE Vector3 -operator/(const Vector3& v, const tf3Scalar& s) -{ - tf3FullAssert(s != tf3Scalar(0.0)); - return v * (tf3Scalar(1.0) / s); -} - -/**@brief Return the vector inversely scaled by s */ -TF3SIMD_FORCE_INLINE Vector3 -operator/(const Vector3& v1, const Vector3& v2) -{ - return Vector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]); -} - -/**@brief Return the dot product between two vectors */ -TF3SIMD_FORCE_INLINE tf3Scalar -tf3Dot(const Vector3& v1, const Vector3& v2) -{ - return v1.dot(v2); -} - - -/**@brief Return the distance squared between two vectors */ -TF3SIMD_FORCE_INLINE tf3Scalar -tf3Distance2(const Vector3& v1, const Vector3& v2) -{ - return v1.distance2(v2); -} - - -/**@brief Return the distance between two vectors */ -TF3SIMD_FORCE_INLINE tf3Scalar -tf3Distance(const Vector3& v1, const Vector3& v2) -{ - return v1.distance(v2); -} - -/**@brief Return the angle between two vectors */ -TF3SIMD_FORCE_INLINE tf3Scalar -tf3Angle(const Vector3& v1, const Vector3& v2) -{ - return v1.angle(v2); -} - -/**@brief Return the cross product of two vectors */ -TF3SIMD_FORCE_INLINE Vector3 -tf3Cross(const Vector3& v1, const Vector3& v2) -{ - return v1.cross(v2); -} - -TF3SIMD_FORCE_INLINE tf3Scalar -tf3Triple(const Vector3& v1, const Vector3& v2, const Vector3& v3) -{ - return v1.triple(v2, v3); -} - -/**@brief Return the linear interpolation between two vectors - * @param v1 One vector - * @param v2 The other vector - * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */ -TF3SIMD_FORCE_INLINE Vector3 -lerp(const Vector3& v1, const Vector3& v2, const tf3Scalar& t) -{ - return v1.lerp(v2, t); -} - - - -TF3SIMD_FORCE_INLINE tf3Scalar Vector3::distance2(const Vector3& v) const -{ - return (v - *this).length2(); -} - -TF3SIMD_FORCE_INLINE tf3Scalar Vector3::distance(const Vector3& v) const -{ - return (v - *this).length(); -} - -TF3SIMD_FORCE_INLINE Vector3 Vector3::normalized() const -{ - return *this / length(); -} - -TF3SIMD_FORCE_INLINE Vector3 Vector3::rotate( const Vector3& wAxis, const tf3Scalar angle ) const -{ - // wAxis must be a unit lenght vector - - Vector3 o = wAxis * wAxis.dot( *this ); - Vector3 x = *this - o; - Vector3 y; - - y = wAxis.cross( *this ); - - return ( o + x * tf3Cos( angle ) + y * tf3Sin( angle ) ); -} - -class tf3Vector4 : public Vector3 -{ -public: - - TF3SIMD_FORCE_INLINE tf3Vector4() {} - - - TF3SIMD_FORCE_INLINE tf3Vector4(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z,const tf3Scalar& w) - : Vector3(x,y,z) - { - m_floats[3] = w; - } - - - TF3SIMD_FORCE_INLINE tf3Vector4 absolute4() const - { - return tf3Vector4( - tf3Fabs(m_floats[0]), - tf3Fabs(m_floats[1]), - tf3Fabs(m_floats[2]), - tf3Fabs(m_floats[3])); - } - - - - tf3Scalar getW() const { return m_floats[3];} - - - TF3SIMD_FORCE_INLINE int maxAxis4() const - { - int maxIndex = -1; - tf3Scalar maxVal = tf3Scalar(-TF3_LARGE_FLOAT); - if (m_floats[0] > maxVal) - { - maxIndex = 0; - maxVal = m_floats[0]; - } - if (m_floats[1] > maxVal) - { - maxIndex = 1; - maxVal = m_floats[1]; - } - if (m_floats[2] > maxVal) - { - maxIndex = 2; - maxVal =m_floats[2]; - } - if (m_floats[3] > maxVal) - { - maxIndex = 3; - } - - - - - return maxIndex; - - } - - - TF3SIMD_FORCE_INLINE int minAxis4() const - { - int minIndex = -1; - tf3Scalar minVal = tf3Scalar(TF3_LARGE_FLOAT); - if (m_floats[0] < minVal) - { - minIndex = 0; - minVal = m_floats[0]; - } - if (m_floats[1] < minVal) - { - minIndex = 1; - minVal = m_floats[1]; - } - if (m_floats[2] < minVal) - { - minIndex = 2; - minVal =m_floats[2]; - } - if (m_floats[3] < minVal) - { - minIndex = 3; - } - - return minIndex; - - } - - - TF3SIMD_FORCE_INLINE int closestAxis4() const - { - return absolute4().maxAxis4(); - } - - - - - /**@brief Set x,y,z and zero w - * @param x Value of x - * @param y Value of y - * @param z Value of z - */ - - -/* void getValue(tf3Scalar *m) const - { - m[0] = m_floats[0]; - m[1] = m_floats[1]; - m[2] =m_floats[2]; - } -*/ -/**@brief Set the values - * @param x Value of x - * @param y Value of y - * @param z Value of z - * @param w Value of w - */ - TF3SIMD_FORCE_INLINE void setValue(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z,const tf3Scalar& w) - { - m_floats[0]=x; - m_floats[1]=y; - m_floats[2]=z; - m_floats[3]=w; - } - - -}; - - -///tf3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization -TF3SIMD_FORCE_INLINE void tf3SwapScalarEndian(const tf3Scalar& sourceVal, tf3Scalar& destVal) -{ - unsigned char* dest = (unsigned char*) &destVal; - const unsigned char* src = (const unsigned char*) &sourceVal; - dest[0] = src[7]; - dest[1] = src[6]; - dest[2] = src[5]; - dest[3] = src[4]; - dest[4] = src[3]; - dest[5] = src[2]; - dest[6] = src[1]; - dest[7] = src[0]; -} -///tf3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization -TF3SIMD_FORCE_INLINE void tf3SwapVector3Endian(const Vector3& sourceVec, Vector3& destVec) -{ - for (int i=0;i<4;i++) - { - tf3SwapScalarEndian(sourceVec[i],destVec[i]); - } - -} - -///tf3UnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization -TF3SIMD_FORCE_INLINE void tf3UnSwapVector3Endian(Vector3& vector) -{ - - Vector3 swappedVec; - for (int i=0;i<4;i++) - { - tf3SwapScalarEndian(vector[i],swappedVec[i]); - } - vector = swappedVec; -} - -TF3SIMD_FORCE_INLINE void tf3PlaneSpace1 (const Vector3& n, Vector3& p, Vector3& q) -{ - if (tf3Fabs(n.z()) > TF3SIMDSQRT12) { - // choose p in y-z plane - tf3Scalar a = n[1]*n[1] + n[2]*n[2]; - tf3Scalar k = tf3RecipSqrt (a); - p.setValue(0,-n[2]*k,n[1]*k); - // set q = n x p - q.setValue(a*k,-n[0]*p[2],n[0]*p[1]); - } - else { - // choose p in x-y plane - tf3Scalar a = n.x()*n.x() + n.y()*n.y(); - tf3Scalar k = tf3RecipSqrt (a); - p.setValue(-n.y()*k,n.x()*k,0); - // set q = n x p - q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k); - } -} - - -struct Vector3FloatData -{ - float m_floats[4]; -}; - -struct Vector3DoubleData -{ - double m_floats[4]; - -}; - -TF3SIMD_FORCE_INLINE void Vector3::serializeFloat(struct Vector3FloatData& dataOut) const -{ - ///could also do a memcpy, check if it is worth it - for (int i=0;i<4;i++) - dataOut.m_floats[i] = float(m_floats[i]); -} - -TF3SIMD_FORCE_INLINE void Vector3::deSerializeFloat(const struct Vector3FloatData& dataIn) -{ - for (int i=0;i<4;i++) - m_floats[i] = tf3Scalar(dataIn.m_floats[i]); -} - - -TF3SIMD_FORCE_INLINE void Vector3::serializeDouble(struct Vector3DoubleData& dataOut) const -{ - ///could also do a memcpy, check if it is worth it - for (int i=0;i<4;i++) - dataOut.m_floats[i] = double(m_floats[i]); -} - -TF3SIMD_FORCE_INLINE void Vector3::deSerializeDouble(const struct Vector3DoubleData& dataIn) -{ - for (int i=0;i<4;i++) - m_floats[i] = tf3Scalar(dataIn.m_floats[i]); -} - - -TF3SIMD_FORCE_INLINE void Vector3::serialize(struct Vector3Data& dataOut) const -{ - ///could also do a memcpy, check if it is worth it - for (int i=0;i<4;i++) - dataOut.m_floats[i] = m_floats[i]; -} - -TF3SIMD_FORCE_INLINE void Vector3::deSerialize(const struct Vector3Data& dataIn) -{ - for (int i=0;i<4;i++) - m_floats[i] = dataIn.m_floats[i]; -} - -} - -#endif //TF3_VECTOR3_H diff --git a/src/Libraries/tf3/include/tf3/TF3Error.h b/src/Libraries/tf3/include/tf3/TF3Error.h deleted file mode 100644 index de12fbc..0000000 --- a/src/Libraries/tf3/include/tf3/TF3Error.h +++ /dev/null @@ -1,11 +0,0 @@ -// TF3 error codes in tf3 namespace (no tf3_msgs dependency) -#ifndef TF3_ERROR_TF3_H -#define TF3_ERROR_TF3_H - -namespace tf3 { -struct TF3Error { - enum { NO_ERROR = 0, LOOKUP_ERROR = 1, CONNECTIVITY_ERROR = 2, EXTRAPOLATION_ERROR = 3 }; -}; -} - -#endif // TF3_ERROR_TF3_H diff --git a/src/Libraries/tf3/include/tf3/buffer_core.h b/src/Libraries/tf3/include/tf3/buffer_core.h deleted file mode 100644 index 75f0fc6..0000000 --- a/src/Libraries/tf3/include/tf3/buffer_core.h +++ /dev/null @@ -1,432 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ - -/** \author Tully Foote */ - -#ifndef TF3_BUFFER_CORE_H -#define TF3_BUFFER_CORE_H - -#include "transform_storage.h" - -#include - -#include - -#include "tf3/compat.h" -#include "tf3/time.h" - - -//////////////////////////backwards startup for porting -//#include "tf/tf.h" - -#include -#include -#include -#include - -namespace tf3 -{ - -typedef std::pair P_TimeAndFrameID; -typedef uint32_t TransformableCallbackHandle; -typedef uint64_t TransformableRequestHandle; - -class TimeCacheInterface; -typedef boost::shared_ptr TimeCacheInterfacePtr; - -enum TransformableResult -{ - TransformAvailable, - TransformFailure, -}; - -/** \brief A Class which provides coordinate transforms between any two frames in a system. - * - * This class provides a simple interface to allow recording and lookup of - * relationships between arbitrary frames of the system. - * - * libTF assumes that there is a tree of coordinate frame transforms which define the relationship between all coordinate frames. - * For example your typical robot would have a transform from global to real world. And then from base to hand, and from base to head. - * But Base to Hand really is composed of base to shoulder to elbow to wrist to hand. - * libTF is designed to take care of all the intermediate steps for you. - * - * Internal Representation - * libTF will store frames with the parameters necessary for generating the transform into that frame from it's parent and a reference to the parent frame. - * Frames are designated using an std::string - * 0 is a frame without a parent (the top of a tree) - * The positions of frames over time must be pushed in. - * - * All function calls which pass frame ids can potentially throw the exception tf::LookupException - */ -class BufferCore -{ -public: - /************* Constants ***********************/ - static const int DEFAULT_CACHE_TIME = 10; //!< The default amount of time to cache data in seconds - static const uint32_t MAX_GRAPH_DEPTH = 1000UL; //!< Maximum graph search depth (deeper graphs will be assumed to have loops) - - /** Constructor - * \param interpolating Whether to interpolate, if this is false the closest value will be returned - * \param cache_time How long to keep a history of transforms in nanoseconds - * - */ - BufferCore(tf3::Duration cache_time_ = tf3::Duration(DEFAULT_CACHE_TIME)); - virtual ~BufferCore(void); - - /** \brief Clear all data */ - void clear(); - - /** \brief Add transform information to the tf data structure - * \param transform The transform to store - * \param authority The source of the information for this transform - * \param is_static Record this transform as a static transform. It will be good across all time. (This cannot be changed after the first call.) - * \return True unless an error occured - */ - bool setTransform(const tf3::TransformStampedMsg& transform, const std::string & authority, bool is_static = false); - - /*********** Accessors *************/ - - /** \brief Get the transform between two frames by frame ID. - * \param target_frame The frame to which data should be transformed - * \param source_frame The frame where the data originated - * \param time The time at which the value of the transform is desired. (0 will get the latest) - * \return The transform between the frames - * - * Possible exceptions tf3::LookupException, tf3::ConnectivityException, - * tf3::ExtrapolationException, tf3::InvalidArgumentException - */ - tf3::TransformStampedMsg - lookupTransform(const std::string& target_frame, const std::string& source_frame, - const tf3::Time& time) const; - - /** \brief Get the transform between two frames by frame ID assuming fixed frame. - * \param target_frame The frame to which data should be transformed - * \param target_time The time to which the data should be transformed. (0 will get the latest) - * \param source_frame The frame where the data originated - * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) - * \param fixed_frame The frame in which to assume the transform is constant in time. - * \return The transform between the frames - * - * Possible exceptions tf3::LookupException, tf3::ConnectivityException, - * tf3::ExtrapolationException, tf3::InvalidArgumentException - */ - - tf3::TransformStampedMsg - lookupTransform(const std::string& target_frame, const tf3::Time& target_time, - const std::string& source_frame, const tf3::Time& source_time, - const std::string& fixed_frame) const; - - /* \brief Lookup the twist of the tracking_frame with respect to the observation frame in the reference_frame using the reference point - * \param tracking_frame The frame to track - * \param observation_frame The frame from which to measure the twist - * \param reference_frame The reference frame in which to express the twist - * \param reference_point The reference point with which to express the twist - * \param reference_point_frame The frame_id in which the reference point is expressed - * \param time The time at which to get the velocity - * \param duration The period over which to average - * \return twist The twist output - * - * This will compute the average velocity on the interval - * (time - duration/2, time+duration/2). If that is too close to the most - * recent reading, in which case it will shift the interval up to - * duration/2 to prevent extrapolation. - * - * Possible exceptions tf3::LookupException, tf3::ConnectivityException, - * tf3::ExtrapolationException, tf3::InvalidArgumentException - * - * New in geometry 1.1 - */ - /* - robot_geometry_msgs::Twist - lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame, - const tf::Point & reference_point, const std::string& reference_point_frame, - const robot::Time& time, const robot::Duration& averaging_interval) const; - */ - /* \brief lookup the twist of the tracking frame with respect to the observational frame - * - * This is a simplified version of - * lookupTwist with it assumed that the reference point is the - * origin of the tracking frame, and the reference frame is the - * observation frame. - * - * Possible exceptions tf3::LookupException, tf3::ConnectivityException, - * tf3::ExtrapolationException, tf3::InvalidArgumentException - * - * New in geometry 1.1 - */ - /* - robot_geometry_msgs::Twist - lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, - const robot::Time& time, const robot::Duration& averaging_interval) const; - */ - /** \brief Test if a transform is possible - * \param target_frame The frame into which to transform - * \param source_frame The frame from which to transform - * \param time The time at which to transform - * \param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL - * \return True if the transform is possible, false otherwise - */ - bool canTransform(const std::string& target_frame, const std::string& source_frame, - const tf3::Time& time, std::string* error_msg = NULL) const; - - /** \brief Test if a transform is possible - * \param target_frame The frame into which to transform - * \param target_time The time into which to transform - * \param source_frame The frame from which to transform - * \param source_time The time from which to transform - * \param fixed_frame The frame in which to treat the transform as constant in time - * \param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL - * \return True if the transform is possible, false otherwise - */ - bool canTransform(const std::string& target_frame, const tf3::Time& target_time, - const std::string& source_frame, const tf3::Time& source_time, - const std::string& fixed_frame, std::string* error_msg = NULL) const; - - /** \brief A way to see what frames have been cached in yaml format - * Useful for debugging tools - */ - std::string allFramesAsYAML(double current_time) const; - - /** Backwards compatibility for #84 - */ - std::string allFramesAsYAML() const; - - /** \brief A way to see what frames have been cached - * Useful for debugging - */ - std::string allFramesAsString() const; - - typedef boost::function TransformableCallback; - - /// \brief Internal use only - TransformableCallbackHandle addTransformableCallback(const TransformableCallback& cb); - /// \brief Internal use only - void removeTransformableCallback(TransformableCallbackHandle handle); - /// \brief Internal use only - TransformableRequestHandle addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, tf3::Time time); - /// \brief Internal use only - void cancelTransformableRequest(TransformableRequestHandle handle); - - - - - // Tell the buffer that there are multiple threads serviciing it. - // This is useful for derived classes to know if they can block or not. - void setUsingDedicatedThread(bool value) { using_dedicated_thread_ = value;}; - // Get the state of using_dedicated_thread_ - bool isUsingDedicatedThread() const { return using_dedicated_thread_;}; - - - - - /* Backwards compatability section for tf::Transformer you should not use these - */ - - /** - * \brief Add a callback that happens when a new transform has arrived - * - * \param callback The callback, of the form void func(); - * \return A boost::signals2::connection object that can be used to remove this - * listener - */ - boost::signals2::connection _addTransformsChangedListener(boost::function callback); - void _removeTransformsChangedListener(boost::signals2::connection c); - - - /**@brief Check if a frame exists in the tree - * @param frame_id_str The frame id in question */ - bool _frameExists(const std::string& frame_id_str) const; - - /**@brief Fill the parent of a frame. - * @param frame_id The frame id of the frame in question - * @param parent The reference to the string to fill the parent - * Returns true unless "NO_PARENT" */ - bool _getParent(const std::string& frame_id, tf3::Time time, std::string& parent) const; - - /** \brief A way to get a std::vector of available frame ids */ - void _getFrameStrings(std::vector& ids) const; - - - CompactFrameID _lookupFrameNumber(const std::string& frameid_str) const { - return lookupFrameNumber(frameid_str); - } - CompactFrameID _lookupOrInsertFrameNumber(const std::string& frameid_str) { - return lookupOrInsertFrameNumber(frameid_str); - } - - int _getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, tf3::Time& time, std::string* error_string) const { - boost::mutex::scoped_lock lock(frame_mutex_); - return getLatestCommonTime(target_frame, source_frame, time, error_string); - } - - CompactFrameID _validateFrameId(const char* function_name_arg, const std::string& frame_id) const { - return validateFrameId(function_name_arg, frame_id); - } - - /**@brief Get the duration over which this transformer will cache */ - tf3::Duration getCacheLength() { return cache_time_;} - - /** \brief Backwards compatabilityA way to see what frames have been cached - * Useful for debugging - */ - std::string _allFramesAsDot(double current_time) const; - std::string _allFramesAsDot() const; - - /** \brief Backwards compatabilityA way to see what frames are in a chain - * Useful for debugging - */ - void _chainAsVector(const std::string & target_frame, tf3::Time target_time, const std::string & source_frame, tf3::Time source_time, const std::string & fixed_frame, std::vector& output) const; - -private: - - /** \brief A way to see what frames have been cached - * Useful for debugging. Use this call internally. - */ - std::string allFramesAsStringNoLock() const; - - - /******************** Internal Storage ****************/ - - /** \brief The pointers to potential frames that the tree can be made of. - * The frames will be dynamically allocated at run time when set the first time. */ - typedef std::vector V_TimeCacheInterface; - V_TimeCacheInterface frames_; - - /** \brief A mutex to protect testing and allocating new frames on the above vector. */ - mutable boost::mutex frame_mutex_; - - /** \brief A map from string frame ids to CompactFrameID */ - typedef boost::unordered_map M_StringToCompactFrameID; - M_StringToCompactFrameID frameIDs_; - /** \brief A map from CompactFrameID frame_id_numbers to string for debugging and output */ - std::vector frameIDs_reverse; - /** \brief A map to lookup the most recent authority for a given frame */ - std::map frame_authority_; - - - /// How long to cache transform history - tf3::Duration cache_time_; - - typedef boost::unordered_map M_TransformableCallback; - M_TransformableCallback transformable_callbacks_; - uint32_t transformable_callbacks_counter_; - boost::mutex transformable_callbacks_mutex_; - - struct TransformableRequest - { - tf3::Time time; - TransformableRequestHandle request_handle; - TransformableCallbackHandle cb_handle; - CompactFrameID target_id; - CompactFrameID source_id; - std::string target_string; - std::string source_string; - }; - typedef std::vector V_TransformableRequest; - V_TransformableRequest transformable_requests_; - boost::mutex transformable_requests_mutex_; - uint64_t transformable_requests_counter_; - - struct RemoveRequestByCallback; - struct RemoveRequestByID; - - // Backwards compatability for tf message_filter - typedef boost::signals2::signal TransformsChangedSignal; - /// Signal which is fired whenever new transform data has arrived, from the thread the data arrived in - TransformsChangedSignal _transforms_changed_; - - - /************************* Internal Functions ****************************/ - - /** \brief An accessor to get a frame, which will throw an exception if the frame is no there. - * \param frame_number The frameID of the desired Reference Frame - * - * This is an internal function which will get the pointer to the frame associated with the frame id - * Possible Exception: tf::LookupException - */ - TimeCacheInterfacePtr getFrame(CompactFrameID c_frame_id) const; - - TimeCacheInterfacePtr allocateFrame(CompactFrameID cfid, bool is_static); - - - bool warnFrameId(const char* function_name_arg, const std::string& frame_id) const; - CompactFrameID validateFrameId(const char* function_name_arg, const std::string& frame_id) const; - - /// String to number for frame lookup with dynamic allocation of new frames - CompactFrameID lookupFrameNumber(const std::string& frameid_str) const; - - /// String to number for frame lookup with dynamic allocation of new frames - CompactFrameID lookupOrInsertFrameNumber(const std::string& frameid_str); - - ///Number to string frame lookup may throw LookupException if number invalid - const std::string& lookupFrameString(CompactFrameID frame_id_num) const; - - void createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const; - - /**@brief Return the latest rostime which is common across the spanning set - * zero if fails to cross */ - int getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, tf3::Time& time, std::string* error_string) const; - - template - int walkToTopParent(F& f, tf3::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const; - - /**@brief Traverse the transform tree. If frame_chain is not NULL, store the traversed frame tree in vector frame_chain. - * */ - template - int walkToTopParent(F& f, tf3::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector *frame_chain) const; - - void testTransformableRequests(); - bool canTransformInternal(CompactFrameID target_id, CompactFrameID source_id, - const tf3::Time& time, std::string* error_msg) const; - bool canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id, - const tf3::Time& time, std::string* error_msg) const; - - - //Whether it is safe to use canTransform with a timeout. (If another thread is not provided it will always timeout.) - bool using_dedicated_thread_; - -public: - friend class TestBufferCore; // For unit testing - -}; - -/** A helper class for testing internal APIs */ -class TestBufferCore -{ -public: - int _walkToTopParent(BufferCore& buffer, tf3::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector *frame_chain) const; - const std::string& _lookupFrameString(BufferCore& buffer, CompactFrameID frame_id_num) const - { - return buffer.lookupFrameString(frame_id_num); - } -}; -} - -#endif //TF3_CORE_H diff --git a/src/Libraries/tf3/include/tf3/compat.h b/src/Libraries/tf3/include/tf3/compat.h deleted file mode 100644 index d4cabb2..0000000 --- a/src/Libraries/tf3/include/tf3/compat.h +++ /dev/null @@ -1,31 +0,0 @@ -// Compatibility types to avoid using robot:: or robot_geometry_msgs:: namespaces inside tf3 -#ifndef TF3_COMPAT_H -#define TF3_COMPAT_H - -#include "tf3/time.h" -#include - -namespace tf3 { - -// Minimal header/message equivalents owned by tf3 (no robot:: or robot_geometry_msgs::) -struct HeaderMsg -{ - uint32_t seq; - Time stamp; - std::string frame_id; -}; - -struct Vector3Msg { double x, y, z; }; -struct QuaternionMsg { double x, y, z, w; }; -struct TransformMsg { Vector3Msg translation; QuaternionMsg rotation; }; - -struct TransformStampedMsg -{ - HeaderMsg header; - std::string child_frame_id; - TransformMsg transform; -}; - -} // namespace tf3 - -#endif // TF3_COMPAT_H diff --git a/src/Libraries/tf3/include/tf3/convert.h b/src/Libraries/tf3/include/tf3/convert.h deleted file mode 100644 index 6516370..0000000 --- a/src/Libraries/tf3/include/tf3/convert.h +++ /dev/null @@ -1,131 +0,0 @@ -/* - * Copyright (c) 2013, Open Source Robotics Foundation - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ - -/** \author Tully Foote */ - -#ifndef TF3_CONVERT_H -#define TF3_CONVERT_H - - -#include -#include -#include -#include -#include - -namespace tf3 { - -/**\brief The templated function expected to be able to do a transform - * - * This is the method which tf3 will use to try to apply a transform for any given datatype. - * \param data_in The data to be transformed. - * \param data_out A reference to the output data. Note this can point to data in and the method should be mutation safe. - * \param transform The transform to apply to data_in to fill data_out. - * - * This method needs to be implemented by client library developers - */ -template - void doTransform(const T& data_in, T& data_out, const tf3::TransformStampedMsg& transform); - -/**\brief Get the timestamp from data - * \param t The data input. - * \return The timestamp associated with the data. The lifetime of the returned - * reference is bound to the lifetime of the argument. - */ -template - const tf3::Time& getTimestamp(const T& t); - -/**\brief Get the frame_id from data - * \param t The data input. - * \return The frame_id associated with the data. The lifetime of the returned - * reference is bound to the lifetime of the argument. - */ -template - const std::string& getFrameId(const T& t); - - - -/* An implementation for Stamped

datatypes */ -template - const tf3::Time& getTimestamp(const tf3::Stamped

& t) - { - return t.stamp_; - } - -/* An implementation for Stamped

datatypes */ -template - const std::string& getFrameId(const tf3::Stamped

& t) - { - return t.frame_id_; - } - -/** Function that converts from one type to a ROS message type. It has to be - * implemented by each data type in tf3_* (except ROS messages) as it is - * used in the "convert" function. - * \param a an object of whatever type - * \return the conversion as a ROS message - */ -template - B toMsg(const A& a); - -/** Function that converts from a ROS message type to another type. It has to be - * implemented by each data type in tf3_* (except ROS messages) as it is used - * in the "convert" function. - * \param a a ROS message to convert from - * \param b the object to convert to - */ -template - void fromMsg(const A&, B& b); - -/** Function that converts any type to any type (messages or not). - * Matching toMsg and from Msg conversion functions need to exist. - * If they don't exist or do not apply (for example, if your two - * classes are ROS messages), just write a specialization of the function. - * \param a an object to convert from - * \param b the object to convert to - */ -template - void convert(const A& a, B& b) - { - //printf("In double type convert\n"); - impl::Converter::value, tf3::message_traits::IsMessage::value>::convert(a, b); - } - -template - void convert(const A& a1, A& a2) - { - //printf("In single type convert\n"); - if(&a1 != &a2) - a2 = a1; - } - - -} - -#endif //TF3_CONVERT_H diff --git a/src/Libraries/tf3/include/tf3/exceptions.h b/src/Libraries/tf3/include/tf3/exceptions.h deleted file mode 100644 index 8ca6a52..0000000 --- a/src/Libraries/tf3/include/tf3/exceptions.h +++ /dev/null @@ -1,110 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ - -/** \author Tully Foote */ - -#ifndef TF3_EXCEPTIONS_H -#define TF3_EXCEPTIONS_H - -#include - -namespace tf3{ - -/** \brief A base class for all tf3 exceptions - * This inherits from robot::exception - * which inherits from std::runtime_exception - */ -class TransformException: public std::runtime_error -{ -public: - TransformException(const std::string errorDescription) : std::runtime_error(errorDescription) { ; }; -}; - - - /** \brief An exception class to notify of no connection - * - * This is an exception class to be thrown in the case - * that the Reference Frame tree is not connected between - * the frames requested. */ -class ConnectivityException:public TransformException -{ -public: - ConnectivityException(const std::string errorDescription) : tf3::TransformException(errorDescription) { ; }; -}; - - -/** \brief An exception class to notify of bad frame number - * - * This is an exception class to be thrown in the case that - * a frame not in the graph has been attempted to be accessed. - * The most common reason for this is that the frame is not - * being published, or a parent frame was not set correctly - * causing the tree to be broken. - */ -class LookupException: public TransformException -{ -public: - LookupException(const std::string errorDescription) : tf3::TransformException(errorDescription) { ; }; -}; - - /** \brief An exception class to notify that the requested value would have required extrapolation beyond current limits. - * - */ -class ExtrapolationException: public TransformException -{ -public: - ExtrapolationException(const std::string errorDescription) : tf3::TransformException(errorDescription) { ; }; -}; - -/** \brief An exception class to notify that one of the arguments is invalid - * - * usually it's an uninitalized Quaternion (0,0,0,0) - * - */ -class InvalidArgumentException: public TransformException -{ -public: - InvalidArgumentException(const std::string errorDescription) : tf3::TransformException(errorDescription) { ; }; -}; - -/** \brief An exception class to notify that a timeout has occured - * - * - */ -class TimeoutException: public TransformException -{ -public: - TimeoutException(const std::string errorDescription) : tf3::TransformException(errorDescription) { ; }; -}; - - -} - - -#endif //TF3_EXCEPTIONS_H diff --git a/src/Libraries/tf3/include/tf3/impl/convert.h b/src/Libraries/tf3/include/tf3/impl/convert.h deleted file mode 100644 index 8b16331..0000000 --- a/src/Libraries/tf3/include/tf3/impl/convert.h +++ /dev/null @@ -1,90 +0,0 @@ -/* - * Copyright (c) 2013, Open Source Robotics Foundation - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ - -#ifndef TF3_IMPL_CONVERT_H -#define TF3_IMPL_CONVERT_H - -namespace tf3 { -namespace impl { - -template -class Converter { -public: - template - static void convert(const A& a, B& b); -}; - -// The case where both A and B are messages should not happen: if you have two -// messages that are interchangeable, well, that's against the ROS purpose: -// only use one type. Worst comes to worst, specialize the original convert -// function for your types. -// if B == A, the templated version of convert with only one argument will be -// used. -// -//template <> -//template -//inline void Converter::convert(const A& a, B& b); - -template <> -template -inline void Converter::convert(const A& a, B& b) -{ -#ifdef _MSC_VER - tf3::fromMsg(a, b); -#else - fromMsg(a, b); -#endif -} - -template <> -template -inline void Converter::convert(const A& a, B& b) -{ -#ifdef _MSC_VER - b = tf3::toMsg(a); -#else - b = toMsg(a); -#endif -} - -template <> -template -inline void Converter::convert(const A& a, B& b) -{ -#ifdef _MSC_VER - tf3::fromMsg(tf3::toMsg(a), b); -#else - fromMsg(toMsg(a), b); -#endif -} - -} -} - -#endif //TF3_IMPL_CONVERT_H diff --git a/src/Libraries/tf3/include/tf3/impl/utils.h b/src/Libraries/tf3/include/tf3/impl/utils.h deleted file mode 100644 index 65f959f..0000000 --- a/src/Libraries/tf3/include/tf3/impl/utils.h +++ /dev/null @@ -1,142 +0,0 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. -// -// 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 TF3_IMPL_UTILS_H -#define TF3_IMPL_UTILS_H - -#include -#include -#include - -namespace tf3 { -namespace impl { - -/** Function needed for the generalization of toQuaternion - * \param q a tf3::Quaternion - * \return a copy of the same quaternion - */ -inline -tf3::Quaternion toQuaternion(const tf3::Quaternion& q) { - return q; - } - -/** Function needed for the generalization of toQuaternion - * \param q a tf3::QuaternionMsg wrapped in a stamped container - * \return a copy of the same quaternion as a tf3::Quaternion - */ -inline -tf3::Quaternion toQuaternion(const tf3::QuaternionMsg& q) { - tf3::Quaternion res; - res.setValue(q.x, q.y, q.z, q.w); - return res; - } - -/** Function needed for the generalization of toQuaternion - * \param t some tf3::Stamped object - * \return a copy of the same quaternion as a tf3::Quaternion - */ -template - tf3::Quaternion toQuaternion(const tf3::Stamped& t) { - tf3::QuaternionMsg q = toMsg(t); - return toQuaternion(q); - } - -/** Generic version of toQuaternion. It tries to convert the argument - * to a robot_geometry_msgs::Quaternion - * \param t some object - * \return a copy of the same quaternion as a tf3::Quaternion - */ -template - tf3::Quaternion toQuaternion(const T& t) { - tf3::QuaternionMsg q = toMsg(t); - return toQuaternion(q); - } - -/** The code below is blantantly copied from urdfdom_headers - * only the normalization has been added. - * It computes the Euler roll, pitch yaw from a tf3::Quaternion - * It is equivalent to tf3::Matrix3x3(q).getEulerYPR(yaw, pitch, roll); - * \param q a tf3::Quaternion - * \param yaw the computed yaw - * \param pitch the computed pitch - * \param roll the computed roll - */ -inline -void getEulerYPR(const tf3::Quaternion& q, double &yaw, double &pitch, double &roll) -{ - double sqw; - double sqx; - double sqy; - double sqz; - - sqx = q.x() * q.x(); - sqy = q.y() * q.y(); - sqz = q.z() * q.z(); - sqw = q.w() * q.w(); - - // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ - double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */ - if (sarg <= -0.99999) { - pitch = -0.5*M_PI; - roll = 0; - yaw = -2 * atan2(q.y(), q.x()); - } else if (sarg >= 0.99999) { - pitch = 0.5*M_PI; - roll = 0; - yaw = 2 * atan2(q.y(), q.x()); - } else { - pitch = asin(sarg); - roll = atan2(2 * (q.y()*q.z() + q.w()*q.x()), sqw - sqx - sqy + sqz); - yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz); - } -} - -/** The code below is a simplified version of getEulerRPY that only - * returns the yaw. It is mostly useful in navigation where only yaw - * matters - * \param q a tf3::Quaternion - * \return the computed yaw - */ -inline -double getYaw(const tf3::Quaternion& q) -{ - double yaw; - - double sqw; - double sqx; - double sqy; - double sqz; - - sqx = q.x() * q.x(); - sqy = q.y() * q.y(); - sqz = q.z() * q.z(); - sqw = q.w() * q.w(); - - // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ - double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */ - - if (sarg <= -0.99999) { - yaw = -2 * atan2(q.y(), q.x()); - } else if (sarg >= 0.99999) { - yaw = 2 * atan2(q.y(), q.x()); - } else { - yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz); - } - return yaw; -} - -} -} - -#endif //TF3_IMPL_UTILS_H diff --git a/src/Libraries/tf3/include/tf3/macros.h b/src/Libraries/tf3/include/tf3/macros.h deleted file mode 100644 index 00b26d2..0000000 --- a/src/Libraries/tf3/include/tf3/macros.h +++ /dev/null @@ -1,13 +0,0 @@ -// Minimal compatibility macros used by tf3 to avoid depending on ROS -#ifndef TF3_MACROS_H_ -#define TF3_MACROS_H_ - -#if defined(__clang__) || defined(__GNUC__) -# define ROS_DEPRECATED __attribute__((deprecated)) -#elif defined(_MSC_VER) -# define ROS_DEPRECATED __declspec(deprecated) -#else -# define ROS_DEPRECATED -#endif - -#endif // TF3_MACROS_H_ diff --git a/src/Libraries/tf3/include/tf3/message_traits.h b/src/Libraries/tf3/include/tf3/message_traits.h deleted file mode 100644 index d826231..0000000 --- a/src/Libraries/tf3/include/tf3/message_traits.h +++ /dev/null @@ -1,12 +0,0 @@ -// Minimal message_traits implementation for tf3 (no ROS dependency) -#ifndef TF3_MESSAGE_TRAITS_H -#define TF3_MESSAGE_TRAITS_H - -namespace tf3 { - namespace message_traits { - template - struct IsMessage { static const bool value = false; }; - } -} - -#endif // TF3_MESSAGE_TRAITS_H diff --git a/src/Libraries/tf3/include/tf3/time.h b/src/Libraries/tf3/include/tf3/time.h deleted file mode 100644 index 11652ab..0000000 --- a/src/Libraries/tf3/include/tf3/time.h +++ /dev/null @@ -1,152 +0,0 @@ -// Minimal tf3::Time and tf3::Duration (non-ROS) for standalone tf3 -#ifndef TF3_TIME_H -#define TF3_TIME_H - -#include -#include -#include -#include -#include - -namespace tf3 { - -class Duration; - -class Time { -public: - uint32_t sec; - uint32_t nsec; - - Time(): sec(0), nsec(0) {} - Time(uint32_t s, uint32_t ns): sec(s), nsec(ns) {} - - static Time now() - { - using namespace std::chrono; - auto now = system_clock::now().time_since_epoch(); - auto ns = duration_cast(now).count(); - Time t; - t.sec = static_cast(ns / 1000000000ULL); - t.nsec = static_cast(ns % 1000000000ULL); - return t; - } - - static Time fromSec(double s) - { - Time t; - t.sec = static_cast(std::floor(s)); - t.nsec = static_cast((s - t.sec) * 1e9); - return t; - } - - double toSec() const { return static_cast(sec) + static_cast(nsec) * 1e-9; } - bool isZero() const { return sec == 0 && nsec == 0; } - - bool operator==(const Time& o) const { return sec == o.sec && nsec == o.nsec; } - bool operator!=(const Time& o) const { return !(*this == o); } - bool operator<(const Time& o) const { return sec < o.sec || (sec == o.sec && nsec < o.nsec); } - bool operator>(const Time& o) const { return o < *this; } - bool operator<=(const Time& o) const { return !(*this > o); } - bool operator>=(const Time& o) const { return !(*this < o); } - - friend Duration operator-(const Time& a, const Time& b); - friend Time operator+(const Time& t, const Duration& d); - friend Time operator-(const Time& t, const Duration& d); - - static const Time& TIME_MAX() - { - static Time tm{std::numeric_limits::max(), std::numeric_limits::max()}; - return tm; - } -}; - -class Duration { -public: - int32_t sec; - int32_t nsec; - - Duration(): sec(0), nsec(0) {} - Duration(int32_t s, int32_t ns): sec(s), nsec(ns) { normalize(); } - explicit Duration(double seconds) - { - sec = static_cast(std::floor(seconds)); - nsec = static_cast((seconds - sec) * 1e9); - normalize(); - } - - static Duration fromSec(double s) { return Duration(s); } - static Duration fromNSec(int64_t ns_total) - { - int32_t s = static_cast(ns_total / 1000000000LL); - int32_t ns = static_cast(ns_total % 1000000000LL); - return Duration(s, ns); - } - - double toSec() const { return static_cast(sec) + static_cast(nsec) * 1e-9; } - - void normalize() - { - if (nsec >= 1000000000L) - { - sec += nsec / 1000000000L; - nsec = nsec % 1000000000L; - } - else if (nsec < 0) - { - int32_t borrow = (std::abs(nsec) / 1000000000L) + 1; - sec -= borrow; - nsec += borrow * 1000000000L; - } - } - - Duration operator+(const Duration& other) const { return Duration(sec + other.sec, nsec + other.nsec); } - Duration operator-(const Duration& other) const { return Duration(sec - other.sec, nsec - other.nsec); } - Duration& operator+=(const Duration& other) { sec += other.sec; nsec += other.nsec; normalize(); return *this; } - Duration& operator-=(const Duration& other) { sec -= other.sec; nsec -= other.nsec; normalize(); return *this; } - - bool operator==(const Duration& o) const { return sec == o.sec && nsec == o.nsec; } - bool operator!=(const Duration& o) const { return !(*this == o); } - bool operator<(const Duration& o) const { return sec < o.sec || (sec == o.sec && nsec < o.nsec); } - bool operator>(const Duration& o) const { return o < *this; } - bool operator<=(const Duration& o) const { return !(*this > o); } - bool operator>=(const Duration& o) const { return !(*this < o); } - - void sleep() const { if (sec < 0 || nsec < 0) return; auto ns_total = static_cast(sec) * 1000000000LL + nsec; std::this_thread::sleep_for(std::chrono::nanoseconds(ns_total)); } -}; - -inline Duration operator-(const Time& a, const Time& b) -{ - int32_t s = static_cast(a.sec) - static_cast(b.sec); - int32_t ns = static_cast(a.nsec) - static_cast(b.nsec); - Duration d(s, ns); - d.normalize(); - return d; -} - -inline Time operator+(const Time& t, const Duration& d) -{ - int64_t total_ns = static_cast(t.sec) * 1000000000LL + t.nsec + static_cast(d.sec) * 1000000000LL + d.nsec; - Time out; - out.sec = static_cast(total_ns / 1000000000LL); - out.nsec = static_cast(total_ns % 1000000000LL); - return out; -} - -inline Time operator-(const Time& t, const Duration& d) -{ - int64_t total_ns = static_cast(t.sec) * 1000000000LL + t.nsec - (static_cast(d.sec) * 1000000000LL + d.nsec); - if (total_ns < 0) total_ns = 0; - Time out; - out.sec = static_cast(total_ns / 1000000000LL); - out.nsec = static_cast(total_ns % 1000000000LL); - return out; -} - -// Provide tf3::TIME_MAX symbol -namespace { - static const Time TIME_MAX = Time::TIME_MAX(); -} - -} // namespace tf3 - -#endif // TF3_TIME_H diff --git a/src/Libraries/tf3/include/tf3/time_cache.h b/src/Libraries/tf3/include/tf3/time_cache.h deleted file mode 100644 index 30e4883..0000000 --- a/src/Libraries/tf3/include/tf3/time_cache.h +++ /dev/null @@ -1,155 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ - -/** \author Tully Foote */ - -#ifndef TF3_TIME_CACHE_H -#define TF3_TIME_CACHE_H - -#include "transform_storage.h" - -#include - -#include "tf3/time.h" - -#include - -namespace tf3 -{ - -typedef std::pair P_TimeAndFrameID; - -class TimeCacheInterface -{ -public: - /** \brief Access data from the cache */ - virtual bool getData(tf3::Time time, TransformStorage & data_out, std::string* error_str = 0)=0; //returns false if data unavailable (should be thrown as lookup exception - - /** \brief Insert data into the cache */ - virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0)=0; - - /** @brief Clear the list of stored values */ - virtual void clearList()=0; - - /** \brief Retrieve the parent at a specific time */ - virtual CompactFrameID getParent(tf3::Time time, std::string* error_str) = 0; - - /** - * \brief Get the latest time stored in this cache, and the parent associated with it. Returns parent = 0 if no data. - */ - virtual P_TimeAndFrameID getLatestTimeAndParent() = 0; - - - /// Debugging information methods - /** @brief Get the length of the stored list */ - virtual unsigned int getListLength()=0; - - /** @brief Get the latest timestamp cached */ - virtual tf3::Time getLatestTimestamp()=0; - - /** @brief Get the oldest timestamp cached */ - virtual tf3::Time getOldestTimestamp()=0; -}; - -typedef boost::shared_ptr TimeCacheInterfacePtr; - -/** \brief A class to keep a sorted linked list in time - * This builds and maintains a list of timestamped - * data. And provides lookup functions to get - * data out as a function of time. */ -class TimeCache : public TimeCacheInterface -{ - public: - static const int MIN_INTERPOLATION_DISTANCE = 5; //!< Number of nano-seconds to not interpolate below. - static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000; //!< Maximum length of linked list, to make sure not to be able to use unlimited memory. - static const int64_t DEFAULT_MAX_STORAGE_TIME = 10ULL * 1000000000LL; //!< default value of 10 seconds storage - - TimeCache(tf3::Duration max_storage_time = tf3::Duration::fromNSec(DEFAULT_MAX_STORAGE_TIME)); - - - /// Virtual methods - - virtual bool getData(tf3::Time time, TransformStorage & data_out, std::string* error_str = 0); - virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0); - virtual void clearList(); - virtual CompactFrameID getParent(tf3::Time time, std::string* error_str); - virtual P_TimeAndFrameID getLatestTimeAndParent(); - - /// Debugging information methods - virtual unsigned int getListLength(); - virtual tf3::Time getLatestTimestamp(); - virtual tf3::Time getOldestTimestamp(); - - -private: - typedef std::deque L_TransformStorage; - L_TransformStorage storage_; - - tf3::Duration max_storage_time_; - - - /// A helper function for getData - //Assumes storage is already locked for it - - inline uint8_t findClosest(TransformStorage*& one, TransformStorage*& two, tf3::Time target_time, std::string* error_str); - - inline void interpolate(const TransformStorage& one, const TransformStorage& two, tf3::Time time, TransformStorage& output); - - - void pruneList(); - - - -}; - -class StaticCache : public TimeCacheInterface -{ - public: - /// Virtual methods - - virtual bool getData(tf3::Time time, TransformStorage & data_out, std::string* error_str = 0); //returns false if data unavailable (should be thrown as lookup exception - virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0); - virtual void clearList(); - virtual CompactFrameID getParent(tf3::Time time, std::string* error_str); - virtual P_TimeAndFrameID getLatestTimeAndParent(); - - - /// Debugging information methods - virtual unsigned int getListLength(); - virtual tf3::Time getLatestTimestamp(); - virtual tf3::Time getOldestTimestamp(); - - -private: - TransformStorage storage_; -}; - -} - -#endif // TF3_TIME_CACHE_H diff --git a/src/Libraries/tf3/include/tf3/transform_datatypes.h b/src/Libraries/tf3/include/tf3/transform_datatypes.h deleted file mode 100644 index e760059..0000000 --- a/src/Libraries/tf3/include/tf3/transform_datatypes.h +++ /dev/null @@ -1,84 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ - -/** \author Tully Foote */ - -#ifndef TF3_TRANSFORM_DATATYPES_H -#define TF3_TRANSFORM_DATATYPES_H - -#include -#include "tf3/time.h" - -namespace tf3 -{ - -/** \brief The data type which will be cross compatable with geometry_msgs - * This is the tf3 datatype equivilant of a MessageStamped */ -template -class Stamped : public T{ - public: - tf3::Time stamp_; ///< The timestamp associated with this data - std::string frame_id_; ///< The frame_id associated this data - - /** Default constructor */ - Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocation - - /** Full constructor */ - Stamped(const T& input, const tf3::Time& timestamp, const std::string & frame_id) : - T (input), stamp_ ( timestamp ), frame_id_ (frame_id){ } ; - - /** Copy Constructor */ - Stamped(const Stamped& s): - T (s), - stamp_(s.stamp_), - frame_id_(s.frame_id_) {} - - /** Copy assignment operator */ - Stamped & operator=(const Stamped & rhs) - { - T::operator=(rhs); - stamp_ = rhs.stamp_; - frame_id_ = rhs.frame_id_; - return *this; - } - - - /** Set the data element */ - void setData(const T& input){*static_cast(this) = input;}; -}; - -/** \brief Comparison Operator for Stamped datatypes */ -template -bool operator==(const Stamped &a, const Stamped &b) { - return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && static_cast(a) == static_cast(b); -} - - -} -#endif //TF3_TRANSFORM_DATATYPES_H diff --git a/src/Libraries/tf3/include/tf3/transform_storage.h b/src/Libraries/tf3/include/tf3/transform_storage.h deleted file mode 100644 index f966386..0000000 --- a/src/Libraries/tf3/include/tf3/transform_storage.h +++ /dev/null @@ -1,80 +0,0 @@ -/* - * Copyright (c) 2010, Willow Garage, Inc. - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ - -/** \author Tully Foote */ - -#ifndef TF3_TRANSFORM_STORAGE_H -#define TF3_TRANSFORM_STORAGE_H - -#include -#include - - -#include "tf3/compat.h" - -namespace tf3 -{ - -typedef uint32_t CompactFrameID; - -/** \brief Storage for transforms and their parent */ -class TransformStorage -{ -public: - TransformStorage(); - TransformStorage(const tf3::TransformStampedMsg& data, CompactFrameID frame_id, CompactFrameID child_frame_id); - - TransformStorage(const TransformStorage& rhs) - { - *this = rhs; - } - - TransformStorage& operator=(const TransformStorage& rhs) - { -#if 01 - rotation_ = rhs.rotation_; - translation_ = rhs.translation_; - stamp_ = rhs.stamp_; - frame_id_ = rhs.frame_id_; - child_frame_id_ = rhs.child_frame_id_; -#endif - return *this; - } - - tf3::Quaternion rotation_; - tf3::Vector3 translation_; - tf3::Time stamp_; - CompactFrameID frame_id_; - CompactFrameID child_frame_id_; -}; - -} - -#endif // TF3_TRANSFORM_STORAGE_H - diff --git a/src/Libraries/tf3/include/tf3/utils.h b/src/Libraries/tf3/include/tf3/utils.h deleted file mode 100644 index 60bc46a..0000000 --- a/src/Libraries/tf3/include/tf3/utils.h +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. -// -// 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 TF3_UTILS_H -#define TF3_UTILS_H - -#include -#include -#include - -namespace tf3 { -/** Return the yaw, pitch, roll of anything that can be converted to a tf3::Quaternion - * The conventions are the usual ROS ones defined in tf3/LineMath/Matrix3x3.h - * \param a the object to get data from (it represents a rotation/quaternion) - * \param yaw yaw - * \param pitch pitch - * \param roll roll - */ -template - void getEulerYPR(const A& a, double& yaw, double& pitch, double& roll) - { - tf3::Quaternion q = impl::toQuaternion(a); - impl::getEulerYPR(q, yaw, pitch, roll); - } - -/** Return the yaw of anything that can be converted to a tf3::Quaternion - * The conventions are the usual ROS ones defined in tf3/LineMath/Matrix3x3.h - * This function is a specialization of getEulerYPR and is useful for its - * wide-spread use in navigation - * \param a the object to get data from (it represents a rotation/quaternion) - * \param yaw yaw - */ -template - double getYaw(const A& a) - { - tf3::Quaternion q = impl::toQuaternion(a); - return impl::getYaw(q); - } - -/** Return the identity for any type that can be converted to a tf3::Transform - * \return an object of class A that is an identity transform - */ -template - A getTransformIdentity() - { - tf3::Transform t; - t.setIdentity(); - A a; - convert(t, a); - return a; - } - -} - -#endif //TF3_UTILS_H diff --git a/src/Libraries/tf3/index.rst b/src/Libraries/tf3/index.rst deleted file mode 100644 index 5001a33..0000000 --- a/src/Libraries/tf3/index.rst +++ /dev/null @@ -1,15 +0,0 @@ -tf2 -===== - -This is the Python API reference of the tf2 package. - -.. toctree:: - :maxdepth: 2 - - tf2 - -Indices and tables -================== - -* :ref:`genindex` -* :ref:`search` diff --git a/src/Libraries/tf3/mainpage.dox b/src/Libraries/tf3/mainpage.dox deleted file mode 100644 index f253a9b..0000000 --- a/src/Libraries/tf3/mainpage.dox +++ /dev/null @@ -1,36 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b tf2 is the second generation of the tf library. - -This library implements the interface defined by tf2::BufferCore. - -There is also a Python wrapper with the same API that class this library using CPython bindings. - -\section codeapi Code API - -The main interface is through the tf2::BufferCore interface. - -It uses the exceptions in exceptions.h and the Stamped datatype -in transform_datatypes.h. - -\section conversions Conversion Interface - -tf2 offers a templated conversion interface for external libraries to specify conversions between -tf2-specific data types and user-defined data types. Various templated functions in tf2_ros use the -conversion interface to apply transformations from the tf server to these custom datatypes. - -The conversion interface is defined in tf2/convert.h. - -Some packages that implement this interface: - -- tf2_bullet -- tf2_eigen -- tf2_geometry_msgs -- tf2_kdl -- tf2_robot_sensor_msgs - -More documentation for the conversion interface is available on the ROS Wiki. - -*/ diff --git a/src/Libraries/tf3/package.xml b/src/Libraries/tf3/package.xml deleted file mode 100644 index 35f74df..0000000 --- a/src/Libraries/tf3/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - tf3 - 0.7.10 - - tf3 is the second generation of the transform library, which lets - the user keep track of multiple coordinate frames over time. tf3 - maintains the relationship between coordinate frames in a tree - structure buffered in time, and lets the user transform points, - vectors, etc between any two coordinate frames at any desired - point in time. - - Tully Foote - Eitan Marder-Eppstein - Wim Meeussen - Tully Foote - BSD - - http://www.ros.org/wiki/tf3 - - catkin - - console_bridge - console_bridge - - \ No newline at end of file diff --git a/src/Libraries/tf3/src/buffer_core.cpp b/src/Libraries/tf3/src/buffer_core.cpp deleted file mode 100644 index ff7ae1b..0000000 --- a/src/Libraries/tf3/src/buffer_core.cpp +++ /dev/null @@ -1,1645 +0,0 @@ -/* - * Copyright (c) 2010, Willow Garage, Inc. - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ - -/** \author Tully Foote */ - -#include "tf3/buffer_core.h" -#include "tf3/time_cache.h" -#include "tf3/exceptions.h" -#include "tf3/TF3Error.h" - -#include -#include -#include "tf3/LinearMath/Transform.h" - -namespace tf3 -{ - -// Tolerance for acceptable quaternion normalization -static double QUATERNION_NORMALIZATION_TOLERANCE = 10e-3; - -/** \brief convert Transform msg to Transform */ -void transformMsgToTF3(const tf3::TransformMsg& msg, tf3::Transform& tf3) -{ tf3 = tf3::Transform(tf3::Quaternion(msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w), tf3::Vector3(msg.translation.x, msg.translation.y, msg.translation.z)); } - -/** \brief convert Transform to Transform msg*/ -void transformTF3ToMsg(const tf3::Transform& tf3, tf3::TransformMsg& msg) -{ - msg.translation.x = tf3.getOrigin().x(); - msg.translation.y = tf3.getOrigin().y(); - msg.translation.z = tf3.getOrigin().z(); - msg.rotation.x = tf3.getRotation().x(); - msg.rotation.y = tf3.getRotation().y(); - msg.rotation.z = tf3.getRotation().z(); - msg.rotation.w = tf3.getRotation().w(); -} - -/** \brief convert Transform to Transform msg*/ -void transformTF3ToMsg(const tf3::Transform& tf3, tf3::TransformStampedMsg& msg, tf3::Time stamp, const std::string& frame_id, const std::string& child_frame_id) -{ - transformTF3ToMsg(tf3, msg.transform); - msg.header.stamp = stamp; - msg.header.frame_id = frame_id; - msg.child_frame_id = child_frame_id; -} - -void transformTF3ToMsg(const tf3::Quaternion& orient, const tf3::Vector3& pos, tf3::TransformMsg& msg) -{ - msg.translation.x = pos.x(); - msg.translation.y = pos.y(); - msg.translation.z = pos.z(); - msg.rotation.x = orient.x(); - msg.rotation.y = orient.y(); - msg.rotation.z = orient.z(); - msg.rotation.w = orient.w(); -} - -void transformTF3ToMsg(const tf3::Quaternion& orient, const tf3::Vector3& pos, tf3::TransformStampedMsg& msg, tf3::Time stamp, const std::string& frame_id, const std::string& child_frame_id) -{ - transformTF3ToMsg(orient, pos, msg.transform); - msg.header.stamp = stamp; - msg.header.frame_id = frame_id; - msg.child_frame_id = child_frame_id; -} - -void setIdentity(tf3::TransformMsg& tx) -{ - tx.translation.x = 0; - tx.translation.y = 0; - tx.translation.z = 0; - tx.rotation.x = 0; - tx.rotation.y = 0; - tx.rotation.z = 0; - tx.rotation.w = 1; -} - -bool startsWithSlash(const std::string& frame_id) -{ - if (frame_id.size() > 0) - if (frame_id[0] == '/') - return true; - return false; -} - -std::string stripSlash(const std::string& in) -{ - std::string out = in; - if (startsWithSlash(in)) - out.erase(0,1); - return out; -} - - -bool BufferCore::warnFrameId(const char* function_name_arg, const std::string& frame_id) const -{ - if (frame_id.size() == 0) - { - std::stringstream ss; - ss << "Invalid argument passed to "<< function_name_arg <<" in tf3 frame_ids cannot be empty"; - CONSOLE_BRIDGE_logWarn("%s",ss.str().c_str()); - return true; - } - - if (startsWithSlash(frame_id)) - { - std::stringstream ss; - ss << "Invalid argument \"" << frame_id << "\" passed to "<< function_name_arg <<" in tf3 frame_ids cannot start with a '/' like: "; - CONSOLE_BRIDGE_logWarn("%s",ss.str().c_str()); - return true; - } - - return false; -} - -CompactFrameID BufferCore::validateFrameId(const char* function_name_arg, const std::string& frame_id) const -{ - if (frame_id.empty()) - { - std::stringstream ss; - ss << "Invalid argument passed to "<< function_name_arg <<" in tf3 frame_ids cannot be empty"; - throw tf3::InvalidArgumentException(ss.str().c_str()); - } - - if (startsWithSlash(frame_id)) - { - std::stringstream ss; - ss << "Invalid argument \"" << frame_id << "\" passed to "<< function_name_arg <<" in tf3 frame_ids cannot start with a '/' like: "; - throw tf3::InvalidArgumentException(ss.str().c_str()); - } - - CompactFrameID id = lookupFrameNumber(frame_id); - if (id == 0) - { - std::stringstream ss; - ss << "\"" << frame_id << "\" passed to "<< function_name_arg <<" does not exist. "; - throw tf3::LookupException(ss.str().c_str()); - } - - return id; -} - -BufferCore::BufferCore(tf3::Duration cache_time) -: transformable_callbacks_counter_(0), - transformable_requests_counter_(0), - using_dedicated_thread_(false), - cache_time_(cache_time) -{ - frameIDs_["NO_PARENT"] = 0; - frames_.push_back(TimeCacheInterfacePtr()); - frameIDs_reverse.push_back("NO_PARENT"); -} - -BufferCore::~BufferCore() -{ - -} - -void BufferCore::clear() -{ - //old_tf_.clear(); - - - boost::mutex::scoped_lock lock(frame_mutex_); - if ( frames_.size() > 1 ) - { - for (std::vector::iterator cache_it = frames_.begin() + 1; cache_it != frames_.end(); ++cache_it) - { - if (*cache_it) - (*cache_it)->clearList(); - } - } - -} - -bool BufferCore::setTransform(const tf3::TransformStampedMsg& transform_in, const std::string& authority, bool is_static) -{ - - /////BACKEARDS COMPATABILITY - /* tf::StampedTransform tf_transform; - tf::transformStampedMsgToTF(transform_in, tf_transform); - if (!old_tf_.setTransform(tf_transform, authority)) - { - printf("Warning old setTransform Failed but was not caught\n"); - }*/ - - /////// New implementation - tf3::TransformStampedMsg stripped = transform_in; - stripped.header.frame_id = stripSlash(stripped.header.frame_id); - stripped.child_frame_id = stripSlash(stripped.child_frame_id); - - - bool error_exists = false; - if (stripped.child_frame_id == stripped.header.frame_id) - { - CONSOLE_BRIDGE_logError("TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with frame_id and child_frame_id \"%s\" because they are the same", authority.c_str(), stripped.child_frame_id.c_str()); - error_exists = true; - } - - if (stripped.child_frame_id == "") - { - CONSOLE_BRIDGE_logError("TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set ", authority.c_str()); - error_exists = true; - } - - if (stripped.header.frame_id == "") - { - CONSOLE_BRIDGE_logError("TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"%s\" from authority \"%s\" because frame_id not set", stripped.child_frame_id.c_str(), authority.c_str()); - error_exists = true; - } - - if (std::isnan(stripped.transform.translation.x) || std::isnan(stripped.transform.translation.y) || std::isnan(stripped.transform.translation.z)|| - std::isnan(stripped.transform.rotation.x) || std::isnan(stripped.transform.rotation.y) || std::isnan(stripped.transform.rotation.z) || std::isnan(stripped.transform.rotation.w)) - { - CONSOLE_BRIDGE_logError("TF_NAN_INPUT: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because of a nan value in the transform (%f %f %f) (%f %f %f %f)", - stripped.child_frame_id.c_str(), authority.c_str(), - stripped.transform.translation.x, stripped.transform.translation.y, stripped.transform.translation.z, - stripped.transform.rotation.x, stripped.transform.rotation.y, stripped.transform.rotation.z, stripped.transform.rotation.w - ); - error_exists = true; - } - - bool valid = std::abs((stripped.transform.rotation.w * stripped.transform.rotation.w - + stripped.transform.rotation.x * stripped.transform.rotation.x - + stripped.transform.rotation.y * stripped.transform.rotation.y - + stripped.transform.rotation.z * stripped.transform.rotation.z) - 1.0f) < QUATERNION_NORMALIZATION_TOLERANCE; - - if (!valid) - { - CONSOLE_BRIDGE_logError("TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because of an invalid quaternion in the transform (%f %f %f %f)", - stripped.child_frame_id.c_str(), authority.c_str(), - stripped.transform.rotation.x, stripped.transform.rotation.y, stripped.transform.rotation.z, stripped.transform.rotation.w); - error_exists = true; - } - - if (error_exists) - return false; - - { - boost::mutex::scoped_lock lock(frame_mutex_); - CompactFrameID frame_number = lookupOrInsertFrameNumber(stripped.child_frame_id); - TimeCacheInterfacePtr frame = getFrame(frame_number); - if (frame == NULL) - frame = allocateFrame(frame_number, is_static); - - std::string error_string; - if (frame->insertData(TransformStorage(stripped, lookupOrInsertFrameNumber(stripped.header.frame_id), frame_number), &error_string)) - { - frame_authority_[frame_number] = authority; - } - else - { - CONSOLE_BRIDGE_logWarn((error_string+" for frame %s (parent %s) at time %lf according to authority %s").c_str(), stripped.child_frame_id.c_str(), stripped.header.frame_id.c_str(), stripped.header.stamp.toSec(), authority.c_str()); - return false; - } - } - - testTransformableRequests(); - - return true; -} - -TimeCacheInterfacePtr BufferCore::allocateFrame(CompactFrameID cfid, bool is_static) -{ - TimeCacheInterfacePtr frame_ptr = frames_[cfid]; - if (is_static) { - frames_[cfid] = TimeCacheInterfacePtr(new StaticCache()); - } else { - frames_[cfid] = TimeCacheInterfacePtr(new TimeCache(cache_time_)); - } - - return frames_[cfid]; -} - -enum WalkEnding -{ - Identity, - TargetParentOfSource, - SourceParentOfTarget, - FullPath, -}; - -// TODO for Jade: Merge walkToTopParent functions; this is now a stub to preserve ABI -template -int BufferCore::walkToTopParent(F& f, tf3::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const -{ - return walkToTopParent(f, time, target_id, source_id, error_string, NULL); -} - -template -int BufferCore::walkToTopParent(F& f, tf3::Time time, CompactFrameID target_id, - CompactFrameID source_id, std::string* error_string, std::vector - *frame_chain) const -{ - if (frame_chain) - frame_chain->clear(); - - // Short circuit if zero length transform to allow lookups on non existant links - if (source_id == target_id) - { - f.finalize(Identity, time); - return tf3::TF3Error::NO_ERROR; - } - - //If getting the latest get the latest common time - if (time == tf3::Time()) - { - int retval = getLatestCommonTime(target_id, source_id, time, error_string); - if (retval != tf3::TF3Error::NO_ERROR) - { - return retval; - } - } - - // Walk the tree to its root from the source frame, accumulating the transform - CompactFrameID frame = source_id; - CompactFrameID top_parent = frame; - uint32_t depth = 0; - - std::string extrapolation_error_string; - bool extrapolation_might_have_occurred = false; - - while (frame != 0) - { - TimeCacheInterfacePtr cache = getFrame(frame); - if (frame_chain) - frame_chain->push_back(frame); - - if (!cache) - { - // There will be no cache for the very root of the tree - top_parent = frame; - break; - } - - CompactFrameID parent = f.gather(cache, time, error_string ? &extrapolation_error_string : NULL); - if (parent == 0) - { - // Just break out here... there may still be a path from source -> target - top_parent = frame; - extrapolation_might_have_occurred = true; - break; - } - - // Early out... target frame is a direct parent of the source frame - if (frame == target_id) - { - f.finalize(TargetParentOfSource, time); - return tf3::TF3Error::NO_ERROR; - } - - f.accum(true); - - top_parent = frame; - frame = parent; - - ++depth; - if (depth > MAX_GRAPH_DEPTH) - { - if (error_string) - { - std::stringstream ss; - ss << "The tf tree is invalid because it contains a loop." << std::endl - << allFramesAsStringNoLock() << std::endl; - *error_string = ss.str(); - } - return tf3::TF3Error::LOOKUP_ERROR; - } - } - - // Now walk to the top parent from the target frame, accumulating its transform - frame = target_id; - depth = 0; - std::vector reverse_frame_chain; - - while (frame != top_parent) - { - TimeCacheInterfacePtr cache = getFrame(frame); - if (frame_chain) - reverse_frame_chain.push_back(frame); - - if (!cache) - { - break; - } - - CompactFrameID parent = f.gather(cache, time, error_string); - if (parent == 0) - { - if (error_string) - { - // optimize performance by not using stringstream - char str[1000]; - snprintf(str, sizeof(str), "%s, when looking up transform from frame [%s] to frame [%s]", error_string->c_str(), lookupFrameString(source_id).c_str(), lookupFrameString(target_id).c_str()); - *error_string = str; - } - - return tf3::TF3Error::EXTRAPOLATION_ERROR; - } - - // Early out... source frame is a direct parent of the target frame - if (frame == source_id) - { - f.finalize(SourceParentOfTarget, time); - if (frame_chain) - { - // Use the walk we just did - frame_chain->swap(reverse_frame_chain); - // Reverse it before returning because this is the reverse walk. - std::reverse(frame_chain->begin(), frame_chain->end()); - } - return tf3::TF3Error::NO_ERROR; - } - - f.accum(false); - - frame = parent; - - ++depth; - if (depth > MAX_GRAPH_DEPTH) - { - if (error_string) - { - std::stringstream ss; - ss << "The tf tree is invalid because it contains a loop." << std::endl - << allFramesAsStringNoLock() << std::endl; - *error_string = ss.str(); - } - return tf3::TF3Error::LOOKUP_ERROR; - } - } - - if (frame != top_parent) - { - if (extrapolation_might_have_occurred) - { - if (error_string) - { - // optimize performance by not using stringstream - char str[1000]; - snprintf(str, sizeof(str), "%s, when looking up transform from frame [%s] to frame [%s]", extrapolation_error_string.c_str(), lookupFrameString(source_id).c_str(), lookupFrameString(target_id).c_str()); - *error_string = str; - } - - return tf3::TF3Error::EXTRAPOLATION_ERROR; - - } - - createConnectivityErrorString(source_id, target_id, error_string); - return tf3::TF3Error::CONNECTIVITY_ERROR; - } - else if (frame_chain){ - // append top_parent to reverse_frame_chain for easier matching/trimming - reverse_frame_chain.push_back(frame); - } - - f.finalize(FullPath, time); - if (frame_chain) - { - // Pruning: Compare the chains starting at the parent (end) until they differ - int m = reverse_frame_chain.size()-1; - int n = frame_chain->size()-1; - for (; m >= 0 && n >= 0; --m, --n) - { - if ((*frame_chain)[n] != reverse_frame_chain[m]) - { - break; - } - } - // Erase all duplicate items from frame_chain - if (n > 0) - { - // N is offset by 1 and leave the common parent for this result - frame_chain->erase(frame_chain->begin() + (n + 2), frame_chain->end()); - } - if (m < reverse_frame_chain.size()) - { - for (int i = m; i >= 0; --i) - { - frame_chain->push_back(reverse_frame_chain[i]); - } - } - } - - return tf3::TF3Error::NO_ERROR; -} - - - -struct TransformAccum -{ - TransformAccum() - : source_to_top_quat(0.0, 0.0, 0.0, 1.0) - , source_to_top_vec(0.0, 0.0, 0.0) - , target_to_top_quat(0.0, 0.0, 0.0, 1.0) - , target_to_top_vec(0.0, 0.0, 0.0) - , result_quat(0.0, 0.0, 0.0, 1.0) - , result_vec(0.0, 0.0, 0.0) - { - } - - CompactFrameID gather(TimeCacheInterfacePtr cache, tf3::Time time, std::string* error_string) - { - if (!cache->getData(time, st, error_string)) - { - return 0; - } - - return st.frame_id_; - } - - void accum(bool source) - { - if (source) - { - source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_; - source_to_top_quat = st.rotation_ * source_to_top_quat; - } - else - { - target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_; - target_to_top_quat = st.rotation_ * target_to_top_quat; - } - } - - void finalize(WalkEnding end, tf3::Time _time) - { - switch (end) - { - case Identity: - break; - case TargetParentOfSource: - result_vec = source_to_top_vec; - result_quat = source_to_top_quat; - break; - case SourceParentOfTarget: - { - tf3::Quaternion inv_target_quat = target_to_top_quat.inverse(); - tf3::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec); - result_vec = inv_target_vec; - result_quat = inv_target_quat; - break; - } - case FullPath: - { - tf3::Quaternion inv_target_quat = target_to_top_quat.inverse(); - tf3::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec); - - result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec; - result_quat = inv_target_quat * source_to_top_quat; - } - break; - }; - - time = _time; - } - - TransformStorage st; - tf3::Time time; - tf3::Quaternion source_to_top_quat; - tf3::Vector3 source_to_top_vec; - tf3::Quaternion target_to_top_quat; - tf3::Vector3 target_to_top_vec; - - tf3::Quaternion result_quat; - tf3::Vector3 result_vec; -}; - -tf3::TransformStampedMsg BufferCore::lookupTransform(const std::string& target_frame, - const std::string& source_frame, - const tf3::Time& time) const -{ - boost::mutex::scoped_lock lock(frame_mutex_); - - if (target_frame == source_frame) { - tf3::TransformStampedMsg identity; - identity.header.frame_id = target_frame; - identity.child_frame_id = source_frame; - identity.transform.rotation.w = 1; - - if (time == tf3::Time()) - { - CompactFrameID target_id = lookupFrameNumber(target_frame); - TimeCacheInterfacePtr cache = getFrame(target_id); - if (cache) - identity.header.stamp = cache->getLatestTimestamp(); - else - identity.header.stamp = time; - } - else - identity.header.stamp = time; - - return identity; - } - - //Identify case does not need to be validated above - CompactFrameID target_id = validateFrameId("lookupTransform argument target_frame", target_frame); - CompactFrameID source_id = validateFrameId("lookupTransform argument source_frame", source_frame); - - std::string error_string; - TransformAccum accum; - int retval = walkToTopParent(accum, time, target_id, source_id, &error_string); - if (retval != tf3::TF3Error::NO_ERROR) - { - switch (retval) - { - case tf3::TF3Error::CONNECTIVITY_ERROR: - throw ConnectivityException(error_string); - case tf3::TF3Error::EXTRAPOLATION_ERROR: - throw ExtrapolationException(error_string); - case tf3::TF3Error::LOOKUP_ERROR: - throw LookupException(error_string); - default: - CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); - assert(0); - } - } - - tf3::TransformStampedMsg output_transform; - transformTF3ToMsg(accum.result_quat, accum.result_vec, output_transform, accum.time, target_frame, source_frame); - return output_transform; -} - -tf3::TransformStampedMsg BufferCore::lookupTransform(const std::string& target_frame, - const tf3::Time& target_time, - const std::string& source_frame, - const tf3::Time& source_time, - const std::string& fixed_frame) const -{ - validateFrameId("lookupTransform argument target_frame", target_frame); - validateFrameId("lookupTransform argument source_frame", source_frame); - validateFrameId("lookupTransform argument fixed_frame", fixed_frame); - - tf3::TransformStampedMsg output; - tf3::TransformStampedMsg temp1 = lookupTransform(fixed_frame, source_frame, source_time); - tf3::TransformStampedMsg temp2 = lookupTransform(target_frame, fixed_frame, target_time); - - tf3::Transform tf1, tfb; - transformMsgToTF3(temp1.transform, tf1); - transformMsgToTF3(temp2.transform, tfb); - transformTF3ToMsg(tfb*tf1, output.transform); - output.header.stamp = temp2.header.stamp; - output.header.frame_id = target_frame; - output.child_frame_id = source_frame; - return output; -} - - - -/* -robot_geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, - const std::string& observation_frame, - const robot::Time& time, - const robot::Duration& averaging_interval) const -{ - try - { - robot_geometry_msgs::Twist t; - old_tf_.lookupTwist(tracking_frame, observation_frame, - time, averaging_interval, t); - return t; - } - catch (tf::LookupException& ex) - { - throw tf3::LookupException(ex.what()); - } - catch (tf::ConnectivityException& ex) - { - throw tf3::ConnectivityException(ex.what()); - } - catch (tf::ExtrapolationException& ex) - { - throw tf3::ExtrapolationException(ex.what()); - } - catch (tf::InvalidArgument& ex) - { - throw tf3::InvalidArgumentException(ex.what()); - } -} - -robot_geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, - const std::string& observation_frame, - const std::string& reference_frame, - const tf3::Point & reference_point, - const std::string& reference_point_frame, - const robot::Time& time, - const robot::Duration& averaging_interval) const -{ - try{ - robot_geometry_msgs::Twist t; - old_tf_.lookupTwist(tracking_frame, observation_frame, reference_frame, reference_point, reference_point_frame, - time, averaging_interval, t); - return t; - } - catch (tf::LookupException& ex) - { - throw tf3::LookupException(ex.what()); - } - catch (tf::ConnectivityException& ex) - { - throw tf3::ConnectivityException(ex.what()); - } - catch (tf::ExtrapolationException& ex) - { - throw tf3::ExtrapolationException(ex.what()); - } - catch (tf::InvalidArgument& ex) - { - throw tf3::InvalidArgumentException(ex.what()); - } -} -*/ - -struct CanTransformAccum -{ - CompactFrameID gather(TimeCacheInterfacePtr cache, tf3::Time time, std::string* error_string) - { - return cache->getParent(time, error_string); - } - - void accum(bool source) - { - } - - void finalize(WalkEnding end, tf3::Time _time) - { - } - - TransformStorage st; -}; - -bool BufferCore::canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id, - const tf3::Time& time, std::string* error_msg) const -{ - if (target_id == 0 || source_id == 0) - { - if (error_msg) - { - if (target_id == 0) - { - *error_msg += std::string("target_frame: " + lookupFrameString(target_id ) + " does not exist."); - } - if (source_id == 0) - { - if (target_id == 0) - { - *error_msg += std::string(" "); - } - *error_msg += std::string("source_frame: " + lookupFrameString(source_id) + " " + lookupFrameString(source_id ) + " does not exist."); - } - } - return false; - } - - if (target_id == source_id) - { - return true; - } - - CanTransformAccum accum; - if (walkToTopParent(accum, time, target_id, source_id, error_msg) == tf3::TF3Error::NO_ERROR) - { - return true; - } - - return false; -} - -bool BufferCore::canTransformInternal(CompactFrameID target_id, CompactFrameID source_id, - const tf3::Time& time, std::string* error_msg) const -{ - boost::mutex::scoped_lock lock(frame_mutex_); - return canTransformNoLock(target_id, source_id, time, error_msg); -} - -bool BufferCore::canTransform(const std::string& target_frame, const std::string& source_frame, - const tf3::Time& time, std::string* error_msg) const -{ - // Short circuit if target_frame == source_frame - if (target_frame == source_frame) - return true; - - if (warnFrameId("canTransform argument target_frame", target_frame)) - return false; - if (warnFrameId("canTransform argument source_frame", source_frame)) - return false; - - boost::mutex::scoped_lock lock(frame_mutex_); - - CompactFrameID target_id = lookupFrameNumber(target_frame); - CompactFrameID source_id = lookupFrameNumber(source_frame); - - if (target_id == 0 || source_id == 0) - { - if (error_msg) - { - if (target_id == 0) - { - *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist."); - } - if (source_id == 0) - { - if (target_id == 0) - { - *error_msg += std::string(" "); - } - *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist."); - } - } - return false; - } - return canTransformNoLock(target_id, source_id, time, error_msg); -} - -bool BufferCore::canTransform(const std::string& target_frame, const tf3::Time& target_time, - const std::string& source_frame, const tf3::Time& source_time, - const std::string& fixed_frame, std::string* error_msg) const -{ - if (warnFrameId("canTransform argument target_frame", target_frame)) - return false; - if (warnFrameId("canTransform argument source_frame", source_frame)) - return false; - if (warnFrameId("canTransform argument fixed_frame", fixed_frame)) - return false; - - boost::mutex::scoped_lock lock(frame_mutex_); - CompactFrameID target_id = lookupFrameNumber(target_frame); - CompactFrameID source_id = lookupFrameNumber(source_frame); - CompactFrameID fixed_id = lookupFrameNumber(fixed_frame); - - if (target_id == 0 || source_id == 0 || fixed_id == 0) - { - if (error_msg) - { - if (target_id == 0) - { - *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist."); - } - if (source_id == 0) - { - if (target_id == 0) - { - *error_msg += std::string(" "); - } - *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist."); - } - if (fixed_id == 0) - { - if (target_id == 0 || source_id == 0) - { - *error_msg += std::string(" "); - } - *error_msg += std::string("fixed_frame: " + fixed_frame + "does not exist."); - } - } - return false; - } - return canTransformNoLock(target_id, fixed_id, target_time, error_msg) && canTransformNoLock(fixed_id, source_id, source_time, error_msg); -} - - -tf3::TimeCacheInterfacePtr BufferCore::getFrame(CompactFrameID frame_id) const -{ - if (frame_id >= frames_.size()) - return TimeCacheInterfacePtr(); - else - { - return frames_[frame_id]; - } -} - -CompactFrameID BufferCore::lookupFrameNumber(const std::string& frameid_str) const -{ - CompactFrameID retval; - M_StringToCompactFrameID::const_iterator map_it = frameIDs_.find(frameid_str); - if (map_it == frameIDs_.end()) - { - retval = CompactFrameID(0); - } - else - retval = map_it->second; - return retval; -} - -CompactFrameID BufferCore::lookupOrInsertFrameNumber(const std::string& frameid_str) -{ - CompactFrameID retval = 0; - M_StringToCompactFrameID::iterator map_it = frameIDs_.find(frameid_str); - if (map_it == frameIDs_.end()) - { - retval = CompactFrameID(frames_.size()); - frames_.push_back(TimeCacheInterfacePtr());//Just a place holder for iteration - frameIDs_[frameid_str] = retval; - frameIDs_reverse.push_back(frameid_str); - } - else - retval = frameIDs_[frameid_str]; - - return retval; -} - -const std::string& BufferCore::lookupFrameString(CompactFrameID frame_id_num) const -{ - if (frame_id_num >= frameIDs_reverse.size()) - { - std::stringstream ss; - ss << "Reverse lookup of frame id " << frame_id_num << " failed!"; - throw tf3::LookupException(ss.str()); - } - else - return frameIDs_reverse[frame_id_num]; -} - -void BufferCore::createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const -{ - if (!out) - { - return; - } - *out = std::string("Could not find a connection between '"+lookupFrameString(target_frame)+"' and '"+ - lookupFrameString(source_frame)+"' because they are not part of the same tree."+ - "Tf has two or more unconnected trees."); -} - -std::string BufferCore::allFramesAsString() const -{ - boost::mutex::scoped_lock lock(frame_mutex_); - return this->allFramesAsStringNoLock(); -} - -std::string BufferCore::allFramesAsStringNoLock() const -{ - std::stringstream mstream; - - TransformStorage temp; - - // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it) - - ///regular transforms - for (unsigned int counter = 1; counter < frames_.size(); counter ++) - { - TimeCacheInterfacePtr frame_ptr = getFrame(CompactFrameID(counter)); - if (frame_ptr == NULL) - continue; - CompactFrameID frame_id_num; - if( frame_ptr->getData(tf3::Time(), temp)) - frame_id_num = temp.frame_id_; - else - { - frame_id_num = 0; - } - mstream << "Frame "<< frameIDs_reverse[counter] << " exists with parent " << frameIDs_reverse[frame_id_num] << "." <getLatestTimestamp(); - else - time = tf3::Time(); - return tf3::TF3Error::NO_ERROR; - } - - std::vector lct_cache; - - // Walk the tree to its root from the source frame, accumulating the list of parent/time as well as the latest time - // in the target is a direct parent - CompactFrameID frame = source_id; - P_TimeAndFrameID temp; - uint32_t depth = 0; - tf3::Time common_time = tf3::Time::TIME_MAX(); - while (frame != 0) - { - TimeCacheInterfacePtr cache = getFrame(frame); - - if (!cache) - { - // There will be no cache for the very root of the tree - break; - } - - P_TimeAndFrameID latest = cache->getLatestTimeAndParent(); - - if (latest.second == 0) - { - // Just break out here... there may still be a path from source -> target - break; - } - - if (!latest.first.isZero()) - { - common_time = std::min(latest.first, common_time); - } - - lct_cache.push_back(latest); - - frame = latest.second; - - // Early out... target frame is a direct parent of the source frame - if (frame == target_id) - { - time = common_time; - if (time == tf3::Time::TIME_MAX()) - { - time = tf3::Time(); - } - return tf3::TF3Error::NO_ERROR; - } - - ++depth; - if (depth > MAX_GRAPH_DEPTH) - { - if (error_string) - { - std::stringstream ss; - ss<<"The tf tree is invalid because it contains a loop." << std::endl - << allFramesAsStringNoLock() << std::endl; - *error_string = ss.str(); - } - return tf3::TF3Error::LOOKUP_ERROR; - } - } - - // Now walk to the top parent from the target frame, accumulating the latest time and looking for a common parent - frame = target_id; - depth = 0; - common_time = tf3::Time::TIME_MAX(); - CompactFrameID common_parent = 0; - while (true) - { - TimeCacheInterfacePtr cache = getFrame(frame); - - if (!cache) - { - break; - } - - P_TimeAndFrameID latest = cache->getLatestTimeAndParent(); - - if (latest.second == 0) - { - break; - } - - if (!latest.first.isZero()) - { - common_time = std::min(latest.first, common_time); - } - - std::vector::iterator it = std::find_if(lct_cache.begin(), lct_cache.end(), TimeAndFrameIDFrameComparator(latest.second)); - if (it != lct_cache.end()) // found a common parent - { - common_parent = it->second; - break; - } - - frame = latest.second; - - // Early out... source frame is a direct parent of the target frame - if (frame == source_id) - { - time = common_time; - if (time == tf3::Time::TIME_MAX()) - { - time = tf3::Time(); - } - return tf3::TF3Error::NO_ERROR; - } - - ++depth; - if (depth > MAX_GRAPH_DEPTH) - { - if (error_string) - { - std::stringstream ss; - ss<<"The tf tree is invalid because it contains a loop." << std::endl - << allFramesAsStringNoLock() << std::endl; - *error_string = ss.str(); - } - return tf3::TF3Error::LOOKUP_ERROR; - } - } - - if (common_parent == 0) - { - createConnectivityErrorString(source_id, target_id, error_string); - return tf3::TF3Error::CONNECTIVITY_ERROR; - } - - // Loop through the source -> root list until we hit the common parent - { - std::vector::iterator it = lct_cache.begin(); - std::vector::iterator end = lct_cache.end(); - for (; it != end; ++it) - { - if (!it->first.isZero()) - { - common_time = std::min(common_time, it->first); - } - - if (it->second == common_parent) - { - break; - } - } - } - - if (common_time == tf3::Time::TIME_MAX()) - { - common_time = tf3::Time(); - } - - time = common_time; - return tf3::TF3Error::NO_ERROR; -} - -std::string BufferCore::allFramesAsYAML(double current_time) const -{ - std::stringstream mstream; - boost::mutex::scoped_lock lock(frame_mutex_); - - TransformStorage temp; - - if (frames_.size() ==1) - mstream <<"{}"; - - mstream.precision(3); - mstream.setf(std::ios::fixed,std::ios::floatfield); - - // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it) - for (unsigned int counter = 1; counter < frames_.size(); counter ++)//one referenced for 0 is no frame - { - CompactFrameID cfid = CompactFrameID(counter); - CompactFrameID frame_id_num; - TimeCacheInterfacePtr cache = getFrame(cfid); - if (!cache) - { - continue; - } - - if(!cache->getData(tf3::Time(), temp)) - { - continue; - } - - frame_id_num = temp.frame_id_; - - std::string authority = "no recorded authority"; - std::map::const_iterator it = frame_authority_.find(cfid); - if (it != frame_authority_.end()) { - authority = it->second; - } - - double rate = cache->getListLength() / std::max((cache->getLatestTimestamp().toSec() - - cache->getOldestTimestamp().toSec() ), 0.0001); - - mstream << std::fixed; //fixed point notation - mstream.precision(3); //3 decimal places - mstream << frameIDs_reverse[cfid] << ": " << std::endl; - mstream << " parent: '" << frameIDs_reverse[frame_id_num] << "'" << std::endl; - mstream << " broadcaster: '" << authority << "'" << std::endl; - mstream << " rate: " << rate << std::endl; - mstream << " most_recent_transform: " << (cache->getLatestTimestamp()).toSec() << std::endl; - mstream << " oldest_transform: " << (cache->getOldestTimestamp()).toSec() << std::endl; - if ( current_time > 0 ) { - mstream << " transform_delay: " << current_time - cache->getLatestTimestamp().toSec() << std::endl; - } - mstream << " buffer_length: " << (cache->getLatestTimestamp() - cache->getOldestTimestamp()).toSec() << std::endl; - } - - return mstream.str(); -} - -std::string BufferCore::allFramesAsYAML() const -{ - return this->allFramesAsYAML(0.0); -} - -TransformableCallbackHandle BufferCore::addTransformableCallback(const TransformableCallback& cb) -{ - boost::mutex::scoped_lock lock(transformable_callbacks_mutex_); - TransformableCallbackHandle handle = ++transformable_callbacks_counter_; - while (!transformable_callbacks_.insert(std::make_pair(handle, cb)).second) - { - handle = ++transformable_callbacks_counter_; - } - - return handle; -} - -struct BufferCore::RemoveRequestByCallback -{ - RemoveRequestByCallback(TransformableCallbackHandle handle) - : handle_(handle) - {} - - bool operator()(const TransformableRequest& req) - { - return req.cb_handle == handle_; - } - - TransformableCallbackHandle handle_; -}; - -void BufferCore::removeTransformableCallback(TransformableCallbackHandle handle) -{ - { - boost::mutex::scoped_lock lock(transformable_callbacks_mutex_); - transformable_callbacks_.erase(handle); - } - - { - boost::mutex::scoped_lock lock(transformable_requests_mutex_); - auto it = std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByCallback(handle)); - if (it != transformable_requests_.end()) - { - transformable_requests_.erase(it, transformable_requests_.end()); - } - } -} - -TransformableRequestHandle BufferCore::addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, tf3::Time time) -{ - // shortcut if target == source - if (target_frame == source_frame) - { - return 0; - } - - TransformableRequest req; - req.target_id = lookupFrameNumber(target_frame); - req.source_id = lookupFrameNumber(source_frame); - - // First check if the request is already transformable. If it is, return immediately - if (canTransformInternal(req.target_id, req.source_id, time, 0)) - { - return 0; - } - - // Might not be transformable at all, ever (if it's too far in the past) - if (req.target_id && req.source_id) - { - tf3::Time latest_time; - // TODO: This is incorrect, but better than nothing. Really we want the latest time for - // any of the frames - getLatestCommonTime(req.target_id, req.source_id, latest_time, 0); - if (!latest_time.isZero() && time + cache_time_ < latest_time) - { - return 0xffffffffffffffffULL; - } - } - - req.cb_handle = handle; - req.time = time; - req.request_handle = ++transformable_requests_counter_; - if (req.request_handle == 0 || req.request_handle == 0xffffffffffffffffULL) - { - req.request_handle = 1; - } - - if (req.target_id == 0) - { - req.target_string = target_frame; - } - - if (req.source_id == 0) - { - req.source_string = source_frame; - } - - boost::mutex::scoped_lock lock(transformable_requests_mutex_); - transformable_requests_.push_back(req); - - return req.request_handle; -} - -struct BufferCore::RemoveRequestByID -{ - RemoveRequestByID(TransformableRequestHandle handle) - : handle_(handle) - {} - - bool operator()(const TransformableRequest& req) - { - return req.request_handle == handle_; - } - - TransformableCallbackHandle handle_; -}; - -void BufferCore::cancelTransformableRequest(TransformableRequestHandle handle) -{ - boost::mutex::scoped_lock lock(transformable_requests_mutex_); - auto it = std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByID(handle)); - if (it != transformable_requests_.end()) - { - transformable_requests_.erase(it, transformable_requests_.end()); - } -} - - - -// backwards compability for tf methods -boost::signals2::connection BufferCore::_addTransformsChangedListener(boost::function callback) -{ - boost::mutex::scoped_lock lock(transformable_requests_mutex_); - return _transforms_changed_.connect(callback); -} - -void BufferCore::_removeTransformsChangedListener(boost::signals2::connection c) -{ - boost::mutex::scoped_lock lock(transformable_requests_mutex_); - c.disconnect(); -} - - -bool BufferCore::_frameExists(const std::string& frame_id_str) const -{ - boost::mutex::scoped_lock lock(frame_mutex_); - return frameIDs_.count(frame_id_str); -} - -bool BufferCore::_getParent(const std::string& frame_id, tf3::Time time, std::string& parent) const -{ - - boost::mutex::scoped_lock lock(frame_mutex_); - CompactFrameID frame_number = lookupFrameNumber(frame_id); - TimeCacheInterfacePtr frame = getFrame(frame_number); - - if (! frame) - return false; - - CompactFrameID parent_id = frame->getParent(time, NULL); - if (parent_id == 0) - return false; - - parent = lookupFrameString(parent_id); - return true; -}; - -void BufferCore::_getFrameStrings(std::vector & vec) const -{ - vec.clear(); - - boost::mutex::scoped_lock lock(frame_mutex_); - - TransformStorage temp; - - // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it) - for (unsigned int counter = 1; counter < frameIDs_reverse.size(); counter ++) - { - vec.push_back(frameIDs_reverse[counter]); - } - return; -} - - - - -void BufferCore::testTransformableRequests() -{ - boost::mutex::scoped_lock lock(transformable_requests_mutex_); - V_TransformableRequest::iterator it = transformable_requests_.begin(); - for (; it != transformable_requests_.end();) - { - TransformableRequest& req = *it; - - // One or both of the frames may not have existed when the request was originally made. - if (req.target_id == 0) - { - req.target_id = lookupFrameNumber(req.target_string); - } - - if (req.source_id == 0) - { - req.source_id = lookupFrameNumber(req.source_string); - } - - tf3::Time latest_time; - bool do_cb = false; - TransformableResult result = TransformAvailable; - // TODO: This is incorrect, but better than nothing. Really we want the latest time for - // any of the frames - getLatestCommonTime(req.target_id, req.source_id, latest_time, 0); - if (!latest_time.isZero() && req.time + cache_time_ < latest_time) - { - do_cb = true; - result = TransformFailure; - } - else if (canTransformInternal(req.target_id, req.source_id, req.time, 0)) - { - do_cb = true; - result = TransformAvailable; - } - - if (do_cb) - { - { - boost::mutex::scoped_lock lock2(transformable_callbacks_mutex_); - M_TransformableCallback::iterator it = transformable_callbacks_.find(req.cb_handle); - if (it != transformable_callbacks_.end()) - { - const TransformableCallback& cb = it->second; - cb(req.request_handle, lookupFrameString(req.target_id), lookupFrameString(req.source_id), req.time, result); - } - } - - if (transformable_requests_.size() > 1) - { - transformable_requests_[it - transformable_requests_.begin()] = transformable_requests_.back(); - } - - transformable_requests_.erase(transformable_requests_.end() - 1); - } - else - { - ++it; - } - } - - // unlock before allowing possible user callbacks to avoid potential deadlock (#91) - lock.unlock(); - - // Backwards compatability callback for tf - _transforms_changed_(); -} - - -std::string BufferCore::_allFramesAsDot(double current_time) const -{ - std::stringstream mstream; - mstream << "digraph G {" << std::endl; - boost::mutex::scoped_lock lock(frame_mutex_); - - TransformStorage temp; - - if (frames_.size() == 1) { - mstream <<"\"no tf data recieved\""; - } - mstream.precision(3); - mstream.setf(std::ios::fixed,std::ios::floatfield); - - for (unsigned int counter = 1; counter < frames_.size(); counter ++) // one referenced for 0 is no frame - { - unsigned int frame_id_num; - TimeCacheInterfacePtr counter_frame = getFrame(counter); - if (!counter_frame) { - continue; - } - if(!counter_frame->getData(tf3::Time(), temp)) { - continue; - } else { - frame_id_num = temp.frame_id_; - } - std::string authority = "no recorded authority"; - std::map::const_iterator it = frame_authority_.find(counter); - if (it != frame_authority_.end()) - authority = it->second; - - double rate = counter_frame->getListLength() / std::max((counter_frame->getLatestTimestamp().toSec() - - counter_frame->getOldestTimestamp().toSec()), 0.0001); - - mstream << std::fixed; //fixed point notation - mstream.precision(3); //3 decimal places - mstream << "\"" << frameIDs_reverse[frame_id_num] << "\"" << " -> " - << "\"" << frameIDs_reverse[counter] << "\"" << "[label=\"" - //<< "Time: " << current_time.toSec() << "\\n" - << "Broadcaster: " << authority << "\\n" - << "Average rate: " << rate << " Hz\\n" - << "Most recent transform: " << (counter_frame->getLatestTimestamp()).toSec() <<" "; - if (current_time > 0) - mstream << "( "<< current_time - counter_frame->getLatestTimestamp().toSec() << " sec old)"; - mstream << "\\n" - // << "(time: " << getFrame(counter)->getLatestTimestamp().toSec() << ")\\n" - // << "Oldest transform: " << (current_time - getFrame(counter)->getOldestTimestamp()).toSec() << " sec old \\n" - // << "(time: " << (getFrame(counter)->getOldestTimestamp()).toSec() << ")\\n" - << "Buffer length: " << (counter_frame->getLatestTimestamp()-counter_frame->getOldestTimestamp()).toSec() << " sec\\n" - <<"\"];" < 0) { - mstream << "edge [style=invis];" <" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl; - } - continue; - } - if (counter_frame->getData(tf3::Time(), temp)) { - frame_id_num = temp.frame_id_; - } else { - frame_id_num = 0; - } - - if(frameIDs_reverse[frame_id_num]=="NO_PARENT") - { - mstream << "edge [style=invis];" < 0) - mstream << "\"Recorded at time: " << current_time << "\"[ shape=plaintext ] ;\n "; - mstream << "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl; - } - } - mstream << "}"; - return mstream.str(); -} - -std::string BufferCore::_allFramesAsDot() const -{ - return _allFramesAsDot(0.0); -} - -void BufferCore::_chainAsVector(const std::string & target_frame, tf3::Time target_time, const std::string & source_frame, tf3::Time source_time, const std::string& fixed_frame, std::vector& output) const -{ - std::string error_string; - - output.clear(); //empty vector - - std::stringstream mstream; - boost::mutex::scoped_lock lock(frame_mutex_); - - TransformAccum accum; - - // Get source frame/time using getFrame - CompactFrameID source_id = lookupFrameNumber(source_frame); - CompactFrameID fixed_id = lookupFrameNumber(fixed_frame); - CompactFrameID target_id = lookupFrameNumber(target_frame); - - std::vector source_frame_chain; - int retval = walkToTopParent(accum, source_time, fixed_id, source_id, &error_string, &source_frame_chain); - - if (retval != tf3::TF3Error::NO_ERROR) - { - switch (retval) - { - case tf3::TF3Error::CONNECTIVITY_ERROR: - throw ConnectivityException(error_string); - case tf3::TF3Error::EXTRAPOLATION_ERROR: - throw ExtrapolationException(error_string); - case tf3::TF3Error::LOOKUP_ERROR: - throw LookupException(error_string); - default: - CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); - assert(0); - } - } - - std::vector target_frame_chain; - retval = walkToTopParent(accum, target_time, target_id, fixed_id, &error_string, &target_frame_chain); - - if (retval != tf3::TF3Error::NO_ERROR) - { - switch (retval) - { - case tf3::TF3Error::CONNECTIVITY_ERROR: - throw ConnectivityException(error_string); - case tf3::TF3Error::EXTRAPOLATION_ERROR: - throw ExtrapolationException(error_string); - case tf3::TF3Error::LOOKUP_ERROR: - throw LookupException(error_string); - default: - CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); - assert(0); - } - } - // If the two chains overlap clear the overlap - if (source_frame_chain.size() > 0 && target_frame_chain.size() > 0 && - source_frame_chain.back() == target_frame_chain.front()) - { - source_frame_chain.pop_back(); - } - // Join the two walks - for (unsigned int i = 0; i < target_frame_chain.size(); ++i) - { - source_frame_chain.push_back(target_frame_chain[i]); - } - - - // Write each element of source_frame_chain as string - for (unsigned int i = 0; i < source_frame_chain.size(); ++i) - { - output.push_back(lookupFrameString(source_frame_chain[i])); - } -} - -int TestBufferCore::_walkToTopParent(BufferCore& buffer, tf3::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector *frame_chain) const -{ - TransformAccum accum; - return buffer.walkToTopParent(accum, time, target_id, source_id, error_string, frame_chain); -} - -} // namespace tf3 diff --git a/src/Libraries/tf3/src/cache.cpp b/src/Libraries/tf3/src/cache.cpp deleted file mode 100644 index 6cb778a..0000000 --- a/src/Libraries/tf3/src/cache.cpp +++ /dev/null @@ -1,338 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ - -/** \author Tully Foote */ - -#include "tf3/time_cache.h" -#include "tf3/exceptions.h" - -#include -#include -#include -#include "tf3/compat.h" -#include - -namespace tf3 { - -TransformStorage::TransformStorage() -{ -} - -TransformStorage::TransformStorage(const tf3::TransformStampedMsg& data, CompactFrameID frame_id, - CompactFrameID child_frame_id) -: stamp_(data.header.stamp) -, frame_id_(frame_id) -, child_frame_id_(child_frame_id) -{ - const tf3::QuaternionMsg& o = data.transform.rotation; - rotation_ = tf3::Quaternion(o.x, o.y, o.z, o.w); - const tf3::Vector3Msg& v = data.transform.translation; - translation_ = tf3::Vector3(v.x, v.y, v.z); -} - -TimeCache::TimeCache(tf3::Duration max_storage_time) -: max_storage_time_(max_storage_time) -{} - -namespace cache { // Avoid ODR collisions https://github.com/ros/geometry2/issues/175 -// hoisting these into separate functions causes an ~8% speedup. Removing calling them altogether adds another ~10% -void createExtrapolationException1(tf3::Time t0, tf3::Time t1, std::string* error_str) -{ - if (error_str) - { - char str[117]; // Text without formatting strings has 77, each timestamp has up to 20 - snprintf(str, sizeof(str), "Lookup would require extrapolation at time %.09f, but only time %.09f is in the buffer", t0.toSec(), t1.toSec()); - *error_str = str; - } -} - -void createExtrapolationException2(tf3::Time t0, tf3::Time t1, std::string* error_str) -{ - if (error_str) - { - // Want this to come out positive, because this is a future extrapolation problem with t0 - // t0 needs to come first because it will be bigger than t1 - tf3::Duration tdiff = t0 - t1; - char str[163]; // Text without formatting strings has 102, each timestamp has up to 20 - snprintf( - str, sizeof(str), - "Lookup would require extrapolation %.09fs into the future. Requested time %.09f but the latest data is at time %.09f", - tdiff.toSec(), t0.toSec(), t1.toSec()); - *error_str = str; - } -} - -void createExtrapolationException3(tf3::Time t0, tf3::Time t1, std::string* error_str) -{ - if (error_str) - { - tf3::Duration tdiff = t1 - t0; - char str[163]; // Text without formatting strings has 102, each timestamp has up to 20 - snprintf( - str, sizeof(str), - "Lookup would require extrapolation %.09fs into the past. Requested time %.09f but the earliest data is at time %.09f", - tdiff.toSec(), t0.toSec(), t1.toSec()); - *error_str = str; - } -} -} // namespace cache - -bool operator>(const TransformStorage& lhs, const TransformStorage& rhs) -{ - return lhs.stamp_ > rhs.stamp_; -} - -uint8_t TimeCache::findClosest(TransformStorage*& one, TransformStorage*& two, tf3::Time target_time, std::string* error_err) -{ - //No values stored - if (storage_.empty()) - { - return 0; - } - - //If time == 0 return the latest - if (target_time.isZero()) - { - one = &storage_.front(); - return 1; - } - - // One value stored - if (++storage_.begin() == storage_.end()) - { - TransformStorage& ts = *storage_.begin(); - if (ts.stamp_ == target_time) - { - one = &ts; - return 1; - } - else - { - cache::createExtrapolationException1(target_time, ts.stamp_, error_err); - return 0; - } - } - - tf3::Time latest_time = (*storage_.begin()).stamp_; - tf3::Time earliest_time = (*(storage_.rbegin())).stamp_; - - if (target_time == latest_time) - { - one = &(*storage_.begin()); - return 1; - } - else if (target_time == earliest_time) - { - one = &(*storage_.rbegin()); - return 1; - } - // Catch cases that would require extrapolation - else if (target_time > latest_time) - { - cache::createExtrapolationException2(target_time, latest_time, error_err); - return 0; - } - else if (target_time < earliest_time) - { - cache::createExtrapolationException3(target_time, earliest_time, error_err); - return 0; - } - - //At least 2 values stored - //Find the first value less than the target value - L_TransformStorage::iterator storage_it; - TransformStorage storage_target_time; - storage_target_time.stamp_ = target_time; - - storage_it = std::lower_bound( - storage_.begin(), - storage_.end(), - storage_target_time, std::greater()); - - //Finally the case were somewhere in the middle Guarenteed no extrapolation :-) - one = &*(storage_it); //Older - two = &*(--storage_it); //Newer - return 2; - - -} - -void TimeCache::interpolate(const TransformStorage& one, const TransformStorage& two, tf3::Time time, TransformStorage& output) -{ - // Check for zero distance case - if( two.stamp_ == one.stamp_ ) - { - output = two; - return; - } - //Calculate the ratio - tf3Scalar ratio = (time - one.stamp_).toSec() / (two.stamp_ - one.stamp_).toSec(); - - //Interpolate translation - output.translation_.setInterpolate3(one.translation_, two.translation_, ratio); - - //Interpolate rotation - output.rotation_ = slerp( one.rotation_, two.rotation_, ratio); - - output.stamp_ = time; - output.frame_id_ = one.frame_id_; - output.child_frame_id_ = one.child_frame_id_; -} - -bool TimeCache::getData(tf3::Time time, TransformStorage & data_out, std::string* error_err) //returns false if data not available -{ - TransformStorage* p_temp_1; - TransformStorage* p_temp_2; - - int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_err); - if (num_nodes == 0) - { - return false; - } - else if (num_nodes == 1) - { - data_out = *p_temp_1; - } - else if (num_nodes == 2) - { - if( p_temp_1->frame_id_ == p_temp_2->frame_id_) - { - interpolate(*p_temp_1, *p_temp_2, time, data_out); - } - else - { - data_out = *p_temp_1; - } - } - else - { - assert(0); - } - - return true; -} - -CompactFrameID TimeCache::getParent(tf3::Time time, std::string* error_err) -{ - TransformStorage* p_temp_1; - TransformStorage* p_temp_2; - - int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_err); - if (num_nodes == 0) - { - return 0; - } - - return p_temp_1->frame_id_; -} - -bool TimeCache::insertData(const TransformStorage& new_data, std::string* error_str) -{ - L_TransformStorage::iterator storage_it = storage_.begin(); - - if(storage_it != storage_.end()) - { - if (storage_it->stamp_ > new_data.stamp_ + max_storage_time_) - { - if (error_str) - { - *error_str = "TF_OLD_DATA ignoring data from the past (Possible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained)"; - } - return false; - } - } - - - while(storage_it != storage_.end()) - { - if (storage_it->stamp_ <= new_data.stamp_) - break; - storage_it++; - } - if (storage_it != storage_.end() && storage_it->stamp_ == new_data.stamp_) - { - if (error_str) - { - *error_str = "TF_REPEATED_DATA ignoring data with redundant timestamp"; - } - return false; - } - else - { - storage_.insert(storage_it, new_data); - } - - pruneList(); - return true; -} - -void TimeCache::clearList() -{ - storage_.clear(); -} - -unsigned int TimeCache::getListLength() -{ - return storage_.size(); -} - -P_TimeAndFrameID TimeCache::getLatestTimeAndParent() -{ - if (storage_.empty()) - { - return std::make_pair(tf3::Time(), 0); - } - - const TransformStorage& ts = storage_.front(); - return std::make_pair(ts.stamp_, ts.frame_id_); -} - -tf3::Time TimeCache::getLatestTimestamp() -{ - if (storage_.empty()) return tf3::Time(); //empty list case - return storage_.front().stamp_; -} - -tf3::Time TimeCache::getOldestTimestamp() -{ - if (storage_.empty()) return tf3::Time(); //empty list case - return storage_.back().stamp_; -} - -void TimeCache::pruneList() -{ - tf3::Time latest_time = storage_.begin()->stamp_; - - while(!storage_.empty() && storage_.back().stamp_ + max_storage_time_ < latest_time) - { - storage_.pop_back(); - } - -} // namespace tf3 -} diff --git a/src/Libraries/tf3/src/static_cache.cpp b/src/Libraries/tf3/src/static_cache.cpp deleted file mode 100644 index b7e2e85..0000000 --- a/src/Libraries/tf3/src/static_cache.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ - -/** \author Tully Foote */ - -#include "tf3/time_cache.h" -#include "tf3/exceptions.h" - -#include "tf3/LinearMath/Transform.h" -#include "tf3/compat.h" - - -using namespace tf3; - - -bool StaticCache::getData(tf3::Time time, TransformStorage & data_out, std::string* error_err) //returns false if data not available -{ - data_out = storage_; - data_out.stamp_ = time; - return true; -}; - -bool StaticCache::insertData(const TransformStorage& new_data, std::string* error_str) -{ - storage_ = new_data; - return true; -}; - - - - -void StaticCache::clearList() { return; }; - -unsigned int StaticCache::getListLength() { return 1; }; - -CompactFrameID StaticCache::getParent(tf3::Time time, std::string* error_err) -{ - return storage_.frame_id_; -} - -P_TimeAndFrameID StaticCache::getLatestTimeAndParent() -{ - return std::make_pair(tf3::Time(), storage_.frame_id_); -} - -tf3::Time StaticCache::getLatestTimestamp() -{ - return tf3::Time(); -}; - -tf3::Time StaticCache::getOldestTimestamp() -{ - return tf3::Time(); -}; - diff --git a/src/Libraries/tf3/test/cache_unittest.cpp b/src/Libraries/tf3/test/cache_unittest.cpp deleted file mode 100644 index c3891b2..0000000 --- a/src/Libraries/tf3/test/cache_unittest.cpp +++ /dev/null @@ -1,414 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ - -#include -#include -#include "tf3/LinearMath/Quaternion.h" -#include - -#include - -#include - -std::vector values; -unsigned int step = 0; - -void seed_rand() -{ - values.clear(); - for (unsigned int i = 0; i < 1000; i++) - { - int pseudo_rand = std::floor(i * M_PI); - values.push_back(( pseudo_rand % 100)/50.0 - 1.0); - //printf("Seeding with %f\n", values.back()); - } -}; - - -double get_rand() -{ - if (values.size() == 0) throw std::runtime_error("you need to call seed_rand first"); - if (step >= values.size()) - step = 0; - else - step++; - return values[step]; -} - -using namespace tf3; - - -void setIdentity(TransformStorage& stor) -{ - stor.translation_.setValue(0.0, 0.0, 0.0); - stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0); -} - -TEST(TimeCache, Repeatability) -{ - unsigned int runs = 100; - - tf3::TimeCache cache; - - TransformStorage stor; - setIdentity(stor); - - for ( uint64_t i = 1; i < runs ; i++ ) - { - stor.frame_id_ = i; - stor.stamp_ = robot::Time().fromNSec(i); - - cache.insertData(stor); - } - - for ( uint64_t i = 1; i < runs ; i++ ) - - { - cache.getData(robot::Time().fromNSec(i), stor); - EXPECT_EQ(stor.frame_id_, i); - EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i)); - } - -} - -TEST(TimeCache, RepeatabilityReverseInsertOrder) -{ - unsigned int runs = 100; - - tf3::TimeCache cache; - - TransformStorage stor; - setIdentity(stor); - - for ( int i = runs -1; i >= 0 ; i-- ) - { - stor.frame_id_ = i; - stor.stamp_ = robot::Time().fromNSec(i); - - cache.insertData(stor); - } - for ( uint64_t i = 1; i < runs ; i++ ) - - { - cache.getData(robot::Time().fromNSec(i), stor); - EXPECT_EQ(stor.frame_id_, i); - EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i)); - } - -} - -#if 0 // jfaust: this doesn't seem to actually be testing random insertion? -TEST(TimeCache, RepeatabilityRandomInsertOrder) -{ - - seed_rand(); - - tf3::TimeCache cache; - double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8}; - std::vector values (my_vals, my_vals + sizeof(my_vals)/sizeof(double)); - unsigned int runs = values.size(); - - TransformStorage stor; - setIdentity(stor); - for ( uint64_t i = 0; i xvalues(2); - std::vector yvalues(2); - std::vector zvalues(2); - - uint64_t offset = 200; - - TransformStorage stor; - setIdentity(stor); - - for ( uint64_t i = 1; i < runs ; i++ ) - { - - for (uint64_t step = 0; step < 2 ; step++) - { - xvalues[step] = 10.0 * get_rand(); - yvalues[step] = 10.0 * get_rand(); - zvalues[step] = 10.0 * get_rand(); - - stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]); - stor.frame_id_ = 2; - stor.stamp_ = robot::Time().fromNSec(step * 100 + offset); - cache.insertData(stor); - } - - for (int pos = 0; pos < 100 ; pos ++) - { - uint64_t time = offset + pos; - cache.getData(robot::Time().fromNSec(time), stor); - uint64_t time_out = stor.stamp_.toNSec(); - double x_out = stor.translation_.x(); - double y_out = stor.translation_.y(); - double z_out = stor.translation_.z(); - // printf("pose %d, %f %f %f, expected %f %f %f\n", pos, x_out, y_out, z_out, - // xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100., - // yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, - // zvalues[0] + (xvalues[1] - zvalues[0]) * (double)pos/100.0); - EXPECT_EQ(time, time_out); - EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon); - EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon); - EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon); - } - - - cache.clearList(); - } - - -} - -/** \brief Make sure we dont' interpolate across reparented data */ -TEST(TimeCache, ReparentingInterpolationProtection) -{ - double epsilon = 1e-6; - uint64_t offset = 555; - - seed_rand(); - - tf3::TimeCache cache; - std::vector xvalues(2); - std::vector yvalues(2); - std::vector zvalues(2); - - TransformStorage stor; - setIdentity(stor); - - for (uint64_t step = 0; step < 2 ; step++) - { - xvalues[step] = 10.0 * get_rand(); - yvalues[step] = 10.0 * get_rand(); - zvalues[step] = 10.0 * get_rand(); - - stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]); - stor.frame_id_ = step + 4; - stor.stamp_ = robot::Time().fromNSec(step * 100 + offset); - cache.insertData(stor); - } - - for (int pos = 0; pos < 100 ; pos ++) - { - EXPECT_TRUE(cache.getData(robot::Time().fromNSec(offset + pos), stor)); - double x_out = stor.translation_.x(); - double y_out = stor.translation_.y(); - double z_out = stor.translation_.z(); - EXPECT_NEAR(xvalues[0], x_out, epsilon); - EXPECT_NEAR(yvalues[0], y_out, epsilon); - EXPECT_NEAR(zvalues[0], z_out, epsilon); - } -} - -TEST(Bullet, Slerp) -{ - - uint64_t runs = 100; - seed_rand(); - - tf3::Quaternion q1, q2; - q1.setEuler(0,0,0); - - for (uint64_t i = 0 ; i < runs ; i++) - { - q2.setEuler(1.0 * get_rand(), - 1.0 * get_rand(), - 1.0 * get_rand()); - - - tf3::Quaternion q3 = slerp(q1,q2,0.5); - - EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5); - } - -} - - -TEST(TimeCache, AngularInterpolation) -{ - uint64_t runs = 100; - double epsilon = 1e-6; - seed_rand(); - - tf3::TimeCache cache; - std::vector yawvalues(2); - std::vector pitchvalues(2); - std::vector rollvalues(2); - uint64_t offset = 200; - - std::vector quats(2); - - TransformStorage stor; - setIdentity(stor); - - for ( uint64_t i = 1; i < runs ; i++ ) - { - - for (uint64_t step = 0; step < 2 ; step++) - { - yawvalues[step] = 10.0 * get_rand() / 100.0; - pitchvalues[step] = 0;//10.0 * get_rand(); - rollvalues[step] = 0;//10.0 * get_rand(); - quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]); - stor.rotation_ = quats[step]; - stor.frame_id_ = 3; - stor.stamp_ = robot::Time().fromNSec(offset + (step * 100)); //step = 0 or 1 - cache.insertData(stor); - } - - for (int pos = 0; pos < 100 ; pos ++) - { - uint64_t time = offset + pos; - cache.getData(robot::Time().fromNSec(time), stor); - uint64_t time_out = stor.stamp_.toNSec(); - tf3::Quaternion quat (stor.rotation_); - - //Generate a ground truth quaternion directly calling slerp - tf3::Quaternion ground_truth = quats[0].slerp(quats[1], pos/100.0); - - //Make sure the transformed one and the direct call match - EXPECT_EQ(time, time_out); - EXPECT_NEAR(0, angle(ground_truth, quat), epsilon); - - } - - cache.clearList(); - } - - -} - -TEST(TimeCache, DuplicateEntries) -{ - - TimeCache cache; - - TransformStorage stor; - setIdentity(stor); - stor.frame_id_ = 3; - stor.stamp_ = robot::Time().fromNSec(1); - - cache.insertData(stor); - - cache.insertData(stor); - - - cache.getData(robot::Time().fromNSec(1), stor); - - //printf(" stor is %f\n", stor.translation_.x()); - EXPECT_TRUE(!std::isnan(stor.translation_.x())); - EXPECT_TRUE(!std::isnan(stor.translation_.y())); - EXPECT_TRUE(!std::isnan(stor.translation_.z())); - EXPECT_TRUE(!std::isnan(stor.rotation_.x())); - EXPECT_TRUE(!std::isnan(stor.rotation_.y())); - EXPECT_TRUE(!std::isnan(stor.rotation_.z())); - EXPECT_TRUE(!std::isnan(stor.rotation_.w())); -} - -int main(int argc, char **argv){ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/src/Libraries/tf3/test/simple_tf3_core.cpp b/src/Libraries/tf3/test/simple_tf3_core.cpp deleted file mode 100644 index 08085a5..0000000 --- a/src/Libraries/tf3/test/simple_tf3_core.cpp +++ /dev/null @@ -1,320 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ - -#include -#include -#include -#include "tf3/LinearMath/Vector3.h" -#include "tf3/exceptions.h" - -TEST(tf3, setTransformFail) -{ - tf3::BufferCore tfc; - robot_geometry_msgs::TransformStamped st; - EXPECT_FALSE(tfc.setTransform(st, "authority1")); - -} - -TEST(tf3, setTransformValid) -{ - tf3::BufferCore tfc; - robot_geometry_msgs::TransformStamped st; - st.header.frame_id = "foo"; - st.header.stamp = robot::Time(1.0); - st.child_frame_id = "child"; - st.transform.rotation.w = 1; - EXPECT_TRUE(tfc.setTransform(st, "authority1")); - -} - -TEST(tf3, setTransformInvalidQuaternion) -{ - tf3::BufferCore tfc; - robot_geometry_msgs::TransformStamped st; - st.header.frame_id = "foo"; - st.header.stamp = robot::Time(1.0); - st.child_frame_id = "child"; - st.transform.rotation.w = 0; - EXPECT_FALSE(tfc.setTransform(st, "authority1")); - -} - -TEST(tf3_lookupTransform, LookupException_Nothing_Exists) -{ - tf3::BufferCore tfc; - EXPECT_THROW(tfc.lookupTransform("a", "b", robot::Time().fromSec(1.0)), tf3::LookupException); - -} - -TEST(tf3_canTransform, Nothing_Exists) -{ - tf3::BufferCore tfc; - EXPECT_FALSE(tfc.canTransform("a", "b", robot::Time().fromSec(1.0))); - - std::string error_msg = std::string(); - EXPECT_FALSE(tfc.canTransform("a", "b", robot::Time().fromSec(1.0), &error_msg)); - ASSERT_STREQ(error_msg.c_str(), "canTransform: target_frame a does not exist. canTransform: source_frame b does not exist."); - -} - -TEST(tf3_lookupTransform, LookupException_One_Exists) -{ - tf3::BufferCore tfc; - robot_geometry_msgs::TransformStamped st; - st.header.frame_id = "foo"; - st.header.stamp = robot::Time(1.0); - st.child_frame_id = "child"; - st.transform.rotation.w = 1; - EXPECT_TRUE(tfc.setTransform(st, "authority1")); - EXPECT_THROW(tfc.lookupTransform("foo", "bar", robot::Time().fromSec(1.0)), tf3::LookupException); - -} - -TEST(tf3_canTransform, One_Exists) -{ - tf3::BufferCore tfc; - robot_geometry_msgs::TransformStamped st; - st.header.frame_id = "foo"; - st.header.stamp = robot::Time(1.0); - st.child_frame_id = "child"; - st.transform.rotation.w = 1; - EXPECT_TRUE(tfc.setTransform(st, "authority1")); - EXPECT_FALSE(tfc.canTransform("foo", "bar", robot::Time().fromSec(1.0))); -} - -TEST(tf3_chainAsVector, chain_v_configuration) -{ - tf3::BufferCore mBC; - tf3::TestBufferCore tBC; - - robot_geometry_msgs::TransformStamped st; - st.header.stamp = robot::Time(0); - st.transform.rotation.w = 1; - - st.header.frame_id = "a"; - st.child_frame_id = "b"; - mBC.setTransform(st, "authority1"); - - st.header.frame_id = "b"; - st.child_frame_id = "c"; - mBC.setTransform(st, "authority1"); - - st.header.frame_id = "a"; - st.child_frame_id = "d"; - mBC.setTransform(st, "authority1"); - - st.header.frame_id = "d"; - st.child_frame_id = "e"; - mBC.setTransform(st, "authority1"); - - std::vector chain; - - - mBC._chainAsVector( "c", robot::Time(), "c", robot::Time(), "c", chain); - EXPECT_EQ( 0, chain.size()); - - mBC._chainAsVector( "a", robot::Time(), "c", robot::Time(), "c", chain); - EXPECT_EQ( 3, chain.size()); - if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" ); - if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); - if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); - - mBC._chainAsVector( "c", robot::Time(), "a", robot::Time(), "c", chain); - EXPECT_EQ( 3, chain.size()); - if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" ); - if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); - if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" ); - - mBC._chainAsVector( "a", robot::Time(), "c", robot::Time(), "a", chain); - EXPECT_EQ( 3, chain.size()); - if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" ); - if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); - if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); - - mBC._chainAsVector( "c", robot::Time(), "a", robot::Time(), "a", chain); - EXPECT_EQ( 3, chain.size()); - if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" ); - if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); - if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" ); - - mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "c", chain); - - EXPECT_EQ( 5, chain.size()); - if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" ); - if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" ); - if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); - if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" ); - if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" ); - - mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "a", chain); - - EXPECT_EQ( 5, chain.size()); - if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" ); - if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" ); - if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); - if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" ); - if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" ); - - mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "e", chain); - - EXPECT_EQ( 5, chain.size()); - if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" ); - if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" ); - if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); - if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" ); - if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" ); -} - -TEST(tf3_walkToTopParent, walk_i_configuration) -{ - tf3::BufferCore mBC; - tf3::TestBufferCore tBC; - - robot_geometry_msgs::TransformStamped st; - st.header.stamp = robot::Time(0); - st.transform.rotation.w = 1; - - st.header.frame_id = "a"; - st.child_frame_id = "b"; - mBC.setTransform(st, "authority1"); - - st.header.frame_id = "b"; - st.child_frame_id = "c"; - mBC.setTransform(st, "authority1"); - - st.header.frame_id = "c"; - st.child_frame_id = "d"; - mBC.setTransform(st, "authority1"); - - st.header.frame_id = "d"; - st.child_frame_id = "e"; - mBC.setTransform(st, "authority1"); - - std::vector id_chain; - tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain); - - EXPECT_EQ(5, id_chain.size() ); - if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); - if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); - if( id_chain.size() >= 3 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[2])); - if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3])); - if( id_chain.size() >= 5 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[4])); - - id_chain.clear(); - tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain); - - EXPECT_EQ(5, id_chain.size() ); - if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0])); - if( id_chain.size() >= 2 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[1])); - if( id_chain.size() >= 3 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[2])); - if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3])); - if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4])); - -} - -TEST(tf3_walkToTopParent, walk_v_configuration) -{ - tf3::BufferCore mBC; - tf3::TestBufferCore tBC; - - robot_geometry_msgs::TransformStamped st; - st.header.stamp = robot::Time(0); - st.transform.rotation.w = 1; - - // st.header.frame_id = "aaa"; - // st.child_frame_id = "aa"; - // mBC.setTransform(st, "authority1"); - // - // st.header.frame_id = "aa"; - // st.child_frame_id = "a"; - // mBC.setTransform(st, "authority1"); - - st.header.frame_id = "a"; - st.child_frame_id = "b"; - mBC.setTransform(st, "authority1"); - - st.header.frame_id = "b"; - st.child_frame_id = "c"; - mBC.setTransform(st, "authority1"); - - st.header.frame_id = "a"; - st.child_frame_id = "d"; - mBC.setTransform(st, "authority1"); - - st.header.frame_id = "d"; - st.child_frame_id = "e"; - mBC.setTransform(st, "authority1"); - - std::vector id_chain; - tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("c"), 0, &id_chain); - - EXPECT_EQ(5, id_chain.size()); - if( id_chain.size() >= 1 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[0])); - if( id_chain.size() >= 2 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[1])); - if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2])); - if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3])); - if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4])); - - tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("c"), mBC._lookupFrameNumber("e"), 0, &id_chain); - EXPECT_EQ(5, id_chain.size()); - if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); - if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); - if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2])); - if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3])); - if( id_chain.size() >= 5 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[4])); - - tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain); - EXPECT_EQ( id_chain.size(), 3 ); - if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); - if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); - if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2])); - - tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain); - EXPECT_EQ( id_chain.size(), 3 ); - if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0])); - if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); - if( id_chain.size() >= 3 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[2])); - - tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("d"), 0, &id_chain); - EXPECT_EQ( id_chain.size(), 2 ); - if( id_chain.size() >= 1 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[0])); - if( id_chain.size() >= 2 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[1])); - - tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("d"), mBC._lookupFrameNumber("e"), 0, &id_chain); - EXPECT_EQ( id_chain.size(), 2 ); - if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); - if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); -} - - -int main(int argc, char **argv){ - testing::InitGoogleTest(&argc, argv); - robot::Time::init(); //needed for robot::TIme::now() - return RUN_ALL_TESTS(); -} diff --git a/src/Libraries/tf3/test/speed_test.cpp b/src/Libraries/tf3/test/speed_test.cpp deleted file mode 100644 index 6adc118..0000000 --- a/src/Libraries/tf3/test/speed_test.cpp +++ /dev/null @@ -1,252 +0,0 @@ -/* - * Copyright (c) 2010, Willow Garage, Inc. - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ - -#include - -#include -#include - -#include - -int main(int argc, char** argv) -{ - uint32_t num_levels = 10; - if (argc > 1) - { - num_levels = boost::lexical_cast(argv[1]); - } - double time_interval = 1.0; - if (argc > 2) - { - time_interval = boost::lexical_cast(argv[2]); - } - - console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_INFO); - - tf3::BufferCore bc; - robot_geometry_msgs::TransformStamped t; - t.header.stamp = robot::Time(1); - t.header.frame_id = "root"; - t.child_frame_id = "0"; - t.transform.translation.x = 1; - t.transform.rotation.w = 1.0; - bc.setTransform(t, "me"); - t.header.stamp = robot::Time(2); - bc.setTransform(t, "me"); - - for (uint32_t i = 1; i < num_levels / 2; ++i) - { - for (double j = time_interval; j < 2.0 + time_interval; j += time_interval) - { - std::stringstream parent_ss; - parent_ss << (i - 1); - std::stringstream child_ss; - child_ss << i; - - t.header.stamp = robot::Time(j); - t.header.frame_id = parent_ss.str(); - t.child_frame_id = child_ss.str(); - bc.setTransform(t, "me"); - } - } - - t.header.frame_id = "root"; - std::stringstream ss; - ss << num_levels/2; - t.header.stamp = robot::Time(1); - t.child_frame_id = ss.str(); - bc.setTransform(t, "me"); - t.header.stamp = robot::Time(2); - bc.setTransform(t, "me"); - - for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i) - { - for (double j = time_interval; j < 2.0 + time_interval; j += time_interval) - { - std::stringstream parent_ss; - parent_ss << (i - 1); - std::stringstream child_ss; - child_ss << i; - - t.header.stamp = robot::Time(j); - t.header.frame_id = parent_ss.str(); - t.child_frame_id = child_ss.str(); - bc.setTransform(t, "me"); - } - } - - //logInfo_STREAM(bc.allFramesAsYAML()); - - std::string v_frame0 = boost::lexical_cast(num_levels - 1); - std::string v_frame1 = boost::lexical_cast(num_levels/2 - 1); - CONSOLE_BRIDGE_logInform("%s to %s", v_frame0.c_str(), v_frame1.c_str()); - robot_geometry_msgs::TransformStamped out_t; - - const uint32_t count = 1000000; - CONSOLE_BRIDGE_logInform("Doing %d %d-level %lf-interval tests", count, num_levels, time_interval); - -#if 01 - { - robot::WallTime start = robot::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { - out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(0)); - } - robot::WallTime end = robot::WallTime::now(); - robot::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - CONSOLE_BRIDGE_logInform("lookupTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); - } -#endif - -#if 01 - { - robot::WallTime start = robot::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { - out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(1)); - } - robot::WallTime end = robot::WallTime::now(); - robot::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - CONSOLE_BRIDGE_logInform("lookupTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); - } -#endif - -#if 01 - { - robot::WallTime start = robot::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { - out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(1.5)); - } - robot::WallTime end = robot::WallTime::now(); - robot::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - CONSOLE_BRIDGE_logInform("lookupTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); - } -#endif - -#if 01 - { - robot::WallTime start = robot::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { - out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(2)); - } - robot::WallTime end = robot::WallTime::now(); - robot::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - CONSOLE_BRIDGE_logInform("lookupTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); - } -#endif - -#if 01 - { - robot::WallTime start = robot::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { - bc.canTransform(v_frame1, v_frame0, robot::Time(0)); - } - robot::WallTime end = robot::WallTime::now(); - robot::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - CONSOLE_BRIDGE_logInform("canTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); - } -#endif - -#if 01 - { - robot::WallTime start = robot::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { - bc.canTransform(v_frame1, v_frame0, robot::Time(1)); - } - robot::WallTime end = robot::WallTime::now(); - robot::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - CONSOLE_BRIDGE_logInform("canTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); - } -#endif - -#if 01 - { - robot::WallTime start = robot::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { - bc.canTransform(v_frame1, v_frame0, robot::Time(1.5)); - } - robot::WallTime end = robot::WallTime::now(); - robot::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - CONSOLE_BRIDGE_logInform("canTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); - } -#endif - -#if 01 - { - robot::WallTime start = robot::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { - bc.canTransform(v_frame1, v_frame0, robot::Time(2)); - } - robot::WallTime end = robot::WallTime::now(); - robot::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - CONSOLE_BRIDGE_logInform("canTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); - } -#endif - -#if 01 - { - robot::WallTime start = robot::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { - bc.canTransform(v_frame1, v_frame0, robot::Time(3)); - } - robot::WallTime end = robot::WallTime::now(); - robot::WallDuration dur = end - start; - CONSOLE_BRIDGE_logInform("canTransform at Time(3) without error string took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); - } -#endif - -#if 01 - { - robot::WallTime start = robot::WallTime::now(); - std::string str; - for (uint32_t i = 0; i < count; ++i) - { - bc.canTransform(v_frame1, v_frame0, robot::Time(3), &str); - } - robot::WallTime end = robot::WallTime::now(); - robot::WallDuration dur = end - start; - CONSOLE_BRIDGE_logInform("canTransform at Time(3) with error string took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); - } -#endif -} diff --git a/src/Libraries/tf3/test/static_cache_test.cpp b/src/Libraries/tf3/test/static_cache_test.cpp deleted file mode 100644 index a2498eb..0000000 --- a/src/Libraries/tf3/test/static_cache_test.cpp +++ /dev/null @@ -1,101 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ - -#include -#include -#include - -#include - -#include - -using namespace tf3; - - -void setIdentity(TransformStorage& stor) -{ - stor.translation_.setValue(0.0, 0.0, 0.0); - stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0); -} - -TEST(StaticCache, Repeatability) -{ - unsigned int runs = 100; - - tf3::StaticCache cache; - - TransformStorage stor; - setIdentity(stor); - - for ( uint64_t i = 1; i < runs ; i++ ) - { - stor.frame_id_ = CompactFrameID(i); - stor.stamp_ = robot::Time().fromNSec(i); - - cache.insertData(stor); - - - cache.getData(robot::Time().fromNSec(i), stor); - EXPECT_EQ(stor.frame_id_, i); - EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i)); - - } -} - -TEST(StaticCache, DuplicateEntries) -{ - - tf3::StaticCache cache; - - TransformStorage stor; - setIdentity(stor); - stor.frame_id_ = CompactFrameID(3); - stor.stamp_ = robot::Time().fromNSec(1); - - cache.insertData(stor); - - cache.insertData(stor); - - - cache.getData(robot::Time().fromNSec(1), stor); - - //printf(" stor is %f\n", stor.transform.translation.x); - EXPECT_TRUE(!std::isnan(stor.translation_.x())); - EXPECT_TRUE(!std::isnan(stor.translation_.y())); - EXPECT_TRUE(!std::isnan(stor.translation_.z())); - EXPECT_TRUE(!std::isnan(stor.rotation_.x())); - EXPECT_TRUE(!std::isnan(stor.rotation_.y())); - EXPECT_TRUE(!std::isnan(stor.rotation_.z())); - EXPECT_TRUE(!std::isnan(stor.rotation_.w())); -} - -int main(int argc, char **argv){ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/Libraries/tf3/test/test_transform_datatypes.cpp b/src/Libraries/tf3/test/test_transform_datatypes.cpp deleted file mode 100644 index 552d6ae..0000000 --- a/src/Libraries/tf3/test/test_transform_datatypes.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/* - * Copyright (c) 2020, Open Source Robotics Foundation, Inc. - * 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. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * 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. - */ -#include - -#include "tf3/transform_datatypes.h" - -#include - - -TEST(Stamped, assignment) -{ - tf3::Stamped first("foobar", robot::Time(0), "my_frame_id"); - tf3::Stamped second("baz", robot::Time(0), "my_frame_id"); - - EXPECT_NE(second, first); - second = first; - EXPECT_EQ(second, first); -} - -TEST(Stamped, setData) -{ - tf3::Stamped first("foobar", robot::Time(0), "my_frame_id"); - tf3::Stamped second("baz", robot::Time(0), "my_frame_id"); - - EXPECT_NE(second, first); - second.setData("foobar"); - EXPECT_EQ(second, first); -} - -TEST(Stamped, copy_constructor) -{ - tf3::Stamped first("foobar", robot::Time(0), "my_frame_id"); - tf3::Stamped second(first); - - EXPECT_EQ(second, first); -} - -TEST(Stamped, default_constructor) -{ - tf3::Stamped first("foobar", robot::Time(0), "my_frame_id"); - tf3::Stamped second; - - EXPECT_NE(second, first); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - robot::Time::init(); - return RUN_ALL_TESTS(); -}