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;