Index: code/branches/questsystem5/src/bullet/LinearMath/btAlignedAllocator.cpp
===================================================================
--- code/branches/questsystem5/src/bullet/LinearMath/btAlignedAllocator.cpp	(revision 2777)
+++ code/branches/questsystem5/src/bullet/LinearMath/btAlignedAllocator.cpp	(revision 2907)
@@ -20,4 +20,19 @@
 int gTotalBytesAlignedAllocs = 0;//detect memory leaks
 
+static void *btAllocDefault(size_t size)
+{
+	return malloc(size);
+}
+
+static void btFreeDefault(void *ptr)
+{
+	free(ptr);
+}
+
+static btAllocFunc *sAllocFunc = btAllocDefault;
+static btFreeFunc *sFreeFunc = btFreeDefault;
+
+
+
 #if defined (BT_HAS_ALIGNED_ALLOCATOR)
 #include <malloc.h>
@@ -50,5 +65,5 @@
   unsigned long offset;
 
-  real = (char *)malloc(size + sizeof(void *) + (alignment-1));
+  real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1));
   if (real) {
     offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1);
@@ -67,23 +82,12 @@
   if (ptr) {
     real = *((void **)(ptr)-1);
-    free(real);
+    sFreeFunc(real);
   }
 }
 #endif
 
-static void *btAllocDefault(size_t size)
-{
-	return malloc(size);
-}
-
-static void btFreeDefault(void *ptr)
-{
-	free(ptr);
-}
 
 static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault;
 static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault;
-static btAllocFunc *sAllocFunc = btAllocDefault;
-static btFreeFunc *sFreeFunc = btFreeDefault;
 
 void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc)
Index: code/branches/questsystem5/src/bullet/LinearMath/btAlignedAllocator.h
===================================================================
--- code/branches/questsystem5/src/bullet/LinearMath/btAlignedAllocator.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/LinearMath/btAlignedAllocator.h	(revision 2907)
@@ -39,5 +39,5 @@
 	void	btAlignedFreeInternal	(void* ptr);
 
-	#define btAlignedAlloc(a,b) btAlignedAllocInternal(a,b)
+	#define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment)
 	#define btAlignedFree(ptr) btAlignedFreeInternal(ptr)
 
@@ -50,6 +50,9 @@
 typedef void (btFreeFunc)(void *memblock);
 
+///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom
+void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
+///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
 void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc);
-void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
+
 
 ///The btAlignedAllocator is a portable class for aligned memory allocations.
Index: code/branches/questsystem5/src/bullet/LinearMath/btAlignedObjectArray.h
===================================================================
--- code/branches/questsystem5/src/bullet/LinearMath/btAlignedObjectArray.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/LinearMath/btAlignedObjectArray.h	(revision 2907)
@@ -59,5 +59,5 @@
 			return (size ? size*2 : 1);
 		}
-		SIMD_FORCE_INLINE	void	copy(int start,int end, T* dest)
+		SIMD_FORCE_INLINE	void	copy(int start,int end, T* dest) const
 		{
 			int i;
@@ -121,11 +121,19 @@
 		}
 
-		SIMD_FORCE_INLINE	int capacity() const
-		{	// return current length of allocated storage
-			return m_capacity;
-		}
-		
+		///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead.
+		btAlignedObjectArray(const btAlignedObjectArray& otherArray)
+		{
+			init();
+
+			int otherSize = otherArray.size();
+			resize (otherSize);
+			otherArray.copy(0, otherSize, m_data);
+		}
+
+		
+		
+		/// return the number of elements in the array
 		SIMD_FORCE_INLINE	int size() const
