.. _program_listing_file_include_geodesy_utm.h:

Program Listing for File utm.h
==============================

|exhale_lsh| :ref:`Return to documentation for file <file_include_geodesy_utm.h>` (``include/geodesy/utm.h``)

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

.. code-block:: cpp

   /* -*- mode: C++ -*- */
   /* $Id: 2f342cef098e0063f550dc455212675ee6b71b9f $ */
   
   /*********************************************************************
   * Software License Agreement (BSD License)
   *
   *  Copyright (C) 2011 Jack O'Quin
   *  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 author nor other 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 _UTM_H_
   #define _UTM_H_
   
   #include <limits>
   #include <ctype.h>
   #include <iostream>
   #include <iomanip>
   
   #include <geodesy/wgs84.h>
   #include "geographic_msgs/msg/geo_point.hpp"
   #include "geometry_msgs/msg/point.hpp"
   #include "geographic_msgs/msg/geo_pose.hpp"
   #include "geometry_msgs/msg/pose.hpp"
   
   namespace geodesy
   {
   
   class UTMPoint
   {
    public:
   
     UTMPoint():
       easting(0.0),
       northing(0.0),
       altitude(std::numeric_limits<double>::quiet_NaN()),
       zone(0),
       band(' ')
     {}
   
     UTMPoint(const UTMPoint &that):
       easting(that.easting),
       northing(that.northing),
       altitude(that.altitude),
       zone(that.zone),
       band(that.band)
     {}
     UTMPoint(const geographic_msgs::msg::GeoPoint &pt);
   
     UTMPoint(double _easting, double _northing, uint8_t _zone, char _band):
       easting(_easting),
       northing(_northing),
       altitude(std::numeric_limits<double>::quiet_NaN()),
       zone(_zone),
       band(_band)
     {}
   
     UTMPoint(double _easting, double _northing, double _altitude,
               uint8_t _zone, char _band):
       easting(_easting),
       northing(_northing),
       altitude(_altitude),
       zone(_zone),
       band(_band)
     {}
   
     // data members
     double easting;           
     double northing;          
     double altitude;          
     uint8_t zone;             
     char   band;              
   
   }; // class UTMPoint
   
   class UTMPose
   {
    public:
   
     UTMPose():
       position(),
       orientation()
     {}
   
     UTMPose(const UTMPose &that):
       position(that.position),
       orientation(that.orientation)
     {}
   
     UTMPose(const geographic_msgs::msg::GeoPose &pose):
       position(pose.position),
       orientation(pose.orientation)
     {}
   
     UTMPose(UTMPoint pt,
             const geometry_msgs::msg::Quaternion &q):
       position(pt),
       orientation(q)
     {}
   
     UTMPose(const geographic_msgs::msg::GeoPoint &pt,
             const geometry_msgs::msg::Quaternion &q):
       position(pt),
       orientation(q)
     {}
   
     // data members
     UTMPoint position;
     geometry_msgs::msg::Quaternion orientation;
   
   }; // class UTMPose
   
   // conversion function prototypes
   void fromMsg(const geographic_msgs::msg::GeoPoint &from, UTMPoint &to,
           const bool& force_zone=false, const char& band='A', const uint8_t& zone=0 );
   void fromMsg(const geographic_msgs::msg::GeoPose &from, UTMPose &to,
           const bool& force_zone=false, const char& band='A', const uint8_t& zone=0 );
   geographic_msgs::msg::GeoPoint toMsg(const UTMPoint &from);
   geographic_msgs::msg::GeoPose toMsg(const UTMPose &from);
   
   static inline bool is2D(const UTMPoint &pt)
   {
     // true if altitude is a NaN
     return (pt.altitude != pt.altitude);
   }
   
   static inline bool is2D(const UTMPose &pose)
   {
     // true if position has no altitude
     return is2D(pose.position);
   }
   
   bool isValid(const UTMPoint &pt);
   bool isValid(const UTMPose &pose);
   
   static inline void normalize(UTMPoint &pt)
   {
     geographic_msgs::msg::GeoPoint ll(toMsg(pt));
     normalize(ll);
     fromMsg(ll, pt);
   }
   
   static inline std::ostream& operator<<(std::ostream& out, const UTMPoint &pt)
   {
     out << "(" << std::setprecision(10) << pt.easting << ", "
         << pt.northing << ", " << std::setprecision(6) << pt.altitude
         << " [" << (unsigned) pt.zone << pt.band << "])";
     return out;
   }
   
   static inline std::ostream& operator<<(std::ostream& out, const UTMPose &pose)
   {
     out << pose.position << ", (["
         << pose.orientation.x << ", "
         << pose.orientation.y << ", "
         << pose.orientation.z << "], "
         << pose.orientation.w << ")";
     return out;
   }
   
   static inline bool sameGridZone(const UTMPoint &pt1, const UTMPoint &pt2)
   {
     return ((pt1.zone == pt2.zone) && (pt1.band == pt2.band));
   }
   
   static inline bool sameGridZone(const UTMPose &pose1, const UTMPose &pose2)
   {
     return sameGridZone(pose1.position, pose2.position);
   }
   
   static inline geometry_msgs::msg::Point toGeometry(const UTMPoint &from)
   {
     geometry_msgs::msg::Point to;
     to.x = from.easting;
     to.y = from.northing;
     to.z = from.altitude;
     return to;
   }
   
   static inline geometry_msgs::msg::Pose toGeometry(const UTMPose &from)
   {
     geometry_msgs::msg::Pose to;
     to.position = toGeometry(from.position);
     to.orientation = from.orientation;
     return to;
   }
   
   }  // namespace geodesy
   
   #endif // _UTM_H_