assert in tests works even if NDEBUG is defined. and fabs->std::abs

Author: kenjihiranabe

drone_physics/body_physics.cpp

@@ -126,9 +126,9 @@ AccelerationType acceleration_in_body_frame(
     double air_v = v - wind.y;
     double air_w = w - wind.z;
 
-    double dot_u =       - g * s_theta            - (q*w - r*v) - d1.x/m * (air_u) - d2.x/m * air_u * std::fabs(air_u);
-    double dot_v =       + g * c_theta * s_phi    - (r*u - p*w) - d1.y/m * (air_v) - d2.y/m * air_v * std::fabs(air_v);
-    double dot_w = -T/m  + g * c_theta * c_phi    - (p*v - q*u) - d1.z/m * (air_w) - d2.z/m * air_w * std::fabs(air_w);
+    double dot_u =       - g * s_theta            - (q*w - r*v) - d1.x/m * (air_u) - d2.x/m * air_u * std::abs(air_u);
+    double dot_v =       + g * c_theta * s_phi    - (r*u - p*w) - d1.y/m * (air_v) - d2.y/m * air_v * std::abs(air_v);
+    double dot_w = -T/m  + g * c_theta * c_phi    - (p*v - q*u) - d1.z/m * (air_w) - d2.z/m * air_w * std::abs(air_w);
     /*****************************************************************/  
 
     return {dot_u, dot_v, dot_w};
@@ -354,7 +354,7 @@ EulerType euler_from_quaternion(const QuaternionType& quaternion)
      * - https://www.docswell.com/s/Kouhei_Ito/KDVNVK-2022-06-15-193343#p3
      * - 
      */
-    using std::atan2; using std::asin; using std::cos; using std::fabs;
+    using std::atan2; using std::asin; using std::cos;
     auto [q0, q1, q2, q3] = quaternion; /* [w, x, y, z] aligned name with the book for reviewability */
 
     // First, see theta,  asin is nearly 1.0, cos(theta) is nearly 0.0
@@ -367,7 +367,7 @@ EulerType euler_from_quaternion(const QuaternionType& quaternion)
      *  Here, the gimbal lock happens, and the euler angle is not unique.
      *  Choose the one with the phi = 0, namely (0, 90, -90).
      **/
-    if (fabs(cos(theta)) < 0.0001) {
+    if (std::abs(cos(theta)) < 0.0001) {
         phi = 0;
         psi = atan2(2*(q0*q3 - q1*q2), 2*(q0*q0 + q2*q2) - 1);
     } else {

drone_physics/ctest.c

@@ -1,6 +1,8 @@
 #include "drone_physics_debug.h"
 #include "drone_physics_osdep.h"
 
+#undef NDEBUG // force assert to work even if release bulid.
+
 const double PI = M_PI;
 
 void static test_frame_all_unit_vectors_with_some_angles() {

drone_physics/glmtest.cpp

@@ -3,7 +3,7 @@
 #include <glm/gtc/matrix_inverse.hpp>
 #include <cmath>
 
-#define assert_almost_equal_dvec4(u, v) assert(std::fabs(u[0]-v[0]) < 0.001 && std::fabs(u[1]-v[1]) < 0.001 && std::fabs(u[2]-v[2]) < 0.001 && std::fabs(u[3]-v[3]) < 0.001 || (out("dvec4 different! u=", u, " "), out("v=", v), false))
+#define assert_almost_equal_dvec4(u, v) assert(std::abs(u[0]-v[0]) < 0.001 && std::abs(u[1]-v[1]) < 0.001 && std::abs(u[2]-v[2]) < 0.001 && std::abs(u[3]-v[3]) < 0.001 || (out("dvec4 different! u=", u, " "), out("v=", v), false))
 
 void out(const char* msg, const glm::dvec4& v, const char* cont = "\n") {
     std::cout << msg << "(" << v[0] << "," << v[1] << "," << v[2] << "," << v[3] << ")" << cont;
@@ -76,7 +76,7 @@ void mixer_test() {
     for (int i = 0; i < ROTOR_NUM; i++) {
         double ratio = delta_omega[i]/omega0; 
 
-        if (std::fabs(ratio) >  0.8) {
+        if (std::abs(ratio) >  0.8) {
             if (debugEnabled) {
                     std::cout << "WARNING: thrust:" << thrust << " tx: " << torque_x << " ty: " << torque_y << " tz: " << torque_z << std::endl;
                     std::cout << "WARNING: delta_omega("<< i << ") is too big(" << delta_omega[i]/omega0 << "%)...limited to 80%." << std::endl;

drone_physics/utest.cpp

@@ -4,6 +4,8 @@
 
 using namespace hako::drone_physics;
 
+#undef NDEBUG // force assert to work even if release bulid.
+
 const double PI = M_PI;
 
 void test_frame_all_unit_vectors_with_angle0() {
@@ -45,18 +47,18 @@ void test_frame_matrix_is_unitary() {
     for (int i = -180; i < 180; i+=30) {
         VelocityType v2 = ground_vector_from_body(v1, EulerType{i * (PI/180), 0, 0});
         double len = length_squared(v2);
-        assert(fabs(len - 1.0) < 0.0001);
+        assert(std::abs(len - 1.0) < 0.0001);
     }
     for (int i = 0; i < 90; i+=30) {
         VelocityType v2 = ground_vector_from_body(v1, EulerType{0, i * (PI/180), 0});
         double len = length_squared(v2);
-        assert(fabs(len - 1.0) < 0.0001);
+        assert(std::abs(len - 1.0) < 0.0001);
     }
 
     for (int i = -180; i < 360; i+=30) {
         VelocityType v2 = ground_vector_from_body(v1, EulerType{0, 0, i * (PI/180)});
         double len = length_squared(v2);
-        assert(fabs(len - 1.0) < 0.0001);
+        assert(std::abs(len - 1.0) < 0.0001);
     }
 
     // conbinations
@@ -67,16 +69,16 @@ void test_frame_matrix_is_unitary() {
             for (int k = -180; k < 180; k+=30) {
                 VelocityType V = ground_vector_from_body(v1, EulerType{i * (PI/180), j * (PI/180), k * (PI/180)});
                 double len = length_squared(V);
-                assert(fabs(len - 1.0) < 0.0001);
+                assert(std::abs(len - 1.0) < 0.0001);
 
                 // bug #89 indicated that need testing (0,1,0) and (0,0,1) vectors.
                 V = ground_vector_from_body(u1, EulerType{i * (PI/180), j * (PI/180), k * (PI/180)});
                 len = length_squared(V);
-                assert(fabs(len - 1.0) < 0.0001);
+                assert(std::abs(len - 1.0) < 0.0001);
 
                 V = ground_vector_from_body(w1, EulerType{i * (PI/180), j * (PI/180), k * (PI/180)});
                 len = length_squared(V);
-                assert(fabs(len - 1.0) < 0.0001);
+                assert(std::abs(len - 1.0) < 0.0001);
 
             }
         }
@@ -329,7 +331,7 @@ void test_ground_angular_acceleration()
 void test_rotor_omega_acceleration() {
     double Kr = 2896, Tr = 4.5e-2, duty_rate = 1, omega = 0;
     double a = rotor_omega_acceleration(Kr, Tr, omega, duty_rate);
-    assert(std::fabs(a - (Kr * duty_rate - omega) / Tr) < 0.0001);
+    assert(std::abs(a - (Kr * duty_rate - omega) / Tr) < 0.0001);
 
     std::ofstream ofs;
     ofs.open("omega_times_series.csv", std::ios::out);
@@ -349,7 +351,7 @@ void test_rotor_omega_acceleration() {
         double a = rotor_omega_acceleration(Kr, Tr, omega_a, duty_rate);
         double b = rotor_omega_acceleration(Vbat, R, Cq, J, K, D, omega_b, duty_rate);
 
-        assert(std::fabs(a - (Kr * duty_rate - omega_a) / Tr) < 0.0001);
+        assert(std::abs(a - (Kr * duty_rate - omega_a) / Tr) < 0.0001);
         omega_a += a*time;
         omega_b += b*time;
 
@@ -358,7 +360,7 @@ void test_rotor_omega_acceleration() {
          *  if duty_rate is a constant, the solution is
          *  Omega = Kr * (1 - exp(-t/Tr) * (duty rate))
          */
-//        assert(std::fabs(omega_a -  Kr*(1 - std::exp(-time/Tr)*duty_rate) < 10));
+//        assert(std::std::abs(omega_a -  Kr*(1 - std::exp(-time/Tr)*duty_rate) < 10));
         ofs << time << "," << a*time << "," << omega_a << ", " << b*time << ", " << omega_b << std::endl;
     }
     ofs.close();