-		{	// return length of sequence
+		{	
 			return m_size;
 		}
@@ -142,4 +150,5 @@
 		
 
+		///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
 		SIMD_FORCE_INLINE	void	clear()
 		{
@@ -157,4 +166,6 @@
 		}
 
+		///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
+		///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
 		SIMD_FORCE_INLINE	void	resize(int newsize, const T& fillData=T())
 		{
@@ -220,4 +231,9 @@
 
 	
+		/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
+		SIMD_FORCE_INLINE	int capacity() const
+		{	
+			return m_capacity;
+		}
 		
 		SIMD_FORCE_INLINE	void reserve(int _Count)
Index: code/branches/questsystem5/src/bullet/LinearMath/btConvexHull.cpp
===================================================================
--- code/branches/questsystem5/src/bullet/LinearMath/btConvexHull.cpp	(revision 2777)
+++ code/branches/questsystem5/src/bullet/LinearMath/btConvexHull.cpp	(revision 2907)
@@ -263,6 +263,6 @@
 		for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
 		{
-			btScalar s = sinf(SIMD_RADS_PER_DEG*(x));
-			btScalar c = cosf(SIMD_RADS_PER_DEG*(x));
+			btScalar s = btSin(SIMD_RADS_PER_DEG*(x));
+			btScalar c = btCos(SIMD_RADS_PER_DEG*(x));
 			int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
 			if(ma==m && mb==m)
@@ -276,6 +276,6 @@
 				for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0))
 				{
-					btScalar s = sinf(SIMD_RADS_PER_DEG*(xx));
-					btScalar c = cosf(SIMD_RADS_PER_DEG*(xx));
+					btScalar s = btSin(SIMD_RADS_PER_DEG*(xx));
+					btScalar c = btCos(SIMD_RADS_PER_DEG*(xx));
 					int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
 					if(mc==m && md==m)
Index: code/branches/questsystem5/src/bullet/LinearMath/btIDebugDraw.h
===================================================================
--- code/branches/questsystem5/src/bullet/LinearMath/btIDebugDraw.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/LinearMath/btIDebugDraw.h	(revision 2907)
@@ -30,4 +30,5 @@
 
 #include "btVector3.h"
+#include "btTransform.h"
 
 
@@ -53,8 +54,23 @@
 		DBG_DisableBulletLCP = 512,
 		DBG_EnableCCD = 1024,
+		DBG_DrawConstraints = (1 << 11),
+		DBG_DrawConstraintLimits = (1 << 12),
 		DBG_MAX_DEBUG_DRAW_MODE
 	};
 
 	virtual ~btIDebugDraw() {};
+
+	virtual void    drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor)
+	{
+		drawLine (from, to, fromColor);
+	}
+
+	virtual void	drawBox (const btVector3& boxMin, const btVector3& boxMax, const btVector3& color, btScalar alpha)
+	{
+	}
+
+	virtual void	drawSphere (const btVector3& p, btScalar radius, const btVector3& color)
+	{
+	}
 
 	virtual void	drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
@@ -110,4 +126,169 @@
 		}
 	}
