From eef784fbe6e9578447b48943027ac02334ee98ec Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Thu, 12 Nov 2015 19:41:35 -0200
Subject: [PATCH] AP_Math: add benchmark for matrix multiplication

---
 .../AP_Math/benchmarks/benchmark_matrix.cpp   | 22 +++++++++++++++++++
 libraries/AP_Math/benchmarks/wscript          | 10 +++++++++
 2 files changed, 32 insertions(+)
 create mode 100644 libraries/AP_Math/benchmarks/benchmark_matrix.cpp
 create mode 100644 libraries/AP_Math/benchmarks/wscript

diff --git a/libraries/AP_Math/benchmarks/benchmark_matrix.cpp b/libraries/AP_Math/benchmarks/benchmark_matrix.cpp
new file mode 100644
index 0000000000..1c4d5670be
--- /dev/null
+++ b/libraries/AP_Math/benchmarks/benchmark_matrix.cpp
@@ -0,0 +1,22 @@
+#include <AP_gbenchmark.h>
+
+#include <AP_Math/AP_Math.h>
+
+static void BM_MatrixMultiplication(benchmark::State& state)
+{
+    Matrix3f m1(Vector3f(1.0f, 2.0f, 3.0f),
+                Vector3f(4.0f, 5.0f, 6.0f),
+                Vector3f(7.0f, 8.0f, 9.0f));
+    Matrix3f m2(Vector3f(1.0f, 2.0f, 3.0f),
+                Vector3f(4.0f, 5.0f, 6.0f),
+                Vector3f(7.0f, 8.0f, 9.0f));
+
+    while (state.KeepRunning()) {
+        Matrix3f m3 = m1 * m2;
+        gbenchmark_escape(&m3);
+    }
+}
+
+BENCHMARK(BM_MatrixMultiplication);
+
+BENCHMARK_MAIN()
diff --git a/libraries/AP_Math/benchmarks/wscript b/libraries/AP_Math/benchmarks/wscript
new file mode 100644
index 0000000000..37a6f3f1f8
--- /dev/null
+++ b/libraries/AP_Math/benchmarks/wscript
@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+import ardupilotwaf
+
+def build(bld):
+    ardupilotwaf.find_benchmarks(
+        bld,
+        use='ap',
+    )