From a7543d369f9bffe1367ee866c97cd63f865699ca Mon Sep 17 00:00:00 2001 From: Gustavo Jose de Sousa Date: Mon, 9 May 2016 14:48:08 -0300 Subject: [PATCH] AP_Math: matrix_alg: protect inverseixi() against overflow Fail on inverse3x3() and inverse4x4() if there's float overflow during the determinant calculation. --- libraries/AP_Math/matrix_alg.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Math/matrix_alg.cpp b/libraries/AP_Math/matrix_alg.cpp index 220245a3b2..881f6378ea 100644 --- a/libraries/AP_Math/matrix_alg.cpp +++ b/libraries/AP_Math/matrix_alg.cpp @@ -242,7 +242,7 @@ bool inverse3x3(float m[], float invOut[]) float det = m[0] * (m[4] * m[8] - m[7] * m[5]) - m[1] * (m[3] * m[8] - m[5] * m[6]) + m[2] * (m[3] * m[7] - m[4] * m[6]); - if (is_zero(det)){ + if (is_zero(det) || isinf(det)) { return false; } @@ -393,7 +393,7 @@ bool inverse4x4(float m[],float invOut[]) det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]; - if (is_zero(det)){ + if (is_zero(det) || isinf(det)){ return false; }