.. _program_listing_file_include_tf2_bullet_tf2_bullet.hpp:

Program Listing for File tf2_bullet.hpp
=======================================

|exhale_lsh| :ref:`Return to documentation for file <file_include_tf2_bullet_tf2_bullet.hpp>` (``include/tf2_bullet/tf2_bullet.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // Copyright 2008 Willow Garage, Inc.
   //
   // 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 HOLDER 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 TF2_BULLET__TF2_BULLET_HPP_
   #define TF2_BULLET__TF2_BULLET_HPP_
   
   #include <iostream>
   
   #include "tf2/convert.h"
   #include "LinearMath/btQuaternion.h"
   #include "LinearMath/btScalar.h"
   #include "LinearMath/btTransform.h"
   #include "geometry_msgs/msg/point_stamped.hpp"
   #include "tf2_ros/buffer_interface.h"
   
   #if (BT_BULLET_VERSION <= 282)
   // Suppress compilation warning on older versions of Bullet.
   // TODO(mjcarroll): Remove this when all platforms have the fix upstream.
   inline int bullet_btInfinityMask()
   {
     return btInfinityMask;
   }
   #endif
   
   namespace tf2
   {
   inline
   btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t)
   {
     return btTransform(
       btQuaternion(
         static_cast<float>(t.transform.rotation.x),
         static_cast<float>(t.transform.rotation.y),
         static_cast<float>(t.transform.rotation.z),
         static_cast<float>(t.transform.rotation.w)),
       btVector3(
         static_cast<float>(t.transform.translation.x),
         static_cast<float>(t.transform.translation.y),
         static_cast<float>(t.transform.translation.z)));
   }
   
   
   template< >
   inline
   void doTransform(
     const tf2::Stamped<btVector3> & t_in, tf2::Stamped<btVector3> & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     t_out =
       tf2::Stamped<btVector3>(
       transformToBullet(transform) * t_in,
       tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id);
   }
   
   inline
   geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped<btVector3> & in)
   {
     geometry_msgs::msg::PointStamped msg;
     msg.header.stamp = tf2_ros::toMsg(in.stamp_);
     msg.header.frame_id = in.frame_id_;
     msg.point.x = in[0];
     msg.point.y = in[1];
     msg.point.z = in[2];
     return msg;
   }
   
   inline
   void fromMsg(const geometry_msgs::msg::PointStamped & msg, tf2::Stamped<btVector3> & out)
   {
     out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
     out.frame_id_ = msg.header.frame_id;
     out[0] = static_cast<float>(msg.point.x);
     out[1] = static_cast<float>(msg.point.y);
     out[2] = static_cast<float>(msg.point.z);
   }
   
   
   template< >
   inline
   void doTransform(
     const tf2::Stamped<btTransform> & t_in, tf2::Stamped<btTransform> & t_out,
     const geometry_msgs::msg::TransformStamped & transform)
   {
     t_out =
       tf2::Stamped<btTransform>(
       transformToBullet(transform) * t_in,
       tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id);
   }
   
   
   }  // namespace tf2
   
   #endif  // TF2_BULLET__TF2_BULLET_HPP_