AP_Math: fixed build of vectorN class on PX4

libraries/AP_Math/math_assert.h

@@ -1,16 +0,0 @@
-/*
-  this can be used to check array indexes in vectors and matrices
- */
-
-#ifndef MATH_ASSERT_H
-#define MATH_ASSERT_H
-
-#if MATH_CHECK_INDEXES
-#include <assert.h>
-#define ASSERT(x) assert(x)
-#else
-#define ASSERT(x) do {} while(0)
-#endif
-
-#endif // MATH_ASSERT_H
-

libraries/AP_Math/matrix3.h

@@ -128,13 +128,17 @@ public:
     // allow a Matrix3 to be used as an array of vectors, 0 indexed
     Vector3<T> & operator[](uint8_t i) {
         Vector3<T> *_v = &a;
-        ASSERT(i >= 0 && i < 3);
+#if MATH_CHECK_INDEXES
+        assert(i >= 0 && i < 3);
+#endif
         return _v[i];
     }
 
     const Vector3<T> & operator[](uint8_t i) const {
         const Vector3<T> *_v = &a;
-        ASSERT(i >= 0 && i < 3);
+#if MATH_CHECK_INDEXES
+        assert(i >= 0 && i < 3);
+#endif
         return _v[i];
     }
 

libraries/AP_Math/quaternion.h

@@ -20,6 +20,9 @@
 #define QUATERNION_H
 
 #include <math.h>
+#if MATH_CHECK_INDEXES
+#include <assert.h>
+#endif
 
 class Quaternion
 {
@@ -64,13 +67,17 @@ public:
     // allow a quaternion to be used as an array, 0 indexed
     float & operator[](uint8_t i) {
         float *_v = &q1;
-        ASSERT(i >= 0 && i < 4);
+#if MATH_CHECK_INDEXES
+        assert(i >= 0 && i < 4);
+#endif
         return _v[i];
     }
 
     const float & operator[](uint8_t i) const {
         const float *_v = &q1;
-        ASSERT(i >= 0 && i < 4);
+#if MATH_CHECK_INDEXES
+        assert(i >= 0 && i < 4);
+#endif
         return _v[i];
     }
 };

libraries/AP_Math/vector3.h

@@ -52,7 +52,10 @@
 
 #include <math.h>
 #include <string.h>
-#include "math_assert.h"
+
+#if MATH_CHECK_INDEXES
+#include <assert.h>
+#endif
 
 template <typename T>
 class Matrix3;
@@ -115,13 +118,17 @@ public:
     // allow a vector3 to be used as an array, 0 indexed
     T & operator[](uint8_t i) {
         T *_v = &x;
-        ASSERT(i >= 0 && i < 3);
+#if MATH_CHECK_INDEXES
+        assert(i >= 0 && i < 3);
+#endif
         return _v[i];
     }
 
     const T & operator[](uint8_t i) const {
         const T *_v = &x;
-        ASSERT(i >= 0 && i < 3);
+#if MATH_CHECK_INDEXES
+        assert(i >= 0 && i < 3);
+#endif
         return _v[i];
     }
 

libraries/AP_Math/vectorN.h

@@ -19,29 +19,35 @@
 
 #include <math.h>
 #include <string.h>
-#include "math_assert.h"
+#if MATH_CHECK_INDEXES
+#include <assert.h>
+#endif
 
 template <typename T, uint8_t N>
 class VectorN
 {
 public:
     // trivial ctor
-    VectorN<T,N>() {
+    inline VectorN<T,N>() {
         memset(_v, 0, sizeof(T)*N);
     }
 
-    T & operator[](uint8_t i) {
-        ASSERT(i >= 0 && i < N);
+    inline T & operator[](uint8_t i) {
+#if MATH_CHECK_INDEXES
+        assert(i >= 0 && i < N);
+#endif
         return _v[i];
     }
 
-    const T & operator[](uint8_t i) const {
-        ASSERT(i >= 0 && i < N);
+    inline const T & operator[](uint8_t i) const {
+#if MATH_CHECK_INDEXES
+        assert(i >= 0 && i < N);
+#endif
         return _v[i];
     }
 
     // zero the vector
-    void zero()
+    inline void zero()
     {
         memset(_v, 0, sizeof(T)*N);
     }