Bullet: Update to svn r2719

Fixes part of T37905, fixed constraint didn't work correctly.

4 files changed, 272 insertions, 207 deletions

diff --git a/extern/bullet2/src/LinearMath/btMatrixX.h b/extern/bullet2/src/LinearMath/btMatrixX.h
index 1c29632c536..865d77967a9 100644
--- a/extern/bullet2/src/LinearMath/btMatrixX.h
+++ b/extern/bullet2/src/LinearMath/btMatrixX.h
@@ -20,6 +20,12 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
+//#define BT_DEBUG_OSTREAM
+#ifdef BT_DEBUG_OSTREAM
+#include <iostream>
+#include <iomanip>      // std::setw
+#endif //BT_DEBUG_OSTREAM
+
 class btIntSortPredicate
 {
 	public:
@@ -30,6 +36,121 @@ class btIntSortPredicate
 };
 
 
+template <typename T>
+struct btVectorX
+{
+	btAlignedObjectArray<T>	m_storage;
+	
+	btVectorX()
+	{
+	}
+	btVectorX(int numRows)
+	{
+		m_storage.resize(numRows);
+	}
+	
+	void resize(int rows)
+	{
+		m_storage.resize(rows);
+	}
+	int cols() const
+	{
+		return 1;
+	}
+	int rows() const
+	{
+		return m_storage.size();
+	}
+	int size() const
+	{
+		return rows();
+	}
+	
+	T nrm2() const
+	{
+		T norm = T(0);
+		
+		int nn = rows();
+		
+		{
+			if (nn == 1)
+			{
+				norm = btFabs((*this)[0]);
+			}
+			else
+			{
+				T scale = 0.0;
+				T ssq = 1.0;
+				
+				/* The following loop is equivalent to this call to the LAPACK
+				 auxiliary routine:   CALL SLASSQ( N, X, INCX, SCALE, SSQ ) */
+				
+				for (int ix=0;ix<nn;ix++)
+				{
+					if ((*this)[ix] != 0.0)
+					{
+						T absxi = btFabs((*this)[ix]);
+						if (scale < absxi)
+						{
+							T temp;
+							temp = scale / absxi;
+							ssq = ssq * (temp * temp) + 1.0;
+							scale = absxi;
+						}
+						else
+						{
+							T temp;
+							temp = absxi / scale;
+							ssq += temp * temp;
+						}
+					}
+				}
+				norm = scale * sqrt(ssq);
+			}
+		}
+		return norm;
+		
+	}
+	void	setZero()
+	{
+		if (m_storage.size())
+		{
+			//	for (int i=0;i<m_storage.size();i++)
+			//		m_storage[i]=0;
+			//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
+			btSetZero(&m_storage[0],m_storage.size());
+		}
+	}
+	const T& operator[] (int index) const
+	{
+		return m_storage[index];
+	}
+	
+	T& operator[] (int index)
+	{
+		return m_storage[index];
+	}
+	
+	T* getBufferPointerWritable()
+	{
+		return m_storage.size() ? &m_storage[0] : 0;
+	}
+	
+	const T* getBufferPointer() const
+	{
+		return m_storage.size() ? &m_storage[0] : 0;
+	}
+	
+};
+/*
+ template <typename T>
+ void setElem(btMatrixX<T>& mat, int row, int col, T val)
+ {
+ mat.setElem(row,col,val);
+ }
+ */
+
+
 template <typename T> 
 struct btMatrixX
 {
@@ -40,8 +161,7 @@ struct btMatrixX
 	int m_setElemOperations;
 
 	btAlignedObjectArray<T>	m_storage;
-	btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
-	btAlignedObjectArray< btAlignedObjectArray<int> > m_colNonZeroElements;
+	mutable btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
 
 	T* getBufferPointerWritable() 
 	{
@@ -78,7 +198,6 @@ struct btMatrixX
 			BT_PROFILE("m_storage.resize");
 			m_storage.resize(rows*cols);
 		}
-		clearSparseInfo();
 	}
 	int cols() const
 	{
@@ -109,49 +228,44 @@ struct btMatrixX
 		}
 	}
 	
