【C++学习笔记】fmod()函数使用&角度归一化案例

通通不止二十块

已于 2024-01-12 16:38:19 修改

阅读量4.4k

点赞数 3

文章标签：学习笔记 c++ 算法自动驾驶

于 2023-11-17 11:02:02 首次发布

本文链接：https://blog.csdn.net/BTChioa/article/details/134434390

版权

fmod()函数用来计算两个浮点型数据的余数，可用于角度的归一化。

一、fmod()函数定义

fmod()函数是C++中的一个数学函数，用于计算两个浮点数的模。它的原型如下：

double fmod(double x, double y);

其中，x和y是要计算模的两个浮点数，函数返回值为x除以y的余数（double）。

二、使用步骤

1.头文件: #include<cmath>

2.使用案例：

#include <iostream>
#include <cmath>

using namespace std;

int main()
{
    double x = 10.5, y = 3.2;
    double result = fmod(x, y);
    cout << "x % y = " << result << endl;
    return 0;
}

该例子中，我们使用了cmath头文件中的fmod()函数，计算了10.5除以3.2的余数，结果为0.9。

三、角度归一化案例：

三角函数为周期函数，反三角函数的计算结果按道理说有无数个，为了方便将其映射到一个周期内。而且C++中的反三角函数给出的结果映射到了[-pi, pi]，计算角度时为了和C++标准库保持一致，也将角度值映射到这个区间。至于这个区间为什么是全闭区间，查看C++标准库（以std::arctan2为例）说明如下：

If no errors occur, the arc tangent of y/x (arctan(yx)) in the range [-π , +π] radians, is returned.

If x is -∞ and y is finite and positive, +π is returned
If x is -∞ and y is finite and negative, -π is returned

1.Apollo中的角度归一化案例

double NormalizeAngle(const double angle) {
    double a = std::fmod(angle + M_PI, 2.0 * M_PI); 
    if (a < 0.0) {
        a += (2.0 * M_PI);
    }
    return a - M_PI;
}

Apollo的代码将角度值映射到了 [-pi, pi) ，左闭右开的区间，该代码对应的公式如下：

2.常规思路角度归一化案例

double NormalizeAngle(const double angle) {
    double a = std::fmod(angle, 2.0 * M_PI); 
    if (a < -M_PI) {
        a += (2.0 * M_PI);
    } else if (a >= M_PI) { // 这里一般不加等号，为了和Apollo代码保持完全一致（映射到[-pi,pi))才加了等号。
    	a -= (2.0 * M_PI);
    }
    return a;
}

该代码对应的公式如下：

3.另外一种角度归一化思路

double NormalizeAngle(const double angle) {
    double a = std::fmod(angle + M_PI, 2.0 * M_PI); 
    if (a < 0) {
        a += M_PI;
    } else { 
    	a -= M_PI;
    }
    return a;
}

该代码对应的公式如下：