search

onlytailei / CppRobotics

cpp implementation of robotics algorithms including localization, mapping, SLAM, path planning and control
MIT License
1.83k stars 490 forks source link

#define YAW_P2P(angle) std::fmod((angle+M_PI), 2*M_PI)-M_PI #8

Closed TanhuanHUST closed 4 years ago

TanhuanHUST commented 4 years ago

set: angleA = -M_Pi angleB = M_PI

YAW_P2P(angleA - angleB) = -6.28318548

but the correct answer is 0；

onlytailei commented 4 years ago

set: angleA = -M_Pi angleB = M_PI

YAW_P2P(angleA - angleB) = -6.28318548

but the correct answer is 0；

That is how define works. Define is not a function, it will simply replace what you provide.

YAW(angleA-angleB) will be replaced as std::fmod((angleA-angleB+M_PI), 2*M_PI) - M_PI in the compliation.

You can add a pair of paretheses around "angle" as (angle)+M_PI.

TanhuanHUST commented 4 years ago

I want to figure out : ans = std::fmod((-M_PI - M_PI +M_PI), 2*M_PI) ; the value of ans is -MPI in windows visual studio 2017 . so i think YAW(yaw) cant convert yaw_ belong to [-pi,pi];