From 4169e082156357b5cd95da01f6a2a717bb7134fe Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 5 Oct 2010 13:57:00 +0000
Subject: [PATCH] added tensor2 norm and fixed equivalent

---
 geometry/Tensor1Expression.h |  3 ++-
 geometry/Tensor2Expression.h | 15 +++++++++++----
 2 files changed, 13 insertions(+), 5 deletions(-)

diff --git a/geometry/Tensor1Expression.h b/geometry/Tensor1Expression.h
index ef651e0df..138e40c86 100644
--- a/geometry/Tensor1Expression.h
+++ b/geometry/Tensor1Expression.h
@@ -73,7 +73,8 @@ namespace tensors {
 
 		template<class B>
 		bool equivalent(const Tensor1Expression<B, I> & q, double tol = 1e-9) const {
-			return ((*this) * (1.0 / norm())).equals(q * (1.0 / q.norm()), tol);
+			return ((*this) * (1.0 / norm())).equals(q * (1.0 / q.norm()), tol)
+			|| ((*this) * (-1.0 / norm())).equals(q * (1.0 / q.norm()), tol);
 		}
 
 		/** Check if two expressions are equal */
diff --git a/geometry/Tensor2Expression.h b/geometry/Tensor2Expression.h
index e0c9d661e..159cf85ff 100644
--- a/geometry/Tensor2Expression.h
+++ b/geometry/Tensor2Expression.h
@@ -133,12 +133,19 @@ namespace tensors {
 			return true;
 		}
 
-		// TODO: broken !!!! Should not treat Tensor1's separately: one scale only!
+		/** norm */
+		double norm() const {
+			double sumsqr = 0.0;
+			for (int i = 0; i < I::dim; i++)
+				for (int j = 0; j < J::dim; j++)
+					sumsqr += iter(i,j) * iter(i,j);
+			return sqrt(sumsqr);
+		}
+
 		template<class B>
 		bool equivalent(const Tensor2Expression<B, I, J> & q, double tol) const {
-			for (int j = 0; j < J::dim; j++)
-				if (!(*this)(j).equivalent(q(j), tol)) return false;
-			return true;
+			return ((*this) * (1.0 / norm())).equals(q * (1.0 / q.norm()), tol)
+			|| ((*this) * (-1.0 / norm())).equals(q * (1.0 / q.norm()), tol);
 		}
 
 		/** element access */