+	void drawTransform(const btTransform& transform, btScalar orthoLen)
+	{
+		btVector3 start = transform.getOrigin();
+		drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0));
+		drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0));
+		drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f));
+	}
+
+	void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle, 
+				const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f))
+	{
+		const btVector3& vx = axis;
+		btVector3 vy = normal.cross(axis);
+		btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
+		int nSteps = (int)((maxAngle - minAngle) / step);
+		if(!nSteps) nSteps = 1;
+		btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle);
+		if(drawSect)
+		{
+			drawLine(center, prev, color);
+		}
+		for(int i = 1; i <= nSteps; i++)
+		{
+			btScalar angle = minAngle + (maxAngle - minAngle) * btScalar(i) / btScalar(nSteps);
+			btVector3 next = center + radiusA * vx * btCos(angle) + radiusB * vy * btSin(angle);
+			drawLine(prev, next, color);
+			prev = next;
+		}
+		if(drawSect)
+		{
+			drawLine(center, prev, color);
+		}
+	}
+	void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius, 
+		btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f))
+	{
+		btVector3 vA[74];
+		btVector3 vB[74];
+		btVector3 *pvA = vA, *pvB = vB, *pT;
+		btVector3 npole = center + up * radius;
+		btVector3 spole = center - up * radius;
+		btVector3 arcStart;
+		btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
+		const btVector3& kv = up;
+		const btVector3& iv = axis;
+		btVector3 jv = kv.cross(iv);
+		bool drawN = false;
+		bool drawS = false;
+		if(minTh <= -SIMD_HALF_PI)
+		{
+			minTh = -SIMD_HALF_PI + step;
+			drawN = true;
+		}
+		if(maxTh >= SIMD_HALF_PI)
+		{
+			maxTh = SIMD_HALF_PI - step;
+			drawS = true;
+		}
+		if(minTh > maxTh)
+		{
+			minTh = -SIMD_HALF_PI + step;
+			maxTh =  SIMD_HALF_PI - step;
+			drawN = drawS = true;
+		}
+		int n_hor = (int)((maxTh - minTh) / step) + 1;
+		if(n_hor < 2) n_hor = 2;
+		btScalar step_h = (maxTh - minTh) / btScalar(n_hor - 1);
+		bool isClosed = false;
+		if(minPs > maxPs)
+		{
+			minPs = -SIMD_PI + step;
+			maxPs =  SIMD_PI;
+			isClosed = true;
+		}
+		else if((maxPs - minPs) >= SIMD_PI * btScalar(2.f))
+		{
+			isClosed = true;
+		}
+		else
+		{
+			isClosed = false;
+		}
+		int n_vert = (int)((maxPs - minPs) / step) + 1;
+		if(n_vert < 2) n_vert = 2;
+		btScalar step_v = (maxPs - minPs) / btScalar(n_vert - 1);
+		for(int i = 0; i < n_hor; i++)
+		{
+			btScalar th = minTh + btScalar(i) * step_h;
+			btScalar sth = radius * btSin(th);
+			btScalar cth = radius * btCos(th);
+			for(int j = 0; j < n_vert; j++)
+			{
+				btScalar psi = minPs + btScalar(j) * step_v;
+				btScalar sps = btSin(psi);
+				btScalar cps = btCos(psi);
+				pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv;
+				if(i)
+				{
+					drawLine(pvA[j], pvB[j], color);
+				}
+				else if(drawS)
+				{
+					drawLine(spole, pvB[j], color);
+				}
+				if(j)
+				{
+					drawLine(pvB[j-1], pvB[j], color);
+				}
+				else
+				{
+					arcStart = pvB[j];
+				}
+				if((i == (n_hor - 1)) && drawN)
+				{
+					drawLine(npole, pvB[j], color);
+				}
+				if(isClosed)
+				{
+					if(j == (n_vert-1))
+					{
+						drawLine(arcStart, pvB[j], color);
+					}
+				}
+				else
+				{
+					if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1))))
+					{
+						drawLine(center, pvB[j], color);
+					}
+				}
+			}
+			pT = pvA; pvA = pvB; pvB = pT;
+		}
+	}
+	
+	void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color)
+	{
+		drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
+		drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
+		drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
+		drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
+		drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+		drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+		drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+		drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+		drawLine(btVector3(bbMin[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+		drawLine(btVector3(bbMax[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+		drawLine(btVector3(bbMax[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+		drawLine(btVector3(bbMin[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+	}
+	void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color)
+	{
+		drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
+		drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
+		drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
+		drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
+		drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+		drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+		drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+		drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+		drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+		drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+		drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+		drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+	}
 };
 
Index: code/branches/questsystem5/src/bullet/LinearMath/btMatrix3x3.h
===================================================================
--- code/branches/questsystem5/src/bullet/LinearMath/btMatrix3x3.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/LinearMath/btMatrix3x3.h	(revision 2907)
@@ -193,4 +193,13 @@
 					 btScalar(0.0), btScalar(0.0), btScalar(1.0)); 
 		}
+
+		static const btMatrix3x3&	getIdentity()
+		{
+			static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
+					 btScalar(0.0), btScalar(1.0), btScalar(0.0), 
+					 btScalar(0.0), btScalar(0.0), btScalar(1.0));
+			return identityMatrix;
+		}
+
   /**@brief Fill the values of the matrix into a 9 element array 
    * @param m The array to be filled */
Index: code/branches/questsystem5/src/bullet/LinearMath/btQuadWord.h
===================================================================
--- code/branches/questsystem5/src/bullet/LinearMath/btQuadWord.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/LinearMath/btQuadWord.h	(revision 2907)
@@ -25,11 +25,11 @@
 #endif
 
-/**@brief The btQuadWordStorage class is base class for btVector3 and btQuaternion. 
+/**@brief The btQuadWord class is base class for btVector3 and btQuaternion. 
  * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
  */
 #ifndef USE_LIBSPE2
-ATTRIBUTE_ALIGNED16(class) btQuadWordStorage
+ATTRIBUTE_ALIGNED16(class) btQuadWord
 #else
-class btQuadWordStorage
+class btQuadWord
 #endif
 {
@@ -46,13 +46,9 @@
 		return mVec128;
 	}
+protected:
 #else //__CELLOS_LV2__ __SPU__
 	btScalar	m_floats[4];
 #endif //__CELLOS_LV2__ __SPU__
 
-};
-
-/** @brief The btQuadWord is base-class for vectors, points */
-class	btQuadWord : public btQuadWordStorage
-{
 	public:
   
@@ -135,9 +131,5 @@
 		{
 		}
-  /**@brief Copy constructor */
-		SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q)
-		{
-			*((btQuadWordStorage*)this) = q;
-		}
+ 
   /**@brief Three argument constructor (zeros w)
    * @param x Value of x
Index: code/branches/questsystem5/src/bullet/LinearMath/btQuaternion.h
===================================================================
--- code/branches/questsystem5/src/bullet/LinearMath/btQuaternion.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/LinearMath/btQuaternion.h	(revision 2907)
@@ -20,4 +20,5 @@
 
 #include "btVector3.h"
+#include "btQuadWord.h"
 
 /**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
@@ -58,5 +59,5 @@
 	{
 		btScalar d = axis.length();
-		assert(d != btScalar(0.0));
+		btAssert(d != btScalar(0.0));
 		btScalar s = btSin(angle * btScalar(0.5)) / d;
 		setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
@@ -177,5 +178,5 @@
 	btQuaternion operator/(const btScalar& s) const
 	{
-		assert(s != btScalar(0.0));
+		btAssert(s != btScalar(0.0));
 		return *this * (btScalar(1.0) / s);
 	}
@@ -185,5 +186,5 @@
 	btQuaternion& operator/=(const btScalar& s) 
 	{
-		assert(s != btScalar(0.0));
+		btAssert(s != btScalar(0.0));
 		return *this *= btScalar(1.0) / s;
 	}
@@ -199,5 +200,5 @@
 	{
 		btScalar s = btSqrt(length2() * q.length2());
-		assert(s != btScalar(0.0));
+		btAssert(s != btScalar(0.0));
 		return btAcos(dot(q) / s);
 	}
@@ -275,4 +276,10 @@
 	}
 
+	static const btQuaternion&	getIdentity()
+	{
+		static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.));
+		return identityQuat;
+	}
+
 	SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
 
Index: code/branches/questsystem5/src/bullet/LinearMath/btScalar.h
===================================================================
--- code/branches/questsystem5/src/bullet/LinearMath/btScalar.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/LinearMath/btScalar.h	(revision 2907)
@@ -26,5 +26,5 @@
 #include <float.h>
 
-#define BT_BULLET_VERSION 273
+#define BT_BULLET_VERSION 274
 
 inline int	btGetVersion()
@@ -46,5 +46,5 @@
 			#define ATTRIBUTE_ALIGNED128(a) a
 		#else
-			#define BT_HAS_ALIGNED_ALLOCATOR
+			//#define BT_HAS_ALIGNED_ALLOCATOR
 			#pragma warning(disable : 4324) // disable padding warning
 //			#pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
@@ -62,6 +62,12 @@
  			#define btFsel(a,b,c) __fsel((a),(b),(c))
 		#else
+
+#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
 			#define BT_USE_SSE
-		#endif
+			#include <emmintrin.h>
+#endif
+
+		#endif//_XBOX
+
 		#endif //__MINGW32__
 
@@ -125,4 +131,7 @@
 
 		#define SIMD_FORCE_INLINE inline
+		///@todo: check out alignment methods for other platforms/compilers
+		///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+		///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
 		#define ATTRIBUTE_ALIGNED16(a) a
 		#define ATTRIBUTE_ALIGNED128(a) a
@@ -233,5 +242,5 @@
   SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
   #endif
-
+	
 #endif
 
Index: code/branches/questsystem5/src/bullet/LinearMath/btTransform.h
===================================================================
--- code/branches/questsystem5/src/bullet/LinearMath/btTransform.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/LinearMath/btTransform.h	(revision 2907)
@@ -191,9 +191,8 @@
 
   /**@brief Return an identity transform */
-	static btTransform	getIdentity()
-	{
-		btTransform tr;
-		tr.setIdentity();
-		return tr;
+	static const btTransform&	getIdentity()
+	{
+		static const btTransform identityTransform(btMatrix3x3::getIdentity());
+		return identityTransform;
 	}
 	
Index: code/branches/questsystem5/src/bullet/LinearMath/btVector3.h
===================================================================
--- code/branches/questsystem5/src/bullet/LinearMath/btVector3.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/LinearMath/btVector3.h	(revision 2907)
@@ -18,22 +18,53 @@
 #define SIMD__VECTOR3_H
 
-#include "btQuadWord.h"
-
+
+#include "btScalar.h"
+#include "btScalar.h"
+#include "btMinMax.h"
 /**@brief btVector3 can be used to represent 3D points and vectors.
  * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
  * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
  */
-class	btVector3 : public btQuadWord {
-
+
+ATTRIBUTE_ALIGNED16(class) btVector3
+{
 public:
+
+#if defined (__SPU__) && defined (__CELLOS_LV2__)
+	union {
+		vec_float4 mVec128;
+		btScalar	m_floats[4];
+	};
+public:
+	vec_float4	get128() const
+	{
+		return mVec128;
+	}
+public:
+#else //__CELLOS_LV2__ __SPU__
+#ifdef BT_USE_SSE // WIN32
+	union {
+		__m128 mVec128;
+		btScalar	m_floats[4];
+	};
+	SIMD_FORCE_INLINE	__m128	get128() const
+	{
+		return mVec128;
+	}
+	SIMD_FORCE_INLINE	void	set128(__m128 v128)
+	{
+		mVec128 = v128;
+	}
+#else
+	btScalar	m_floats[4];
+#endif
+#endif //__CELLOS_LV2__ __SPU__
+
+	public:
+
   /**@brief No initialization constructor */
 	SIMD_FORCE_INLINE btVector3() {}
 
-  /**@brief Constructor from btQuadWordStorage (btVector3 inherits from this so is also valid)
-   * Note: Vector3 derives from btQuadWordStorage*/
-	SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) 
-		: btQuadWord(q)
-	{
-	}
+ 
 	
   /**@brief Constructor from scalars 
@@ -42,13 +73,11 @@
    * @param z Z value 
    */
-	SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) 
-		:btQuadWord(x,y,z,btScalar(0.))
-	{
-	}
-
-//	SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 
-//		: btQuadWord(x,y,z,w)
-//	{
-//	}
+	SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
+	{
+		m_floats[0] = x;
+		m_floats[1] = y;
+		m_floats[2] = z;
+		m_floats[3] = btScalar(0.);
+	}
 
 	
@@ -58,5 +87,5 @@
 	{
 
-		m_floats[0] += v.x(); m_floats[1] += v.y(); m_floats[2] += v.z();
+		m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2];
 		return *this;
 	}
@@ -67,5 +96,5 @@
 	SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) 
 	{
-		m_floats[0] -= v.x(); m_floats[1] -= v.y(); m_floats[2] -= v.z();
+		m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2];
 		return *this;
 	}
@@ -74,5 +103,5 @@
 	SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
 	{
-		m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s;
+		m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s;
 		return *this;
 	}
@@ -90,5 +119,5 @@
 	SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
 	{
-		return m_floats[0] * v.x() + m_floats[1] * v.y() + m_floats[2] * v.z();
+		return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2];
 	}
 
@@ -149,14 +178,14 @@
 	{
 		return btVector3(
-			m_floats[1] * v.z() - m_floats[2] * v.y(),
-			m_floats[2] * v.x() - m_floats[0] * v.z(),
-			m_floats[0] * v.y() - m_floats[1] * v.x());
+			m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
+			m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
+			m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
 	}
 
 	SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
 	{
-		return m_floats[0] * (v1.y() * v2.z() - v1.z() * v2.y()) + 
-			m_floats[1] * (v1.z() * v2.x() - v1.x() * v2.z()) + 
-			m_floats[2] * (v1.x() * v2.y() - v1.y() * v2.x());
+		return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + 
+			m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + 
+			m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
 	}
 
@@ -165,5 +194,5 @@
 	SIMD_FORCE_INLINE int minAxis() const
 	{
-		return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2);
+		return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
 	}
 
@@ -172,5 +201,5 @@
 	SIMD_FORCE_INLINE int maxAxis() const 
 	{
-		return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0);
+		return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
 	}
 
@@ -188,7 +217,7 @@
 	{
 		btScalar s = btScalar(1.0) - rt;
-		m_floats[0] = s * v0.x() + rt * v1.x();
-		m_floats[1] = s * v0.y() + rt * v1.y();
-		m_floats[2] = s * v0.z() + rt * v1.z();
+		m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
+		m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
+		m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
 		//don't do the unused w component
 		//		m_co[3] = s * v0[3] + rt * v1[3];
@@ -200,7 +229,7 @@
 	SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const 
 	{
-		return btVector3(m_floats[0] + (v.x() - m_floats[0]) * t,
-			m_floats[1] + (v.y() - m_floats[1]) * t,
-			m_floats[2] + (v.z() - m_floats[2]) * t);
+		return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
+			m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
+			m_floats[2] + (v.m_floats[2] -m_floats[2]) * t);
 	}
 
@@ -209,9 +238,82 @@
 	SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
 	{
-		m_floats[0] *= v.x(); m_floats[1] *= v.y(); m_floats[2] *= v.z();
+		m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2];
 		return *this;
 	}
 
