AP_Math: move zeroing to header, use memset, reuse in identity

this method is in ITCM memory on STM32 - which makes small optimisations worthwhile
ArduPilot · Nov 4, 2024 · dc62483 · dc62483
1 parent 4e0930a
commit dc62483
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 12 deletions.
diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp
@@ -227,14 +227,6 @@ bool Matrix3<T>::invert()
     return success;
 }
 
-template <typename T>
-void Matrix3<T>::zero(void)
-{
-    a.x = a.y = a.z = 0;
-    b.x = b.y = b.z = 0;
-    c.x = c.y = c.z = 0;
-}
-
 // create rotation matrix for rotation about the vector v by angle theta
 // See: http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/
 template <typename T>

diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h
@@ -219,14 +219,14 @@ class Matrix3 {
     bool invert() WARN_IF_UNUSED;
 
     // zero the matrix
-    void        zero(void);
+    void        zero(void) {
+        memset((void*)this, 0, sizeof(*this));
+    }
 
     // setup the identity matrix
     void        identity(void) {
+        zero();
         a.x = b.y = c.z = 1;
-        a.y = a.z = 0;
-        b.x = b.z = 0;
-        c.x = c.y = 0;
     }
 
     // check if any elements are NAN