From dc62483e0c4782c97217d09f10980abf79969c50 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 17 Oct 2024 13:33:22 +1100 Subject: [PATCH] AP_Math: move zeroing to header, use memset, reuse in identity this method is in ITCM memory on STM32 - which makes small optimisations worthwhile --- libraries/AP_Math/matrix3.cpp | 8 -------- libraries/AP_Math/matrix3.h | 8 ++++---- 2 files changed, 4 insertions(+), 12 deletions(-) diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp index 0dfd75032b..ccfcb7bc01 100644 --- a/libraries/AP_Math/matrix3.cpp +++ b/libraries/AP_Math/matrix3.cpp @@ -227,14 +227,6 @@ bool Matrix3::invert() return success; } -template -void Matrix3::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 diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index 02e18699a5..77ed088b8c 100644 --- a/libraries/AP_Math/matrix3.h +++ b/libraries/AP_Math/matrix3.h @@ -219,14 +219,14 @@ public: 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