math_util.cpp
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from this software without specific prior written permission.
00016 //
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 
00030 #include <swri_math_util/math_util.h>
00031 
00032 #include <cmath>
00033 
00034 namespace swri_math_util
00035 {
00036   double Round(double value)
00037   {
00038     return (value > 0.0) ? std::floor(value + 0.5) : std::ceil(value - 0.5);
00039   }
00040 
00041   double ToNearest(double value, double multiple)
00042   {
00043     if (multiple == 0)
00044     {
00045       return 0;
00046     }
00047 
00048     return Round(value / multiple) * multiple;
00049   }
00050 
00051   double UpToNearest(double value, double multiple)
00052   {
00053     if (multiple == 0)
00054     {
00055       return 0;
00056     }
00057 
00058     return std::ceil(value / multiple) * multiple;
00059   }
00060 
00061   bool IsNear(double v1, double v2, double epsilon)
00062   {
00063     return std::fabs(v1 - v2) <= epsilon;
00064   }
00065 
00066   double unWrapAngle(double static_angle,
00067                      double variable_angle,
00068                      double threshold)
00069   {
00070     if (std::abs(static_angle - variable_angle) > threshold)
00071     {
00072       if (variable_angle < static_angle)
00073       {
00074         variable_angle += swri_math_util::_2pi;
00075       }
00076       else
00077       {
00078         variable_angle -= swri_math_util::_2pi;
00079       }
00080     }
00081     return variable_angle;
00082   }
00083 }


swri_math_util
Author(s): Marc Alban
autogenerated on Tue Oct 3 2017 03:19:18