Fix occasional fusion divergence by detecting it and resetting the fusion.
Change-Id: I51186e12fb9b2316e3671e3908174f4495df89a0
diff --git a/services/sensorservice/mat.h b/services/sensorservice/mat.h
index 1302ca3..a76fc91 100644
--- a/services/sensorservice/mat.h
+++ b/services/sensorservice/mat.h
@@ -295,6 +295,29 @@
return r;
}
+// Calculate the trace of a matrix
+template <typename TYPE, size_t C> static TYPE trace(const mat<TYPE, C, C>& m) {
+ TYPE t;
+ for (size_t i=0 ; i<C ; i++)
+ t += m[i][i];
+ return t;
+}
+
+// Test positive-semidefiniteness of a matrix
+template <typename TYPE, size_t C>
+static bool isPositiveSemidefinite(const mat<TYPE, C, C>& m, TYPE tolerance) {
+ for (size_t i=0 ; i<C ; i++)
+ if (m[i][i] < 0)
+ return false;
+
+ for (size_t i=0 ; i<C ; i++)
+ for (size_t j=i+1 ; j<C ; j++)
+ if (fabs(m[i][j] - m[j][i]) > tolerance)
+ return false;
+
+ return true;
+}
+
// Transpose a vector
template <
template<typename T, size_t S> class VEC,