-	
+	 /**@brief Return the x value */
+		SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
+  /**@brief Return the y value */
+		SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
+  /**@brief Return the z value */
+		SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
+  /**@brief Set the x value */
+		SIMD_FORCE_INLINE void	setX(btScalar x) { m_floats[0] = x;};
+  /**@brief Set the y value */
+		SIMD_FORCE_INLINE void	setY(btScalar y) { m_floats[1] = y;};
+  /**@brief Set the z value */
+		SIMD_FORCE_INLINE void	setZ(btScalar z) {m_floats[2] = z;};
+  /**@brief Set the w value */
+		SIMD_FORCE_INLINE void	setW(btScalar w) { m_floats[3] = w;};
+  /**@brief Return the x value */
+		SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
+  /**@brief Return the y value */
+		SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
+  /**@brief Return the z value */
+		SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
+  /**@brief Return the w value */
+		SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
+
+	//SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_floats[0])[i];	}      
+	//SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
+	///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
+	SIMD_FORCE_INLINE	operator       btScalar *()       { return &m_floats[0]; }
+	SIMD_FORCE_INLINE	operator const btScalar *() const { return &m_floats[0]; }
+
+	SIMD_FORCE_INLINE	bool	operator==(const btVector3& other) const
+	{
+		return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
+	}
+
+	SIMD_FORCE_INLINE	bool	operator!=(const btVector3& other) const
+	{
+		return !(*this == other);
+	}
+
+	 /**@brief Set each element to the max of the current values and the values of another btVector3
+   * @param other The other btVector3 to compare with 
+   */
+		SIMD_FORCE_INLINE void	setMax(const btVector3& other)
+		{
+			btSetMax(m_floats[0], other.m_floats[0]);
+			btSetMax(m_floats[1], other.m_floats[1]);
+			btSetMax(m_floats[2], other.m_floats[2]);
+			btSetMax(m_floats[3], other.w());
+		}
+  /**@brief Set each element to the min of the current values and the values of another btVector3
+   * @param other The other btVector3 to compare with 
+   */
+		SIMD_FORCE_INLINE void	setMin(const btVector3& other)
+		{
+			btSetMin(m_floats[0], other.m_floats[0]);
+			btSetMin(m_floats[1], other.m_floats[1]);
+			btSetMin(m_floats[2], other.m_floats[2]);
+			btSetMin(m_floats[3], other.w());
+		}
+
+		SIMD_FORCE_INLINE void 	setValue(const btScalar& x, const btScalar& y, const btScalar& z)
+		{
+			m_floats[0]=x;
+			m_floats[1]=y;
+			m_floats[2]=z;
+			m_floats[3] = 0.f;
+		}
+
+		void	getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
+		{
+			v0->setValue(0.		,-z()		,y());
+			v1->setValue(z()	,0.			,-x());
+			v2->setValue(-y()	,x()	,0.);
+		}
 
 };