+	
+	void setElem(int row,int col, T val)
+	{
+		m_setElemOperations++;
+		m_storage[row*m_cols+col] = val;
+	}
+	
+	void mulElem(int row,int col, T val)
+	{
+		m_setElemOperations++;
+		//mul doesn't change sparsity info
+
+		m_storage[row*m_cols+col] *= val;
+	}
+	
+	
+	
+	
 	void copyLowerToUpperTriangle()
 	{
 		int count=0;
-		for (int row=0;row<m_rowNonZeroElements1.size();row++)
+		for (int row=0;row<rows();row++)
 		{
-			for (int j=0;j<m_rowNonZeroElements1[row].size();j++)
+			for (int col=0;col<row;col++)
 			{
-				int col = m_rowNonZeroElements1[row][j];
 				setElem(col,row, (*this)(row,col));
 				count++;
-
+				
 			}
 		}
 		//printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
 	}
-	void setElem(int row,int col, T val)
-	{
-		m_setElemOperations++;
-		if (val)
-		{
-			if (m_storage[col+row*m_cols]==0.f)
-			{
-				m_rowNonZeroElements1[row].push_back(col);
-				m_colNonZeroElements[col].push_back(row);
-			}
-			m_storage[row*m_cols+col] = val;
-		}
-	}
+	
 	const T& operator() (int row,int col) const
 	{
 		return m_storage[col+row*m_cols];
 	}
 
-	void clearSparseInfo()
-	{
-		BT_PROFILE("clearSparseInfo=0");
-		m_rowNonZeroElements1.resize(m_rows);
-		m_colNonZeroElements.resize(m_cols);
-		for (int i=0;i<m_rows;i++)
-			m_rowNonZeroElements1[i].resize(0);
-		for (int j=0;j<m_cols;j++)
-			m_colNonZeroElements[j].resize(0);
-	}
 
 	void setZero()
 	{
@@ -162,12 +276,21 @@ struct btMatrixX
 			//for (int i=0;i<m_storage.size();i++)
 	//			m_storage[i]=0;
 		}
+	}
+	
+	void setIdentity()
+	{
+		btAssert(rows() == cols());
+		
+		setZero();
+		for (int row=0;row<rows();row++)
 		{
-			BT_PROFILE("clearSparseInfo=0");
-			clearSparseInfo();
+			setElem(row,row,1);
 		}
 	}
 
+	
+
 	void	printMatrix(const char* msg)
 	{
 		printf("%s ---------------------\n",msg);
@@ -182,28 +305,9 @@ struct btMatrixX
 		printf("\n---------------------\n");
 
 	}
-	void	printNumZeros(const char* msg)
-	{
-		printf("%s: ",msg);
-		int numZeros = 0;
-		for (int i=0;i<m_storage.size();i++)
-			if (m_storage[i]==0)
-				numZeros++;
-		int total = m_cols*m_rows;
-		int computedNonZero = total-numZeros;
-		int nonZero = 0;
-		for (int i=0;i<m_colNonZeroElements.size();i++)
-			nonZero += m_colNonZeroElements[i].size();
-		btAssert(computedNonZero==nonZero);
-		if(computedNonZero!=nonZero)
-		{
-			printf("Error: computedNonZero=%d, but nonZero=%d\n",computedNonZero,nonZero);
-		}
-		//printf("%d numZeros out of %d (%f)\n",numZeros,m_cols*m_rows,numZeros/(m_cols*m_rows));
-		printf("total %d, %d rows, %d cols, %d non-zeros (%f %)\n", total, rows(),cols(), nonZero,100.f*(T)nonZero/T(total));
-	}
-	/*
-	void rowComputeNonZeroElements()
+
+
+	void rowComputeNonZeroElements() const
 	{
 		m_rowNonZeroElements1.resize(rows());
 		for (int i=0;i<rows();i++)
@@ -218,13 +322,11 @@ struct btMatrixX
 			}
 		}
 	}
-	*/
 	btMatrixX transpose() const
 	{
 		//transpose is optimized for sparse matrices
 		btMatrixX tr(m_cols,m_rows);
 		tr.setZero();
-#if 0
 		for (int i=0;i<m_cols;i++)
 			for (int j=0;j<m_rows;j++)
 			{
@@ -234,37 +336,13 @@ struct btMatrixX
 					tr.setElem(i,j,v);
 				}
 			}
