AP_Math: added vectorN class, and index checking

libraries/AP_Math/math_assert.h

@@ -0,0 +1,16 @@
+/*
+  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,6 +128,13 @@ 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);
+        return _v[i];
+    }
+
+    const Vector3<T> & operator[](uint8_t i) const {
+        const Vector3<T> *_v = &a;
+        ASSERT(i >= 0 && i < 3);
         return _v[i];
     }
 

libraries/AP_Math/quaternion.h

@@ -60,5 +60,18 @@ public:
 
     // create eulers from a quaternion
     void        to_euler(float *roll, float *pitch, float *yaw);
+
+    // allow a quaternion to be used as an array, 0 indexed
+    float & operator[](uint8_t i) {
+        float *_v = &q1;
+        ASSERT(i >= 0 && i < 4);
+        return _v[i];
+    }
+
+    const float & operator[](uint8_t i) const {
+        const float *_v = &q1;
+        ASSERT(i >= 0 && i < 4);
+        return _v[i];
+    }
 };
 #endif // QUATERNION_H

libraries/AP_Math/vector3.h

@@ -52,6 +52,7 @@
 
 #include <math.h>
 #include <string.h>
+#include "math_assert.h"
 
 template <typename T>
 class Matrix3;
@@ -114,6 +115,13 @@ 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);
+        return _v[i];
+    }
+
+    const T & operator[](uint8_t i) const {
+        const T *_v = &x;
+        ASSERT(i >= 0 && i < 3);
         return _v[i];
     }
 

libraries/AP_Math/vectorN.h

@@ -0,0 +1,53 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef VECTORN_H
+#define VECTORN_H
+
+#include <math.h>
+#include <string.h>
+#include "math_assert.h"
+
+template <typename T, uint8_t N>
+class VectorN
+{
+public:
+    // trivial ctor
+    VectorN<T,N>() {
+        memset(_v, 0, sizeof(T)*N);
+    }
+
+    T & operator[](uint8_t i) {
+        ASSERT(i >= 0 && i < N);
+        return _v[i];
+    }
+
+    const T & operator[](uint8_t i) const {
+        ASSERT(i >= 0 && i < N);
+        return _v[i];
+    }
+
+    // zero the vector
+    void zero()
+    {
+        memset(_v, 0, sizeof(T)*N);
+    }
+
+private:
+    T _v[N];
+};
+
+#endif // VECTORN_H