@@ -221,5 +323,5 @@
 operator+(const btVector3& v1, const btVector3& v2) 
 {
-	return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
+	return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]);
 }
 
@@ -228,5 +330,5 @@
 operator*(const btVector3& v1, const btVector3& v2) 
 {
-	return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z());
+	return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]);
 }
 
@@ -235,5 +337,5 @@
 operator-(const btVector3& v1, const btVector3& v2)
 {
-	return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
+	return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]);
 }
 /**@brief Return the negative of the vector */
@@ -241,5 +343,5 @@
 operator-(const btVector3& v)
 {
-	return btVector3(-v.x(), -v.y(), -v.z());
+	return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
 }
 
@@ -248,5 +350,5 @@
 operator*(const btVector3& v, const btScalar& s)
 {
-	return btVector3(v.x() * s, v.y() * s, v.z() * s);
+	return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
 }
 
@@ -270,5 +372,5 @@
 operator/(const btVector3& v1, const btVector3& v2)
 {
-	return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
+	return btVector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]);
 }
 
@@ -326,9 +428,5 @@
 }
 
-/**@brief Test if each element of the vector is equivalent */
-SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) 
-{
-	return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z();
-}
+
 
 SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
@@ -405,5 +503,5 @@
 		{
 			maxIndex = 2;
-			maxVal = m_floats[2];
+			maxVal =m_floats[2];
 		}
 		if (m_floats[3] > maxVal)
@@ -438,5 +536,5 @@
 		{
 			minIndex = 2;
-			minVal = m_floats[2];
+			minVal =m_floats[2];
 		}
 		if (m_floats[3] < minVal)
@@ -455,4 +553,38 @@
 		return absolute4().maxAxis4();
 	}
+
+	
+ 
+
+  /**@brief Set x,y,z and zero w 
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   */
+		
+
+/*		void getValue(btScalar *m) const 
+		{
+			m[0] = m_floats[0];
+			m[1] = m_floats[1];
+			m[2] =m_floats[2];
+		}
+*/
+/**@brief Set the values 
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   * @param w Value of w
+   */
+		SIMD_FORCE_INLINE void	setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
+		{
+			m_floats[0]=x;
+			m_floats[1]=y;
+			m_floats[2]=z;
+			m_floats[3]=w;
+		}
+
+
+ 
 
 };
