From 13936eb9a5f7ff15c1d3a40e9b25a2cceb4af74b Mon Sep 17 00:00:00 2001
From: "erwin.coumans@gmail.com"
 <erwin.coumans@gmail.com@08e121b0-ae19-0410-a57b-3be3395fd4fd>
Date: Tue, 29 Oct 2013 22:53:01 +0000
Subject: [PATCH] use square epsilon in btVector3::fuzzyZero (length2)

---
 src/LinearMath/btMatrixX.h | 21 +++++++++++++++++++--
 src/LinearMath/btVector3.h |  2 +-
 2 files changed, 20 insertions(+), 3 deletions(-)

diff --git a/src/LinearMath/btMatrixX.h b/src/LinearMath/btMatrixX.h
index 7d7500788..52cc79b3a 100644
--- a/src/LinearMath/btMatrixX.h
+++ b/src/LinearMath/btMatrixX.h
@@ -22,7 +22,7 @@ subject to the following restrictions:
 
 //#define BT_DEBUG_OSTREAM
 #ifdef BT_DEBUG_OSTREAM
-#include <ostream>
+#include <iostream>
 #include <iomanip>      // std::setw
 #endif //BT_DEBUG_OSTREAM
 
@@ -616,7 +616,7 @@ typedef btVectorX<double> btVectorXd;
 
 #ifdef BT_DEBUG_OSTREAM
 template <typename T> 
-std::ostream& operator<< (std::ostream& os, btMatrixX<T>& mat)
+std::ostream& operator<< (std::ostream& os, const btMatrixX<T>& mat)
 	{
 		
 		os << " [";
@@ -633,6 +633,23 @@ std::ostream& operator<< (std::ostream& os, btMatrixX<T>& mat)
 		os << " ]";
 		//printf("\n---------------------\n");
 
+		return os;
+	}
+template <typename T> 
+std::ostream& operator<< (std::ostream& os, const btVectorX<T>& mat)
+	{
+		
+		os << " [";
+		//printf("%s ---------------------\n",msg);
+		for (int i=0;i<mat.rows();i++)
+		{
+				os << std::setw(10) << mat[i];
+			if (i!=mat.rows()-1)
+				os << std::endl << "  ";
+		}
+		os << " ]";
+		//printf("\n---------------------\n");
+
 		return os;
 	}
 
diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h
index 8fe887476..112b70dd6 100644
--- a/src/LinearMath/btVector3.h
+++ b/src/LinearMath/btVector3.h
@@ -688,7 +688,7 @@ public:
 
 	SIMD_FORCE_INLINE bool fuzzyZero() const 
 	{
-		return length2() < SIMD_EPSILON;
+		return length2() < SIMD_EPSILON*SIMD_EPSILON;
 	}
 
 	SIMD_FORCE_INLINE	void	serialize(struct	btVector3Data& dataOut) const;