-#else		
-		for (int i=0;i<m_colNonZeroElements.size();i++)
-			for (int h=0;h<m_colNonZeroElements[i].size();h++)
-			{
-				int j = m_colNonZeroElements[i][h];
-				T v = (*this)(j,i);
-				tr.setElem(i,j,v);
-			}
-#endif
 		return tr;
 	}
 
-	void sortRowIndexArrays()
-	{
-		for (int i=0;i<m_rowNonZeroElements1[i].size();i++)
-		{
-			m_rowNonZeroElements1[i].quickSort(btIntSortPredicate());
-		}
-	}
-
-	void sortColIndexArrays()
-	{
-		for (int i=0;i<m_colNonZeroElements[i].size();i++)
-		{
-			m_colNonZeroElements[i].quickSort(btIntSortPredicate());
-		}
-	}
 
 	btMatrixX operator*(const btMatrixX& other)
 	{
-		//btMatrixX*btMatrixX implementation, optimized for sparse matrices
+		//btMatrixX*btMatrixX implementation, brute force
 		btAssert(cols() == other.rows());
 
 		btMatrixX res(rows(),other.cols());
@@ -272,76 +350,18 @@ struct btMatrixX
 //		BT_PROFILE("btMatrixX mul");
 		for (int j=0; j < res.cols(); ++j)
 		{
-			//int numZero=other.m_colNonZeroElements[j].size();
-			//if (numZero)
 			{
 				for (int i=0; i < res.rows(); ++i)
-				//for (int g = 0;g<m_colNonZeroElements[j].size();g++)
 				{
 					T dotProd=0;
 					T dotProd2=0;
 					int waste=0,waste2=0;
 
-					bool doubleWalk = false;
-					if (doubleWalk)
-					{
-						int numRows = m_rowNonZeroElements1[i].size();
-						int numOtherCols = other.m_colNonZeroElements[j].size();
-						for (int ii=0;ii<numRows;ii++)
-						{
-							int vThis=m_rowNonZeroElements1[i][ii];
-						}
-
-						for (int ii=0;ii<numOtherCols;ii++)
-						{
-							int vOther = other.m_colNonZeroElements[j][ii];
-						}
-
-
-						int indexRow = 0;
-						int indexOtherCol = 0;
-						while (indexRow < numRows && indexOtherCol < numOtherCols)
-						{
-							int vThis=m_rowNonZeroElements1[i][indexRow];
-							int vOther = other.m_colNonZeroElements[j][indexOtherCol];
-							if (vOther==vThis)
-							{
-								dotProd += (*this)(i,vThis) * other(vThis,j);
-							}
-							if (vThis<vOther)
-							{
-								indexRow++;
-							} else
-							{
-								indexOtherCol++;
-							}
-						}
-
-					} else
 					{
 						bool useOtherCol = true;
-						if (other.m_colNonZeroElements[j].size() <m_rowNonZeroElements1[i].size())
-						{
-						useOtherCol=true;
-						}
-						if (!useOtherCol )
 						{
-							for (int q=0;q<other.m_colNonZeroElements[j].size();q++)
+							for (int v=0;v<rows();v++)
 							{
-								int v = other.m_colNonZeroElements[j][q];
-								T w = (*this)(i,v);
-								if (w!=0.f)
-								{
-									dotProd+=w*other(v,j);
-								}
-						
-							}
-						}
-						else
-						{
-							for (int q=0;q<m_rowNonZeroElements1[i].size();q++)
-							{
-								int v=m_rowNonZeroElements1[i][q];
 								T w = (*this)(i,v);
 								if (other(v,j)!=0.f)
 								{
@@ -404,73 +424,61 @@ struct btMatrixX
 			bb += 8;
 		}
 	}
-
-};
-
-template <typename T> 
-struct btVectorX
-{
-	btAlignedObjectArray<T>	m_storage;
-
-	btVectorX()
-	{
-	}
-	btVectorX(int numRows)
-	{
-		m_storage.resize(numRows);
-	}
-
-	void resize(int rows)
-	{
-		m_storage.resize(rows);
-	}
-	int cols() const
-	{
-		return 1;
-	}
-	int rows() const
-	{
-		return m_storage.size();
-	}
-	int size() const
-	{
-		return rows();
-	}
-	void	setZero()
-	{
-	//	for (int i=0;i<m_storage.size();i++)
-	//		m_storage[i]=0;
-		//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
-		btSetZero(&m_storage[0],m_storage.size());
-	}
-	const T& operator[] (int index) const
+	
+	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const T value)
 	{
-		return m_storage[index];
+		int numRows = rowend+1-rowstart;
+		int numCols = colend+1-colstart;
+		
+		for (int row=0;row<numRows;row++)
+		{
+			for (int col=0;col<numCols;col++)
+			{
+				setElem(rowstart+row,colstart+col,value);
+			}
+		}
 	}
-
-	T& operator[] (int index)
+	
+	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btMatrixX& block)
 	{
-		return m_storage[index];
+		btAssert(rowend+1-rowstart == block.rows());
+		btAssert(colend+1-colstart == block.cols());
+		for (int row=0;row<block.rows();row++)
+		{
+			for (int col=0;col<block.cols();col++)
+			{
+				setElem(rowstart+row,colstart+col,block(row,col));
+			}
+		}
 	}
-
-	T* getBufferPointerWritable() 
+	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btVectorX<T>& block)
 	{
-		return m_storage.size() ? &m_storage[0] : 0;
+		btAssert(rowend+1-rowstart == block.rows());
+		btAssert(colend+1-colstart == block.cols());
+		for (int row=0;row<block.rows();row++)
+		{
+			for (int col=0;col<block.cols();col++)
+			{
+				setElem(rowstart+row,colstart+col,block[row]);
+			}
+		}
 	}
-
-	const T* getBufferPointer() const
+	
+	
+	btMatrixX negative()
 	{
-		return m_storage.size() ? &m_storage[0] : 0;
+		btMatrixX neg(rows(),cols());
+		for (int i=0;i<rows();i++)
+			for (int j=0;j<cols();j++)
+			{
+				T v = (*this)(i,j);
+				neg.setElem(i,j,-v);
+			}
+		return neg;
 	}
 
 };
-/*
-template <typename T> 
-void setElem(btMatrixX<T>& mat, int row, int col, T val)
-{
-	mat.setElem(row,col,val);
-}
-*/
+
 
 
 typedef btMatrixX<float> btMatrixXf;
@@ -480,6 +488,47 @@ typedef btMatrixX<double> btMatrixXd;
 typedef btVectorX<double> btVectorXd;
 
 
+#ifdef BT_DEBUG_OSTREAM
+template <typename T> 
+std::ostream& operator<< (std::ostream& os, const btMatrixX<T>& mat)
+	{
+		
+		os << " [";
+		//printf("%s ---------------------\n",msg);
+		for (int i=0;i<mat.rows();i++)
+		{
+			for (int j=0;j<mat.cols();j++)
+			{
+				os << std::setw(12) << mat(i,j);
+			}
+			if (i!=mat.rows()-1)
+				os << std::endl << "  ";
+		}
+		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(12) << mat[i];
+			if (i!=mat.rows()-1)
+				os << std::endl << "  ";
+		}
+		os << " ]";
+		//printf("\n---------------------\n");
+
+		return os;
+	}
+
+#endif //BT_DEBUG_OSTREAM
+
 
 inline void setElem(btMatrixXd& mat, int row, int col, double val)
 {
diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h
index 37c6dec191d..401e11eaaa8 100644
--- a/extern/bullet2/src/LinearMath/btScalar.h
+++ b/extern/bullet2/src/LinearMath/btScalar.h
@@ -284,6 +284,10 @@ static int btNanMask = 0x7F800001;
 #ifndef BT_INFINITY
 static  int btInfinityMask = 0x7F800000;
 #define BT_INFINITY (*(float*)&btInfinityMask)
+inline int btGetInfinityMask()//suppress stupid compiler warning
+{
+	return btInfinityMask;
+}
 #endif
 
 //use this, in case there are clashes (such as xnamath.h)
@@ -336,6 +340,10 @@ inline __m128 operator * (const __m128 A, const __m128 B)
 	#ifndef BT_INFINITY
 	static  int btInfinityMask = 0x7F800000;
 	#define BT_INFINITY (*(float*)&btInfinityMask)
+	inline int btGetInfinityMask()//suppress stupid compiler warning
+	{
+		return btInfinityMask;
+	}
 	#endif
 #endif//BT_USE_NEON
 
@@ -432,7 +440,7 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
 #endif
 
 #define SIMD_PI           btScalar(3.1415926535897932384626433832795029)
-#define SIMD_2_PI         btScalar(2.0) * SIMD_PI
+#define SIMD_2_PI         (btScalar(2.0) * SIMD_PI)
 #define SIMD_HALF_PI      (SIMD_PI * btScalar(0.5))
 #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
 #define SIMD_DEGS_PER_RAD  (btScalar(360.0) / SIMD_2_PI)
@@ -728,4 +736,5 @@ template <typename T>T* btAlignPointer(T* unalignedPtr, size_t alignment)
 	return converter.ptr;
 }
 
+
 #endif //BT_SCALAR_H
diff --git a/extern/bullet2/src/LinearMath/btVector3.cpp b/extern/bullet2/src/LinearMath/btVector3.cpp
index a84f7baee2e..2ba7029c9be 100644
--- a/extern/bullet2/src/LinearMath/btVector3.cpp
+++ b/extern/bullet2/src/LinearMath/btVector3.cpp
@@ -821,6 +821,7 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa
 
 
 #elif defined BT_USE_NEON
+
 #define ARM_NEON_GCC_COMPATIBILITY  1
 #include <arm_neon.h>
 #include <sys/types.h>
@@ -884,7 +885,12 @@ static long _mindot_large_sel( const float *vv, const float *vec, unsigned long
 
 
 
-#define vld1q_f32_aligned_postincrement( _ptr ) ({ float32x4_t _r; asm( "vld1.f32  {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; })
+#if defined __arm__
+# define vld1q_f32_aligned_postincrement( _ptr ) ({ float32x4_t _r; asm( "vld1.f32 {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; })
+#else
+//support 64bit arm
+# define vld1q_f32_aligned_postincrement( _ptr) ({ float32x4_t _r = ((float32x4_t*)(_ptr))[0]; (_ptr) = (const float*) ((const char*)(_ptr) + 16L); /*return*/ _r; })
+#endif
 
 
 long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult )
diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h
index 89685929288..112b70dd66b 100644
--- a/extern/bullet2/src/LinearMath/btVector3.h
+++ b/extern/bullet2/src/LinearMath/btVector3.h
@@ -297,7 +297,7 @@ public:
 	SIMD_FORCE_INLINE btVector3& normalize() 
 	{
 		
-		btAssert(length() != btScalar(0));
+		btAssert(!fuzzyZero());
 
 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)		
         // dot product first
@@ -685,9 +685,10 @@ public:
 		return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
 	}
 
+
 	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;
@@ -950,9 +951,9 @@ SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
 
 SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
 {
-	btVector3 norm = *this;
+	btVector3 nrm = *this;
 
-	return norm.normalize();
+	return nrm.normalize();
 } 
 
 SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const
@@ -1010,21 +1011,21 @@ SIMD_FORCE_INLINE   long    btVector3::maxDot( const btVector3 *array, long arra
     if( array_count < scalar_cutoff )	
 #endif
     {
-        btScalar maxDot = -SIMD_INFINITY;
+        btScalar maxDot1 = -SIMD_INFINITY;
         int i = 0;
         int ptIndex = -1;
         for( i = 0; i < array_count; i++ )
         {
             btScalar dot = array[i].dot(*this);
             
-            if( dot > maxDot )
+            if( dot > maxDot1 )
             {
-                maxDot = dot;
+                maxDot1 = dot;
                 ptIndex = i;
             }
         }
         
-        dotOut = maxDot;
+        dotOut = maxDot1;
         return ptIndex;
     }
 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)