|
|
|
@ -49,28 +49,28 @@ quint64 groundTimeMilliseconds()
@@ -49,28 +49,28 @@ quint64 groundTimeMilliseconds()
|
|
|
|
|
|
|
|
|
|
float limitAngleToPMPIf(float angle) |
|
|
|
|
{ |
|
|
|
|
while (angle > ((float)M_PI+FLT_EPSILON)) { |
|
|
|
|
angle -= 2.0f * (float)M_PI; |
|
|
|
|
} |
|
|
|
|
// while (angle > ((float)M_PI+FLT_EPSILON)) {
|
|
|
|
|
// angle -= 2.0f * (float)M_PI;
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
while (angle <= -((float)M_PI+FLT_EPSILON)) { |
|
|
|
|
angle += 2.0f * (float)M_PI; |
|
|
|
|
} |
|
|
|
|
// while (angle <= -((float)M_PI+FLT_EPSILON)) {
|
|
|
|
|
// angle += 2.0f * (float)M_PI;
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
return angle; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
double limitAngleToPMPId(double angle) |
|
|
|
|
{ |
|
|
|
|
if (angle < -M_PI) { |
|
|
|
|
while (angle < -M_PI) { |
|
|
|
|
angle += M_PI; |
|
|
|
|
} |
|
|
|
|
} else if (angle > M_PI) { |
|
|
|
|
while (angle > M_PI) { |
|
|
|
|
angle -= M_PI; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
// if (angle < -M_PI) {
|
|
|
|
|
// while (angle < -M_PI) {
|
|
|
|
|
// angle += M_PI;
|
|
|
|
|
// }
|
|
|
|
|
// } else if (angle > M_PI) {
|
|
|
|
|
// while (angle > M_PI) {
|
|
|
|
|
// angle -= M_PI;
|
|
|
|
|
// }
|
|
|
|
|
// }
|
|
|
|
|
return angle; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|