Merge branch 'master' into nazara-next

thirdparty/build/lua.lua

@@ -9,10 +9,6 @@ LIBRARY.Files = {
 	"../thirdparty/src/Lua/*.cpp"
 }
 
-LIBRARY.OsDefines.Windows = {
-	"LUA_USE_WINDOWS"
-}
-
 LIBRARY.OsDefines.Posix = {
 	"LUA_USE_LINUX"
 }

thirdparty/build/newton.lua

@@ -0,0 +1,38 @@
+LIBRARY.Name = "newton"
+
+LIBRARY.Defines = {
+	"_CRT_SECURE_NO_WARNINGS",
+	"_NEWTON_STATIC_LIB",
+}
+
+LIBRARY.OsDefines.Windows = {
+	"_WINDOWS"
+}
+
+LIBRARY.Language = "C++"
+
+LIBRARY.Files = {
+	"../thirdparty/include/newton/**.h",
+	"../thirdparty/src/newton/**.h",
+	"../thirdparty/src/newton/**.c",
+	"../thirdparty/src/newton/**.cpp",
+}
+
+LIBRARY.Includes = {
+	"../thirdparty/src/newton/dgCore",
+	"../thirdparty/src/newton/dgMeshUtil",
+	"../thirdparty/src/newton/dgPhysics",
+	"../thirdparty/src/newton/dgNewton",
+	"../thirdparty/src/newton/dContainers",
+	"../thirdparty/src/newton/dMath"
+}
+
+LIBRARY.Custom = function()
+	vectorextensions("SSE3")
+
+	filter({"architecture:x86_64", "system:linux"})
+		defines("_POSIX_VER_64")
+
+	filter({"architecture:x86", "system:linux"})
+		defines("_POSIX_VER")
+end

thirdparty/include/Newton/dgTypes.h

@@ -0,0 +1,696 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DGTYPES_H__
+#define __DGTYPES_H__
+
+#ifdef _MSC_VER 
+	#ifdef _M_ARM
+		#ifndef _ARM_VER
+			#define _ARM_VER
+		#endif
+	#elif defined (_M_X64)
+		#ifndef _WIN_64_VER
+			#define _WIN_64_VER
+		#endif
+	#elif defined (_M_IX86)
+		#ifndef _WIN_32_VER
+			#define _WIN_32_VER
+		#endif
+	#else 
+		#error target platform not defined
+	#endif
+
+	#if _MSC_VER >= 1400
+		#define HAVE_STRUCT_TIMESPEC
+	#endif
+#endif
+
+#if (defined (_WIN_32_VER) || defined (_WIN_64_VER) || (defined (_MSC_VER ) && defined (_ARM_VER)) )
+	#pragma warning (disable: 4100) //unreferenced formal parameter
+	#pragma warning (disable: 4201) //nonstandard extension used : nameless struct/union
+	#pragma warning (disable: 4324) //structure was padded due to __declspec(align())
+	#pragma warning (disable: 4514) //unreferenced inline function has been removed
+	#pragma warning (disable: 4530) //C++ exception handler used, but unwind semantics are not enabled. Specify /EHsc
+	#pragma warning (disable: 4625) //copy constructor could not be generated because a base class copy constructor is inaccessible or deleted
+	#pragma warning (disable: 4626) //assignment operator could not be generated because a base class assignment operator is inaccessible or deleted
+	#pragma warning (disable: 4640) //construction of local static object is not thread-safe
+	#pragma warning (disable: 4820) //bytes padding added after data member
+	#pragma warning (disable: 4005) //'__useHeader': macro redefinition
+	//#pragma warning (disable: 4577) // 'noexcept' used with no exception handling mode specified; termination on exception is not guaranteed. Specify /EHsc
+    
+	#include <io.h> 
+	#include <direct.h> 
+	#include <malloc.h>
+	#include <float.h>
+	#include <stdarg.h>
+	#include <process.h>
+
+	#ifdef _DEBUG
+		#pragma warning (disable: 4127)	//conditional expression is constant
+	#endif
+
+	#pragma warning (push, 3) 
+		#include <windows.h>
+		#include <crtdbg.h>
+		//#ifndef _DURANGO
+		//	#include <tlhelp32.h>
+		//#endif
+	#pragma warning (pop) 
+#endif
+
+#include <new>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdarg.h>
+#include <time.h>
+#include <math.h>
+#include <float.h>
+#include <ctype.h>
+
+#if (defined (__MINGW32__) || defined (__MINGW64__))
+	#include <io.h> 
+	#include <direct.h> 
+	#include <malloc.h>
+	#include <float.h>
+	#include <windows.h>
+	#include <process.h>
+#endif
+
+#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+	#include <intrin.h>
+	#include <emmintrin.h> 
+	#include <pmmintrin.h>
+#endif
+
+#if (defined (_POSIX_VER) || defined (_POSIX_VER_64) || defined (__MINGW32__) || defined (__MINGW64__))
+  /* CMake defines NDEBUG for _not_ debug builds. Therefore, set
+     Newton's _DEBUG flag only when NDEBUG is not defined.
+  */
+  #ifndef NDEBUG
+    #define _DEBUG 1
+  #endif
+
+	#include <unistd.h>
+	#include <assert.h>
+	#if (!defined(__arm__) && !defined(__aarch64__)) // it was __ARMCC_VERSION before, it should be __ARM__ or aarch64, otherwise cross compiling in gcc fails.
+		extern "C" 
+		{ 
+			// for SSE3 and up
+			#include <pmmintrin.h> 
+			#include <emmintrin.h> 
+			#include <mmintrin.h> 
+		} 
+	#endif
+#endif
+
+#ifdef _MACOSX_VER
+	#include <unistd.h>
+	#include <sys/sysctl.h>
+    #include <assert.h> 
+    #if (defined __i386__ || defined __x86_64__)
+		#include <fenv.h>
+		#include <pmmintrin.h> 
+		#include <emmintrin.h>  //sse3
+        #include <mmintrin.h> 
+    #endif
+#endif
+
+// uncomment out D_PROFILER to enable profiler frame capture profiler traces
+// alternatively the end application can use a command line option to enable this define
+//#define D_PROFILER
+
+// uncomment this for Scalar floating point 
+// alternatively the end application can use a command line option to enable this define
+//#define DG_SCALAR_VECTOR_CLASS
+
+// uncomment this for Scalar floating point 
+// alternatively the end application can use a command line option to enable this define
+//#define __ANDROID__
+
+// by default newton run on a separate thread and 
+// optionally concurrent with the calling thread,
+// it also uses a thread job pool for multi core systems.
+// define DG_USE_THREAD_EMULATION on the command line for 
+// platform that do not support hardware multi threading or 
+// if the and application want to control threading at the application level 
+//#define DG_USE_THREAD_EMULATION
+
+#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+	#if _MSC_VER < 1700
+		#ifndef DG_USE_THREAD_EMULATION
+			#define DG_USE_THREAD_EMULATION
+		#endif
+	#endif
+#endif
+
+#ifndef DG_USE_THREAD_EMULATION
+	#include <mutex>
+	#include <thread>
+	#include <condition_variable>
+#endif
+
+
+//************************************************************
+#ifdef DG_DISABLE_ASSERT
+	#define dgAssert(x)
+#else 
+	#if defined (_WIN_32_VER) || defined (_WIN_64_VER) 
+		#define dgAssert(x) _ASSERTE(x)
+	#elif defined (_MSC_VER ) && defined (_ARM_VER) 
+		#define dgAssert(x) _ASSERTE(x)
+	#else
+		#ifdef _DEBUG
+			#define dgAssert(x) assert(x)
+		#else 
+			#define dgAssert(x)
+		#endif
+	#endif
+#endif
+
+
+#define	DG_MAX_THREADS_HIVE_COUNT		16
+
+#ifdef _DEBUG
+//#define __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+#endif
+
+
+#ifdef DLL_DECLSPEC
+#undef DLL_DECLSPEC
+#endif
+
+#ifdef _DEBUG
+	#define DG_INLINE inline
+#else
+	#if defined(_MSC_VER)
+			#define DG_INLINE __forceinline 
+	#else 
+		#define DG_INLINE	inline
+		//#define DG_INLINE	 __attribute__((always_inline))
+	#endif
+#endif
+
+
+#define DG_VECTOR_SIMD_SIZE		16
+#define DG_VECTOR_AVX2_SIZE		32
+
+#if defined(_MSC_VER)
+	#define	DG_GCC_VECTOR_ALIGNMENT
+	#define	DG_MSC_VECTOR_ALIGNMENT    __declspec(align(DG_VECTOR_SIMD_SIZE))
+#else
+	#define	DG_GCC_VECTOR_ALIGNMENT    __attribute__ ((aligned (DG_VECTOR_SIMD_SIZE)))
+	#define	DG_MSC_VECTOR_ALIGNMENT
+#endif
+
+#if defined(_MSC_VER)
+	#define	DG_GCC_AVX_ALIGNMENT
+	#define	DG_MSC_AVX_ALIGNMENT       __declspec(align(DG_VECTOR_AVX2_SIZE))
+#else
+	#define	DG_GCC_AVX_ALIGNMENT       __attribute__ ((aligned (DG_VECTOR_AVX2_SIZE)))
+	#define	DG_MSC_AVX_ALIGNMENT
+#endif
+
+#if defined(_MSC_VER)
+	#define DG_LIBRARY_EXPORT __declspec(dllexport)
+	#define DG_LIBRARY_IMPORT __declspec(dllimport)
+	#define DG_LIBRARY_STATIC
+#else
+	#define DG_LIBRARY_EXPORT __attribute__((visibility("default")))
+	#define DG_LIBRARY_IMPORT __attribute__((visibility("default")))
+	#define DG_LIBRARY_STATIC
+#endif
+
+
+#if ((defined (_WIN_32_VER) || defined (_WIN_64_VER)) && (_MSC_VER  >= 1600))
+	#include <stdint.h>
+	typedef int8_t dgInt8;
+	typedef uint8_t dgUnsigned8;
+
+	typedef int16_t dgInt16;
+	typedef uint16_t dgUnsigned16;
+
+	typedef int32_t dgInt32;
+	typedef uint32_t dgUnsigned32;
+
+	typedef int64_t dgInt64;
+	typedef uint64_t dgUnsigned64;
+#else
+	typedef char dgInt8;
+	typedef unsigned char dgUnsigned8;
+
+	typedef short dgInt16;
+	typedef unsigned short dgUnsigned16;
+
+	typedef int dgInt32;
+	typedef unsigned dgUnsigned32;
+	typedef unsigned int dgUnsigned32;
+
+	typedef long long dgInt64;
+	typedef unsigned long long dgUnsigned64;
+	typedef double dgFloat64;
+#endif
+
+
+typedef double dgFloat64;
+
+#ifdef _NEWTON_USE_DOUBLE
+	typedef double dgFloat32;
+#else
+	typedef float dgFloat32;
+#endif
+
+
+class dgTriplex
+{
+	public:
+	dgFloat32 m_x;
+	dgFloat32 m_y;
+	dgFloat32 m_z;
+};
+
+#define dgPi		 	dgFloat32 (3.141592f)
+#define dgPI2		 	dgFloat32 (dgPi * 2.0f)
+#define dgEXP		 	dgFloat32 (2.71828f)
+#define dgEpsilon	  	dgFloat32 (1.0e-5f)
+#define dgDegreeToRad  	dgFloat32 (dgPi / 180.0f)
+#define dgRadToDegree  	dgFloat32 (180.0f / dgPi)
+
+class dgBigVector;
+#ifndef _NEWTON_USE_DOUBLE
+class dgVector;
+#endif 
+
+
+#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+	#define dgApi __cdecl 	
+	#define dgStdApi __stdcall 	
+#else
+	#define dgApi 	
+	#define dgStdApi
+#endif
+
+
+
+#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+	#define dgCheckFloat(x) (_finite(x) && !_isnan(x))
+//	#define dgCheckFloat(x) 1
+#else
+	#define dgCheckFloat(x) (isfinite(x) && !isnan(x))
+//		#define dgCheckFloat(x) 1
+#endif
+
+typedef void (dgApi *dgDeserialize) (void* const userData, void* buffer, dgInt32 size);
+typedef void (dgApi *dgSerialize) (void* const userData, const void* const buffer, dgInt32 size);
+typedef bool (dgApi *dgReportProgress) (dgFloat32 progressNormalzedPercent, void* const userData);
+
+// assume this function returns memory aligned to 16 bytes
+#define dgAlloca(type, count) (type*) alloca (sizeof (type) * (count))
+
+DG_INLINE dgInt32 dgExp2 (dgInt32 x)
+{
+	dgInt32 exp;
+	for (exp = -1; x; x >>= 1) {
+		exp ++;
+	}
+	return exp;
+}
+
+DG_INLINE dgInt32 dgBitReversal(dgInt32 v, dgInt32 base)
+{
+	dgInt32 x = 0;
+	dgInt32 power = dgExp2 (base) - 1;
+	do {
+		x += (v & 1) << power;
+		v >>= 1;
+		power--;
+	} while (v);
+	dgAssert(x < base);
+	return x;
+}
+
+
+template <class T> 
+DG_INLINE T dgMin(T A, T B)
+{
+	return (A < B) ? A : B; 
+}
+
+template <class T> 
+DG_INLINE T dgMax(T A, T B)
+{
+	return (A > B) ? A : B; 
+}
+
+template <class T>
+DG_INLINE T dgMin(T A, T B, T C)
+{
+	return dgMin(dgMin (A, B), C);
+}
+
+template <class T>
+DG_INLINE T dgMax(T A, T B, T C)
+{
+	return dgMax(dgMax (A, B), C);
+}
+
+template <class T>
+DG_INLINE T dgClamp(T val, T min, T max)
+{
+	return dgMax (min, dgMin (max, val));
+}
+
+template <class T> 
+DG_INLINE void dgSwap(T& A, T& B)
+{
+	T tmp (A);
+	A = B;
+	B = tmp;
+}	
+
+template <class T>
+DG_INLINE T dgAbs(T A)
+{
+	// according to Intel this is better because is does not read after write
+	return (A >= T(0)) ? A : -A;
+}
+
+template <class T>
+DG_INLINE T dgSign(T A)
+{
+	return (A >= T(0)) ? T(1) : T(-1);
+}
+
+template <class T> 
+DG_INLINE bool dgAreEqual(T A, T B, T tol)
+{
+	if ((dgAbs(A) < tol) && (dgAbs(B) < tol)) {
+		return true;
+	}
+/*
+	dgInt32 exp0;
+	dgFloat64 mantissa0 = frexp(dgFloat64 (A), &exp0);
+
+	dgInt32 exp1;
+	dgFloat64 mantissa1 = frexp(dgFloat64(B), &exp1);
+
+	if ((exp0 < -12) && (exp1 < -12)) {
+		return true;
+	}
+
+	if (exp0 != exp1) {
+		return false;
+	}
+	return dgAbs(mantissa0 - mantissa1) < tol;
+*/	
+	T den = dgMax(dgAbs(A), dgAbs(B)) + tol;
+	A /= den;
+	B /= den;
+	return dgAbs(A - B) < tol;
+}
+
+#ifdef _NEWTON_USE_DOUBLE
+	union dgFloatSign
+	{
+		struct {
+			dgInt32 m_dommy;
+			dgInt32 m_iVal;
+		} m_integer;
+		dgFloat64 m_fVal;
+	};
+#else
+	union dgFloatSign
+	{
+		struct {
+			dgInt32 m_iVal;
+		} m_integer;
+		dgFloat32 m_fVal;
+	};
+#endif
+
+union dgDoubleInt
+{
+	struct {
+		dgInt32 m_intL;
+		dgInt32 m_intH;
+	};
+	void* m_ptr;
+	dgInt64 m_int;
+	dgFloat64 m_float;
+};
+
+
+void dgGetMinMax (dgBigVector &Min, dgBigVector &Max, const dgFloat64* const vArray, dgInt32 vCount, dgInt32 strideInBytes);
+dgInt32 dgVertexListToIndexList (dgFloat64* const vertexList, dgInt32 strideInBytes, dgInt32 compareCount,     dgInt32 vertexCount,         dgInt32* const indexListOut, dgFloat64 tolerance = dgEpsilon);
+dgInt32 dgVertexListToIndexList (dgFloat32* const vertexList, dgInt32 strideInBytes, dgInt32 floatSizeInBytes, dgInt32 unsignedSizeInBytes, dgInt32 vertexCount, dgInt32* const indexListOut, dgFloat32 tolerance = dgEpsilon);
+
+#define PointerToInt(x) ((size_t)x)
+#define IntToPointer(x) ((void*)(size_t(x)))
+
+
+#ifndef _MSC_VER 
+	#define _stricmp(x,y) strcasecmp(x,y)
+#endif
+
+#define dgSqrt(x)			dgFloat32 (sqrt(x))	
+#define dgSin(x)			dgFloat32 (sin(x))
+#define dgCos(x)			dgFloat32 (cos(x))
+#define dgAsin(x)			dgFloat32 (asin(x))
+#define dgAcos(x)			dgFloat32 (acos(x))
+#define dgLog(x)			dgFloat32 (log(x))
+#define dgCeil(x)			dgFloat32 (ceil(x))
+#define dgFloor(x)			dgFloat32 (floor(x))	
+#define dgPow(x,y)			dgFloat32 (pow(x,y))
+#define dgFmod(x,y)			dgFloat32 (fmod(x,y))
+#define dgAtan2(x,y)		dgFloat32 (atan2(x,y))
+#define dgRsqrt(x)			(dgFloat32 (1.0f) / dgSqrt(x))
+#define dgClearFP()			_clearfp() 
+#define dgControlFP(x,y)	_controlfp(x,y)
+
+enum dgSerializeRevisionNumber
+{
+	m_firstRevision = 100,
+	// add new serialization revision number here
+	m_currentRevision 
+};
+
+dgUnsigned64 dgGetTimeInMicrosenconds();
+dgFloat64 dgRoundToFloat(dgFloat64 val);
+void dgSerializeMarker(dgSerialize serializeCallback, void* const userData);
+dgInt32 dgDeserializeMarker(dgDeserialize serializeCallback, void* const userData);
+
+class dgFloatExceptions
+{
+	public:
+	#ifdef _MSC_VER
+		#define DG_FLOAT_EXECTIONS_MASK	(EM_INVALID | EM_DENORMAL | EM_ZERODIVIDE)
+	#else 
+		#define DG_FLOAT_EXECTIONS_MASK	0
+	#endif
+
+	dgFloatExceptions(dgUnsigned32 mask = DG_FLOAT_EXECTIONS_MASK);
+	~dgFloatExceptions();
+
+	private:
+	#if (defined (_MSC_VER) && defined (_WIN_32_VER))
+	dgUnsigned32 m_mask;
+#endif
+};
+
+class dgSetPrecisionDouble 
+{
+	public:
+	dgSetPrecisionDouble();
+	~dgSetPrecisionDouble();
+	#if (defined (_MSC_VER) && defined (_WIN_32_VER))
+	dgInt32 m_mask; 
+	#endif
+};
+
+DG_INLINE dgInt32 dgAtomicExchangeAndAdd (dgInt32* const addend, dgInt32 amount)
+{
+	#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+		return _InterlockedExchangeAdd((long*)addend, long(amount));
+	#elif defined (_MSC_VER ) && defined (_ARM_VER) 
+		return _InterlockedExchangeAdd((long*)addend, long(amount));
+	#elif (defined (__MINGW32__) || defined (__MINGW64__))
+		return InterlockedExchangeAdd((long*)addend, long(amount));
+	#elif (defined (_POSIX_VER) || defined (_POSIX_VER_64) ||defined (_MACOSX_VER)|| defined ANDROID)
+		return __sync_fetch_and_add((int32_t*)addend, amount);
+	#else
+		#error "dgAtomicExchangeAndAdd implementation required"
+	#endif
+}
+
+DG_INLINE dgInt32 dgInterlockedExchange(dgInt32* const ptr, dgInt32 value)
+{
+	#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+		return _InterlockedExchange((long*) ptr, value);
+	#elif defined (_MSC_VER ) && defined (_ARM_VER) 
+		return _InterlockedExchange((long*)ptr, value);
+	#elif (defined (__MINGW32__) || defined (__MINGW64__))
+		return InterlockedExchange((long*) ptr, value);
+	#elif (defined (_POSIX_VER) || defined (_POSIX_VER_64) ||defined (_MACOSX_VER))
+		return __sync_lock_test_and_set((int32_t*)ptr, value);
+	#else
+		#error "dgInterlockedExchange implementation required"
+	#endif
+}
+
+/*
+DG_INLINE void* dgInterlockedExchange(void** const ptr, void* value)
+{
+	#if defined(_WIN32)
+		#ifdef _M_X64 
+			return (void*)_InterlockedExchange64((dgInt64*)ptr, dgInt64 (value));
+		#else
+			return (void*)_InterlockedExchange((long*) ptr, dgInt32(value));
+		#endif
+	#elif defined(__linux__) || defined(_MACOSX_VER)
+		#if defined(__ANDROID__)
+			return __sync_lock_test_and_set(ptr, value);
+		#else
+			#ifdef __x86_64__
+				return (void*)__sync_lock_test_and_set((dgInt64*) ptr, (dgInt64)value);
+			#else
+				return (void*)__sync_lock_test_and_set((dgInt32*) ptr, (dgInt32) value);
+			#endif
+		#endif
+	#else
+		#error "dgInterlockedExchange implementation required"
+	#endif
+}
+*/
+
+DG_INLINE dgInt32 dgInterlockedTest(dgInt32* const ptr, dgInt32 value)
+{
+	#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+		return _InterlockedCompareExchange((long*)ptr, value, value);
+	#elif defined (_MSC_VER ) && defined (_ARM_VER) 
+		return _InterlockedCompareExchange((long*)ptr, value, value);
+	#elif (defined (__MINGW32__) || defined (__MINGW64__))
+		return InterlockedCompareExchange((long*)ptr, value, value);
+	#elif (defined (_POSIX_VER) || defined (_POSIX_VER_64) ||defined (_MACOSX_VER))
+		return __sync_lock_test_and_set((int32_t*)ptr, value);
+	#else
+		#error "dgInterlockedTest implementation required"
+	#endif
+}
+
+DG_INLINE void dgThreadYield()
+{
+#ifndef DG_USE_THREAD_EMULATION
+	std::this_thread::yield();
+#endif
+}
+
+DG_INLINE void dgThreadPause()
+{
+#ifndef DG_USE_THREAD_EMULATION
+	//#if defined (_WIN_32_VER) || defined (_WIN_64_VER) || defined (WIN32) || defined (i386_) || defined (x86_64_)
+	#if defined (_WIN_32_VER) || defined (_WIN_64_VER)
+		_mm_pause();
+	#else 
+		std::this_thread::yield();
+	#endif
+#endif
+}
+
+DG_INLINE void dgSpinLock(dgInt32* const ptr)
+{
+#ifndef DG_USE_THREAD_EMULATION 
+	while (dgInterlockedExchange(ptr, 1)) {
+		dgThreadYield();
+	}
+#endif
+}
+
+
+DG_INLINE void dgSpinUnlock (dgInt32* const ptr)
+{
+	#ifndef DG_USE_THREAD_EMULATION 
+		dgInterlockedExchange(ptr, 0);
+	#else
+		*ptr = 0;
+	#endif
+}
+
+
+class dgScopeSpinLock
+{
+	public:
+	DG_INLINE dgScopeSpinLock(dgInt32* const lock)
+		:m_atomicLock(lock)
+	{
+		dgSpinLock(m_atomicLock);
+	}
+
+	DG_INLINE ~dgScopeSpinLock()
+	{
+		dgSpinUnlock(m_atomicLock);
+	}
+
+	dgInt32* m_atomicLock;
+};
+
+class dgScopeSpinPause
+{
+	public:
+	DG_INLINE dgScopeSpinPause(dgInt32* const lock)
+		:m_atomicLock(lock)
+	{
+		while (dgInterlockedExchange(m_atomicLock, 1)) {
+			dgThreadPause();
+		}
+	}
+
+	DG_INLINE ~dgScopeSpinPause()
+	{
+		dgSpinUnlock(m_atomicLock);
+	}
+
+	dgInt32* m_atomicLock;
+};
+
+
+
+#ifdef _MACOSX_VER
+#include <sys/time.h>
+#ifndef CLOCK_REALTIME
+	#define CLOCK_REALTIME 0
+#endif
+#ifndef CLOCK_MONOTONIC
+	#define CLOCK_MONOTONIC 0
+#endif
+//clock_gettime is not implemented on OSX
+DG_INLINE int clock_gettime(int /*clk_id*/, struct timespec* t) {
+    struct timeval now;
+    int rv = gettimeofday(&now, NULL);
+    if (rv) return rv;
+    t->tv_sec  = now.tv_sec;
+    t->tv_nsec = now.tv_usec * 1000;
+    return 0;
+}
+#endif
+
+#endif
+

thirdparty/include/kiwi/AssocVector.h

@@ -0,0 +1,356 @@
+////////////////////////////////////////////////////////////////////////////////
+// The Loki Library
+// Copyright (c) 2001 by Andrei Alexandrescu
+// This code accompanies the book:
+// Alexandrescu, Andrei. "Modern C++ Design: Generic Programming and Design
+//     Patterns Applied". Copyright (c) 2001. Addison-Wesley.
+// Permission to use, copy, modify, distribute and sell this software for any
+//     purpose is hereby granted without fee, provided that the above copyright
+//     notice appear in all copies and that both that copyright notice and this
+//     permission notice appear in supporting documentation.
+// The author or Addison-Wesley Longman make no representations about the
+//     suitability of this software for any purpose. It is provided "as is"
+//     without express or implied warranty.
+////////////////////////////////////////////////////////////////////////////////
+// Updated 2019 by Matthieu Dartiailh for C++11 compliancy
+////////////////////////////////////////////////////////////////////////////////
+#pragma once
+
+// $Id: AssocVector.h 765 2006-10-18 13:55:32Z syntheticpp $
+
+
+#include <algorithm>
+#include <functional>
+#include <vector>
+#include <utility>
+
+namespace Loki
+{
+////////////////////////////////////////////////////////////////////////////////
+// class template AssocVectorCompare
+// Used by AssocVector
+////////////////////////////////////////////////////////////////////////////////
+
+    namespace Private
+    {
+        template <class Value, class C>
+        class AssocVectorCompare : public C
+        {
+            typedef std::pair<typename C::first_argument_type, Value>
+                Data;
+            typedef typename C::first_argument_type first_argument_type;
+
+        public:
+            AssocVectorCompare()
+            {}
+
+            AssocVectorCompare(const C& src) : C(src)
+            {}
+
+            bool operator()(const first_argument_type& lhs,
+                const first_argument_type& rhs) const
+            { return C::operator()(lhs, rhs); }
+
+            bool operator()(const Data& lhs, const Data& rhs) const
+            { return operator()(lhs.first, rhs.first); }
+
+            bool operator()(const Data& lhs,
+                const first_argument_type& rhs) const
+            { return operator()(lhs.first, rhs); }
+
+            bool operator()(const first_argument_type& lhs,
+                const Data& rhs) const
+            { return operator()(lhs, rhs.first); }
+        };
+    }
+
+////////////////////////////////////////////////////////////////////////////////
+// class template AssocVector
+// An associative vector built as a syntactic drop-in replacement for std::map
+// BEWARE: AssocVector doesn't respect all map's guarantees, the most important
+//     being:
+// * iterators are invalidated by insert and erase operations
+// * the complexity of insert/erase is O(N) not O(log N)
+// * value_type is std::pair<K, V> not std::pair<const K, V>
+// * iterators are random
+////////////////////////////////////////////////////////////////////////////////
+
+
+    template
+    <
+        class K,
+        class V,
+        class C = std::less<K>,
+        class A = std::allocator< std::pair<K, V> >
+    >
+    class AssocVector
+        : private std::vector< std::pair<K, V>, A >
+        , private Private::AssocVectorCompare<V, C>
+    {
+        typedef std::vector<std::pair<K, V>, A> Base;
+        typedef Private::AssocVectorCompare<V, C> MyCompare;
+
+    public:
+        typedef K key_type;
+        typedef V mapped_type;
+        typedef typename Base::value_type value_type;
+
+        typedef C key_compare;
+        typedef A allocator_type;
+        typedef typename A::reference reference;
+        typedef typename A::const_reference const_reference;
+        typedef typename Base::iterator iterator;
+        typedef typename Base::const_iterator const_iterator;
+        typedef typename Base::size_type size_type;
+        typedef typename Base::difference_type difference_type;
+        typedef typename A::pointer pointer;
+        typedef typename A::const_pointer const_pointer;
+        typedef typename Base::reverse_iterator reverse_iterator;
+        typedef typename Base::const_reverse_iterator const_reverse_iterator;
+
+        class value_compare
+            : public std::function<bool(value_type, value_type)>
+            , private key_compare
+        {
+            friend class AssocVector;
+
+        protected:
+            value_compare(key_compare pred) : key_compare(pred)
+            {}
+
+        public:
+            bool operator()(const value_type& lhs, const value_type& rhs) const
+            { return key_compare::operator()(lhs.first, rhs.first); }
+        };
+
+        // 23.3.1.1 construct/copy/destroy
+
+        explicit AssocVector(const key_compare& comp = key_compare(),
+            const A& alloc = A())
+        : Base(alloc), MyCompare(comp)
+        {}
+
+        template <class InputIterator>
+        AssocVector(InputIterator first, InputIterator last,
+            const key_compare& comp = key_compare(),
+            const A& alloc = A())
+        : Base(first, last, alloc), MyCompare(comp)
+        {
+            MyCompare& me = *this;
+            std::sort(begin(), end(), me);
+        }
+
+        AssocVector& operator=(const AssocVector& rhs)
+        {
+            AssocVector(rhs).swap(*this);
+            return *this;
+        }
+
+        // iterators:
+        // The following are here because MWCW gets 'using' wrong
+        iterator begin() { return Base::begin(); }
+        const_iterator begin() const { return Base::begin(); }
+        iterator end() { return Base::end(); }
+        const_iterator end() const { return Base::end(); }
+        reverse_iterator rbegin() { return Base::rbegin(); }
+        const_reverse_iterator rbegin() const { return Base::rbegin(); }
+        reverse_iterator rend() { return Base::rend(); }
+        const_reverse_iterator rend() const { return Base::rend(); }
+
+        // capacity:
+        bool empty() const { return Base::empty(); }
+        size_type size() const { return Base::size(); }
+        size_type max_size() { return Base::max_size(); }
+
+        // 23.3.1.2 element access:
+        mapped_type& operator[](const key_type& key)
+        { return insert(value_type(key, mapped_type())).first->second; }
+
+        // modifiers:
+        std::pair<iterator, bool> insert(const value_type& val)
+        {
+            bool found(true);
+            iterator i(lower_bound(val.first));
+
+            if (i == end() || this->operator()(val.first, i->first))
+            {
+                i = Base::insert(i, val);
+                found = false;
+            }
+            return std::make_pair(i, !found);
+        }
+        //Section [23.1.2], Table 69
+        //http://developer.apple.com/documentation/DeveloperTools/gcc-3.3/libstdc++/23_containers/howto.html#4
+        iterator insert(iterator pos, const value_type& val)
+        {
+            if( (pos == begin() || this->operator()(*(pos-1),val)) &&
+                (pos == end()    || this->operator()(val, *pos)) )
+            {
+                return Base::insert(pos, val);
+            }
+            return insert(val).first;
+        }
+
+        template <class InputIterator>
+        void insert(InputIterator first, InputIterator last)
+        { for (; first != last; ++first) insert(*first); }
+
+        void erase(iterator pos)
+        { Base::erase(pos); }
+
+        size_type erase(const key_type& k)
+        {
+            iterator i(find(k));
+            if (i == end()) return 0;
+            erase(i);
+            return 1;
+        }
+
+        void erase(iterator first, iterator last)
+        { Base::erase(first, last); }
+
+        void swap(AssocVector& other)
+        {
+            Base::swap(other);
+            MyCompare& me = *this;
+            MyCompare& rhs = other;
+            std::swap(me, rhs);
+        }
+
+        void clear()
+        { Base::clear(); }
+
+        // observers:
+        key_compare key_comp() const
+        { return *this; }
+
+        value_compare value_comp() const
+        {
+            const key_compare& comp = *this;
+            return value_compare(comp);
+        }
+
+        // 23.3.1.3 map operations:
+        iterator find(const key_type& k)
+        {
+            iterator i(lower_bound(k));
+            if (i != end() && this->operator()(k, i->first))
+            {
+                i = end();
+            }
+            return i;
+        }
+
+        const_iterator find(const key_type& k) const
+        {
+            const_iterator i(lower_bound(k));
+            if (i != end() && this->operator()(k, i->first))
+            {
+                i = end();
+            }
+            return i;
+        }
+
+        size_type count(const key_type& k) const
+        { return find(k) != end(); }
+
+        iterator lower_bound(const key_type& k)
+        {
+            MyCompare& me = *this;
+            return std::lower_bound(begin(), end(), k, me);
+        }
+
+        const_iterator lower_bound(const key_type& k) const
+        {
+            const MyCompare& me = *this;
+            return std::lower_bound(begin(), end(), k, me);
+        }
+
+        iterator upper_bound(const key_type& k)
+        {
+            MyCompare& me = *this;
+            return std::upper_bound(begin(), end(), k, me);
+        }
+
+        const_iterator upper_bound(const key_type& k) const
+        {
+            const MyCompare& me = *this;
+            return std::upper_bound(begin(), end(), k, me);
+        }
+
+        std::pair<iterator, iterator> equal_range(const key_type& k)
+        {
+            MyCompare& me = *this;
+            return std::equal_range(begin(), end(), k, me);
+        }
+
+        std::pair<const_iterator, const_iterator> equal_range(
+            const key_type& k) const
+        {
+            const MyCompare& me = *this;
+            return std::equal_range(begin(), end(), k, me);
+        }
+
+        template <class K1, class V1, class C1, class A1>
+        friend bool operator==(const AssocVector<K1, V1, C1, A1>& lhs,
+                        const AssocVector<K1, V1, C1, A1>& rhs);
+
+        bool operator<(const AssocVector& rhs) const
+        {
+            const Base& me = *this;
+            const Base& yo = rhs;
+            return me < yo;
+        }
+
+        template <class K1, class V1, class C1, class A1>
+        friend bool operator!=(const AssocVector<K1, V1, C1, A1>& lhs,
+                               const AssocVector<K1, V1, C1, A1>& rhs);
+
+        template <class K1, class V1, class C1, class A1>
+        friend bool operator>(const AssocVector<K1, V1, C1, A1>& lhs,
+                              const AssocVector<K1, V1, C1, A1>& rhs);
+
+        template <class K1, class V1, class C1, class A1>
+        friend bool operator>=(const AssocVector<K1, V1, C1, A1>& lhs,
+                               const AssocVector<K1, V1, C1, A1>& rhs);
+
+        template <class K1, class V1, class C1, class A1>
+        friend bool operator<=(const AssocVector<K1, V1, C1, A1>& lhs,
+                               const AssocVector<K1, V1, C1, A1>& rhs);
+    };
+
+    template <class K, class V, class C, class A>
+    inline bool operator==(const AssocVector<K, V, C, A>& lhs,
+                           const AssocVector<K, V, C, A>& rhs)
+    {
+      const std::vector<std::pair<K, V>, A>& me = lhs;
+      return me == rhs;
+    }
+
+    template <class K, class V, class C, class A>
+    inline bool operator!=(const AssocVector<K, V, C, A>& lhs,
+                           const AssocVector<K, V, C, A>& rhs)
+    { return !(lhs == rhs); }
+
+    template <class K, class V, class C, class A>
+    inline bool operator>(const AssocVector<K, V, C, A>& lhs,
+                          const AssocVector<K, V, C, A>& rhs)
+    { return rhs < lhs; }
+
+    template <class K, class V, class C, class A>
+    inline bool operator>=(const AssocVector<K, V, C, A>& lhs,
+                           const AssocVector<K, V, C, A>& rhs)
+    { return !(lhs < rhs); }
+
+    template <class K, class V, class C, class A>
+    inline bool operator<=(const AssocVector<K, V, C, A>& lhs,
+                           const AssocVector<K, V, C, A>& rhs)
+    { return !(rhs < lhs); }
+
+
+    // specialized algorithms:
+    template <class K, class V, class C, class A>
+    void swap(AssocVector<K, V, C, A>& lhs, AssocVector<K, V, C, A>& rhs)
+    { lhs.swap(rhs); }
+
+} // namespace Loki

thirdparty/include/kiwi/constraint.h

@@ -0,0 +1,119 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+#include <map>
+#include <vector>
+#include "expression.h"
+#include "shareddata.h"
+#include "strength.h"
+#include "term.h"
+#include "variable.h"
+
+namespace kiwi
+{
+
+enum RelationalOperator
+{
+    OP_LE,
+    OP_GE,
+    OP_EQ
+};
+
+class Constraint
+{
+
+public:
+    Constraint() : m_data(0) {}
+
+    Constraint(const Expression &expr,
+               RelationalOperator op,
+               double strength = strength::required) : m_data(new ConstraintData(expr, op, strength)) {}
+
+    Constraint(const Constraint &other, double strength) : m_data(new ConstraintData(other, strength)) {}
+
+    ~Constraint() {}
+
+    const Expression &expression() const
+    {
+        return m_data->m_expression;
+    }
+
+    RelationalOperator op() const
+    {
+        return m_data->m_op;
+    }
+
+    double strength() const
+    {
+        return m_data->m_strength;
+    }
+
+    bool operator!() const
+    {
+        return !m_data;
+    }
+
+private:
+    static Expression reduce(const Expression &expr)
+    {
+        std::map<Variable, double> vars;
+        typedef std::vector<Term>::const_iterator iter_t;
+        iter_t end = expr.terms().end();
+        for (iter_t it = expr.terms().begin(); it != end; ++it)
+            vars[it->variable()] += it->coefficient();
+        std::vector<Term> terms(vars.begin(), vars.end());
+        return Expression(terms, expr.constant());
+    }
+
+    class ConstraintData : public SharedData
+    {
+
+    public:
+        ConstraintData(const Expression &expr,
+                       RelationalOperator op,
+                       double strength) : SharedData(),
+                                          m_expression(reduce(expr)),
+                                          m_strength(strength::clip(strength)),
+                                          m_op(op) {}
+
+        ConstraintData(const Constraint &other, double strength) : SharedData(),
+                                                                   m_expression(other.expression()),
+                                                                   m_strength(strength::clip(strength)),
+                                                                   m_op(other.op()) {}
+
+        ~ConstraintData() {}
+
+        Expression m_expression;
+        double m_strength;
+        RelationalOperator m_op;
+
+    private:
+        ConstraintData(const ConstraintData &other);
+
+        ConstraintData &operator=(const ConstraintData &other);
+    };
+
+    SharedDataPtr<ConstraintData> m_data;
+
+    friend bool operator<(const Constraint &lhs, const Constraint &rhs)
+    {
+        return lhs.m_data < rhs.m_data;
+    }
+
+    friend bool operator==(const Constraint &lhs, const Constraint &rhs)
+    {
+        return lhs.m_data == rhs.m_data;
+    }
+
+    friend bool operator!=(const Constraint &lhs, const Constraint &rhs)
+    {
+        return lhs.m_data != rhs.m_data;
+    }
+};
+
+} // namespace kiwi

thirdparty/include/kiwi/debug.h

@@ -0,0 +1,200 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+#include <iostream>
+#include <sstream>
+#include <vector>
+#include "constraint.h"
+#include "solverimpl.h"
+#include "term.h"
+
+namespace kiwi
+{
+
+namespace impl
+{
+
+class DebugHelper
+{
+
+public:
+    static void dump(const SolverImpl &solver, std::ostream &out)
+    {
+        out << "Objective" << std::endl;
+        out << "---------" << std::endl;
+        dump(*solver.m_objective, out);
+        out << std::endl;
+        out << "Tableau" << std::endl;
+        out << "-------" << std::endl;
+        dump(solver.m_rows, out);
+        out << std::endl;
+        out << "Infeasible" << std::endl;
+        out << "----------" << std::endl;
+        dump(solver.m_infeasible_rows, out);
+        out << std::endl;
+        out << "Variables" << std::endl;
+        out << "---------" << std::endl;
+        dump(solver.m_vars, out);
+        out << std::endl;
+        out << "Edit Variables" << std::endl;
+        out << "--------------" << std::endl;
+        dump(solver.m_edits, out);
+        out << std::endl;
+        out << "Constraints" << std::endl;
+        out << "-----------" << std::endl;
+        dump(solver.m_cns, out);
+        out << std::endl;
+        out << std::endl;
+    }
+
+    static void dump(const SolverImpl::RowMap &rows, std::ostream &out)
+    {
+        typedef SolverImpl::RowMap::const_iterator iter_t;
+        iter_t end = rows.end();
+        for (iter_t it = rows.begin(); it != end; ++it)
+        {
+            dump(it->first, out);
+            out << " | ";
+            dump(*it->second, out);
+        }
+    }
+
+    static void dump(const std::vector<Symbol> &symbols, std::ostream &out)
+    {
+        typedef std::vector<Symbol>::const_iterator iter_t;
+        iter_t end = symbols.end();
+        for (iter_t it = symbols.begin(); it != end; ++it)
+        {
+            dump(*it, out);
+            out << std::endl;
+        }
+    }
+
+    static void dump(const SolverImpl::VarMap &vars, std::ostream &out)
+    {
+        typedef SolverImpl::VarMap::const_iterator iter_t;
+        iter_t end = vars.end();
+        for (iter_t it = vars.begin(); it != end; ++it)
+        {
+            out << it->first.name() << " = ";
+            dump(it->second, out);
+            out << std::endl;
+        }
+    }
+
+    static void dump(const SolverImpl::CnMap &cns, std::ostream &out)
+    {
+        typedef SolverImpl::CnMap::const_iterator iter_t;
+        iter_t end = cns.end();
+        for (iter_t it = cns.begin(); it != end; ++it)
+            dump(it->first, out);
+    }
+
+    static void dump(const SolverImpl::EditMap &edits, std::ostream &out)
+    {
+        typedef SolverImpl::EditMap::const_iterator iter_t;
+        iter_t end = edits.end();
+        for (iter_t it = edits.begin(); it != end; ++it)
+            out << it->first.name() << std::endl;
+    }
+
+    static void dump(const Row &row, std::ostream &out)
+    {
+        typedef Row::CellMap::const_iterator iter_t;
+        out << row.constant();
+        iter_t end = row.cells().end();
+        for (iter_t it = row.cells().begin(); it != end; ++it)
+        {
+            out << " + " << it->second << " * ";
+            dump(it->first, out);
+        }
+        out << std::endl;
+    }
+
+    static void dump(const Symbol &symbol, std::ostream &out)
+    {
+        switch (symbol.type())
+        {
+        case Symbol::Invalid:
+            out << "i";
+            break;
+        case Symbol::External:
+            out << "v";
+            break;
+        case Symbol::Slack:
+            out << "s";
+            break;
+        case Symbol::Error:
+            out << "e";
+            break;
+        case Symbol::Dummy:
+            out << "d";
+            break;
+        default:
+            break;
+        }
+        out << symbol.id();
+    }
+
+    static void dump(const Constraint &cn, std::ostream &out)
+    {
+        typedef std::vector<Term>::const_iterator iter_t;
+        iter_t begin = cn.expression().terms().begin();
+        iter_t end = cn.expression().terms().end();
+        for (iter_t it = begin; it != end; ++it)
+        {
+            out << it->coefficient() << " * ";
+            out << it->variable().name() << " + ";
+        }
+        out << cn.expression().constant();
+        switch (cn.op())
+        {
+        case OP_LE:
+            out << " <= 0 ";
+            break;
+        case OP_GE:
+            out << " >= 0 ";
+            break;
+        case OP_EQ:
+            out << " == 0 ";
+            break;
+        default:
+            break;
+        }
+        out << " | strength = " << cn.strength() << std::endl;
+    }
+};
+
+} // namespace impl
+
+namespace debug
+{
+
+template <typename T>
+void dump(const T &value)
+{
+    impl::DebugHelper::dump(value, std::cout);
+}
+
+template <typename T>
+void dump(const T &value, std::ostream &out)
+{
+    impl::DebugHelper::dump(value, out);
+}
+
+template <typename T>
+std::string dumps(const T &value)
+{
+    std::stringstream stream;
+    impl::DebugHelper::dump(value, stream);
+    return stream.str();
+}
+
+} // namespace debug
+
+} // namespace kiwi

thirdparty/include/kiwi/errors.h

@@ -0,0 +1,162 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+#include <exception>
+#include <string>
+#include "constraint.h"
+#include "variable.h"
+
+namespace kiwi
+{
+
+class UnsatisfiableConstraint : public std::exception
+{
+
+public:
+    UnsatisfiableConstraint(const Constraint &constraint) : m_constraint(constraint) {}
+
+    ~UnsatisfiableConstraint() throw() {}
+
+    const char *what() const throw()
+    {
+        return "The constraint can not be satisfied.";
+    }
+
+    const Constraint &constraint() const
+    {
+        return m_constraint;
+    }
+
+private:
+    Constraint m_constraint;
+};
+
+class UnknownConstraint : public std::exception
+{
+
+public:
+    UnknownConstraint(const Constraint &constraint) : m_constraint(constraint) {}
+
+    ~UnknownConstraint() throw() {}
+
+    const char *what() const throw()
+    {
+        return "The constraint has not been added to the solver.";
+    }
+
+    const Constraint &constraint() const
+    {
+        return m_constraint;
+    }
+
+private:
+    Constraint m_constraint;
+};
+
+class DuplicateConstraint : public std::exception
+{
+
+public:
+    DuplicateConstraint(const Constraint &constraint) : m_constraint(constraint) {}
+
+    ~DuplicateConstraint() throw() {}
+
+    const char *what() const throw()
+    {
+        return "The constraint has already been added to the solver.";
+    }
+
+    const Constraint &constraint() const
+    {
+        return m_constraint;
+    }
+
+private:
+    Constraint m_constraint;
+};
+
+class UnknownEditVariable : public std::exception
+{
+
+public:
+    UnknownEditVariable(const Variable &variable) : m_variable(variable) {}
+
+    ~UnknownEditVariable() throw() {}
+
+    const char *what() const throw()
+    {
+        return "The edit variable has not been added to the solver.";
+    }
+
+    const Variable &variable() const
+    {
+        return m_variable;
+    }
+
+private:
+    Variable m_variable;
+};
+
+class DuplicateEditVariable : public std::exception
+{
+
+public:
+    DuplicateEditVariable(const Variable &variable) : m_variable(variable) {}
+
+    ~DuplicateEditVariable() throw() {}
+
+    const char *what() const throw()
+    {
+        return "The edit variable has already been added to the solver.";
+    }
+
+    const Variable &variable() const
+    {
+        return m_variable;
+    }
+
+private:
+    Variable m_variable;
+};
+
+class BadRequiredStrength : public std::exception
+{
+
+public:
+    BadRequiredStrength() {}
+
+    ~BadRequiredStrength() throw() {}
+
+    const char *what() const throw()
+    {
+        return "A required strength cannot be used in this context.";
+    }
+};
+
+class InternalSolverError : public std::exception
+{
+
+public:
+    InternalSolverError() : m_msg("An internal solver error ocurred.") {}
+
+    InternalSolverError(const char *msg) : m_msg(msg) {}
+
+    InternalSolverError(const std::string &msg) : m_msg(msg) {}
+
+    ~InternalSolverError() throw() {}
+
+    const char *what() const throw()
+    {
+        return m_msg.c_str();
+    }
+
+private:
+    std::string m_msg;
+};
+
+} // namespace kiwi

thirdparty/include/kiwi/expression.h

@@ -0,0 +1,52 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+#include <vector>
+#include "term.h"
+
+namespace kiwi
+{
+
+class Expression
+{
+
+public:
+    Expression(double constant = 0.0) : m_constant(constant) {}
+
+    Expression(const Term &term, double constant = 0.0) : m_terms(1, term), m_constant(constant) {}
+
+    Expression(const std::vector<Term> &terms, double constant = 0.0) : m_terms(terms), m_constant(constant) {}
+
+    ~Expression() {}
+
+    const std::vector<Term> &terms() const
+    {
+        return m_terms;
+    }
+
+    double constant() const
+    {
+        return m_constant;
+    }
+
+    double value() const
+    {
+        typedef std::vector<Term>::const_iterator iter_t;
+        double result = m_constant;
+        iter_t end = m_terms.end();
+        for (iter_t it = m_terms.begin(); it != end; ++it)
+            result += it->value();
+        return result;
+    }
+
+private:
+    std::vector<Term> m_terms;
+    double m_constant;
+};
+
+} // namespace kiwi

thirdparty/include/kiwi/kiwi.h

@@ -0,0 +1,19 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+#include "constraint.h"
+#include "debug.h"
+#include "errors.h"
+#include "expression.h"
+#include "shareddata.h"
+#include "solver.h"
+#include "strength.h"
+#include "symbolics.h"
+#include "term.h"
+#include "variable.h"
+#include "version.h"

thirdparty/include/kiwi/maptype.h

@@ -0,0 +1,37 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2019, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+#include <functional>
+#include <map>
+#include <memory>
+#include <utility>
+#include "AssocVector.h"
+
+namespace kiwi
+{
+
+namespace impl
+{
+
+template <
+    typename K,
+    typename V,
+    typename C = std::less<K>,
+    typename A = std::allocator<std::pair<K, V>>>
+using MapType = Loki::AssocVector<K, V, C, A>;
+
+// template<
+// 	typename K,
+// 	typename V,
+// 	typename C = std::less<K>,
+// 	typename A = std::allocator< std::pair<const K, V> > >
+// using MapType = std::map<K, V, C, A>;
+
+} // namespace impl
+
+} // namespace kiwi

thirdparty/include/kiwi/row.h

@@ -0,0 +1,188 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+#include "maptype.h"
+#include "symbol.h"
+#include "util.h"
+
+namespace kiwi
+{
+
+namespace impl
+{
+
+class Row
+{
+
+public:
+    typedef MapType<Symbol, double> CellMap;
+
+    Row() : m_constant(0.0) {}
+
+    Row(double constant) : m_constant(constant) {}
+
+    Row(const Row &other) : m_cells(other.m_cells), m_constant(other.m_constant) {}
+
+    ~Row() {}
+
+    const CellMap &cells() const
+    {
+        return m_cells;
+    }
+
+    double constant() const
+    {
+        return m_constant;
+    }
+
+    /* Add a constant value to the row constant.
+
+	The new value of the constant is returned.
+
+	*/
+    double add(double value)
+    {
+        return m_constant += value;
+    }
+
+    /* Insert a symbol into the row with a given coefficient.
+
+	If the symbol already exists in the row, the coefficient will be
+	added to the existing coefficient. If the resulting coefficient
+	is zero, the symbol will be removed from the row.
+
+	*/
+    void insert(const Symbol &symbol, double coefficient = 1.0)
+    {
+        if (nearZero(m_cells[symbol] += coefficient))
+            m_cells.erase(symbol);
+    }
+
+    /* Insert a row into this row with a given coefficient.
+
+	The constant and the cells of the other row will be multiplied by
+	the coefficient and added to this row. Any cell with a resulting
+	coefficient of zero will be removed from the row.
+
+	*/
+    void insert(const Row &other, double coefficient = 1.0)
+    {
+        typedef CellMap::const_iterator iter_t;
+        m_constant += other.m_constant * coefficient;
+        iter_t end = other.m_cells.end();
+        for (iter_t it = other.m_cells.begin(); it != end; ++it)
+        {
+            double coeff = it->second * coefficient;
+            if (nearZero(m_cells[it->first] += coeff))
+                m_cells.erase(it->first);
+        }
+    }
+
+    /* Remove the given symbol from the row.
+
+	*/
+    void remove(const Symbol &symbol)
+    {
+        CellMap::iterator it = m_cells.find(symbol);
+        if (it != m_cells.end())
+            m_cells.erase(it);
+    }
+
+    /* Reverse the sign of the constant and all cells in the row.
+
+	*/
+    void reverseSign()
+    {
+        typedef CellMap::iterator iter_t;
+        m_constant = -m_constant;
+        iter_t end = m_cells.end();
+        for (iter_t it = m_cells.begin(); it != end; ++it)
+            it->second = -it->second;
+    }
+
+    /* Solve the row for the given symbol.
+
+	This method assumes the row is of the form a * x + b * y + c = 0
+	and (assuming solve for x) will modify the row to represent the
+	right hand side of x = -b/a * y - c / a. The target symbol will
+	be removed from the row, and the constant and other cells will
+	be multiplied by the negative inverse of the target coefficient.
+
+	The given symbol *must* exist in the row.
+
+	*/
+    void solveFor(const Symbol &symbol)
+    {
+        typedef CellMap::iterator iter_t;
+        double coeff = -1.0 / m_cells[symbol];
+        m_cells.erase(symbol);
+        m_constant *= coeff;
+        iter_t end = m_cells.end();
+        for (iter_t it = m_cells.begin(); it != end; ++it)
+            it->second *= coeff;
+    }
+
+    /* Solve the row for the given symbols.
+
+	This method assumes the row is of the form x = b * y + c and will
+	solve the row such that y = x / b - c / b. The rhs symbol will be
+	removed from the row, the lhs added, and the result divided by the
+	negative inverse of the rhs coefficient.
+
+	The lhs symbol *must not* exist in the row, and the rhs symbol
+	*must* exist in the row.
+
+	*/
+    void solveFor(const Symbol &lhs, const Symbol &rhs)
+    {
+        insert(lhs, -1.0);
+        solveFor(rhs);
+    }
+
+    /* Get the coefficient for the given symbol.
+
+	If the symbol does not exist in the row, zero will be returned.
+
+	*/
+    double coefficientFor(const Symbol &symbol) const
+    {
+        CellMap::const_iterator it = m_cells.find(symbol);
+        if (it == m_cells.end())
+            return 0.0;
+        return it->second;
+    }
+
+    /* Substitute a symbol with the data from another row.
+
+	Given a row of the form a * x + b and a substitution of the
+	form x = 3 * y + c the row will be updated to reflect the
+	expression 3 * a * y + a * c + b.
+
+	If the symbol does not exist in the row, this is a no-op.
+
+	*/
+    void substitute(const Symbol &symbol, const Row &row)
+    {
+        typedef CellMap::iterator iter_t;
+        iter_t it = m_cells.find(symbol);
+        if (it != m_cells.end())
+        {
+            double coefficient = it->second;
+            m_cells.erase(it);
+            insert(row, coefficient);
+        }
+    }
+
+private:
+    CellMap m_cells;
+    double m_constant;
+};
+
+} // namespace impl
+
+} // namespace kiwi

thirdparty/include/kiwi/shareddata.h

@@ -0,0 +1,151 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+
+namespace kiwi
+{
+
+class SharedData
+{
+
+public:
+    SharedData() : m_refcount(0) {}
+
+    SharedData(const SharedData &other) : m_refcount(0) {}
+
+    int m_refcount;
+
+private:
+    SharedData &operator=(const SharedData &other);
+};
+
+template <typename T>
+class SharedDataPtr
+{
+
+public:
+    typedef T Type;
+
+    SharedDataPtr() : m_data(0) {}
+
+    explicit SharedDataPtr(T *data) : m_data(data)
+    {
+        incref(m_data);
+    }
+
+    ~SharedDataPtr()
+    {
+        decref(m_data);
+    }
+
+    T *data()
+    {
+        return m_data;
+    }
+
+    const T *data() const
+    {
+        return m_data;
+    }
+
+    operator T *()
+    {
+        return m_data;
+    }
+
+    operator const T *() const
+    {
+        return m_data;
+    }
+
+    T *operator->()
+    {
+        return m_data;
+    }
+
+    const T *operator->() const
+    {
+        return m_data;
+    }
+
+    T &operator*()
+    {
+        return *m_data;
+    }
+
+    const T &operator*() const
+    {
+        return *m_data;
+    }
+
+    bool operator!() const
+    {
+        return !m_data;
+    }
+
+    bool operator<(const SharedDataPtr<T> &other) const
+    {
+        return m_data < other.m_data;
+    }
+
+    bool operator==(const SharedDataPtr<T> &other) const
+    {
+        return m_data == other.m_data;
+    }
+
+    bool operator!=(const SharedDataPtr<T> &other) const
+    {
+        return m_data != other.m_data;
+    }
+
+    SharedDataPtr(const SharedDataPtr<T> &other) : m_data(other.m_data)
+    {
+        incref(m_data);
+    }
+
+    SharedDataPtr<T> &operator=(const SharedDataPtr<T> &other)
+    {
+        if (m_data != other.m_data)
+        {
+            T *temp = m_data;
+            m_data = other.m_data;
+            incref(m_data);
+            decref(temp);
+        }
+        return *this;
+    }
+
+    SharedDataPtr<T> &operator=(T *other)
+    {
+        if (m_data != other)
+        {
+            T *temp = m_data;
+            m_data = other;
+            incref(m_data);
+            decref(temp);
+        }
+        return *this;
+    }
+
+private:
+    static void incref(T *data)
+    {
+        if (data)
+            ++data->m_refcount;
+    }
+
+    static void decref(T *data)
+    {
+        if (data && --data->m_refcount == 0)
+            delete data;
+    }
+
+    T *m_data;
+};
+
+} // namespace kiwi

thirdparty/include/kiwi/solver.h

@@ -0,0 +1,178 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+#include "constraint.h"
+#include "debug.h"
+#include "solverimpl.h"
+#include "strength.h"
+#include "variable.h"
+
+
+namespace kiwi
+{
+
+class Solver
+{
+
+public:
+
+	Solver() {}
+
+	~Solver() {}
+
+	/* Add a constraint to the solver.
+
+	Throws
+	------
+	DuplicateConstraint
+		The given constraint has already been added to the solver.
+
+	UnsatisfiableConstraint
+		The given constraint is required and cannot be satisfied.
+
+	*/
+	void addConstraint( const Constraint& constraint )
+	{
+		m_impl.addConstraint( constraint );
+	}
+
+	/* Remove a constraint from the solver.
+
+	Throws
+	------
+	UnknownConstraint
+		The given constraint has not been added to the solver.
+
+	*/
+	void removeConstraint( const Constraint& constraint )
+	{
+		m_impl.removeConstraint( constraint );
+	}
+
+	/* Test whether a constraint has been added to the solver.
+
+	*/
+	bool hasConstraint( const Constraint& constraint ) const
+	{
+		return m_impl.hasConstraint( constraint );
+	}
+
+	/* Add an edit variable to the solver.
+
+	This method should be called before the `suggestValue` method is
+	used to supply a suggested value for the given edit variable.
+
+	Throws
+	------
+	DuplicateEditVariable
+		The given edit variable has already been added to the solver.
+
+	BadRequiredStrength
+		The given strength is >= required.
+
+	*/
+	void addEditVariable( const Variable& variable, double strength )
+	{
+		m_impl.addEditVariable( variable, strength );
+	}
+
+	/* Remove an edit variable from the solver.
+
+	Throws
+	------
+	UnknownEditVariable
+		The given edit variable has not been added to the solver.
+
+	*/
+	void removeEditVariable( const Variable& variable )
+	{
+		m_impl.removeEditVariable( variable );
+	}
+
+	/* Test whether an edit variable has been added to the solver.
+
+	*/
+	bool hasEditVariable( const Variable& variable ) const
+	{
+		return m_impl.hasEditVariable( variable );
+	}
+
+	/* Suggest a value for the given edit variable.
+
+	This method should be used after an edit variable as been added to
+	the solver in order to suggest the value for that variable. After
+	all suggestions have been made, the `solve` method can be used to
+	update the values of all variables.
+
+	Throws
+	------
+	UnknownEditVariable
+		The given edit variable has not been added to the solver.
+
+	*/
+	void suggestValue( const Variable& variable, double value )
+	{
+		m_impl.suggestValue( variable, value );
+	}
+
+	/* Update the values of the external solver variables.
+
+	*/
+	void updateVariables()
+	{
+		m_impl.updateVariables();
+	}
+
+	/* Reset the solver to the empty starting condition.
+
+	This method resets the internal solver state to the empty starting
+	condition, as if no constraints or edit variables have been added.
+	This can be faster than deleting the solver and creating a new one
+	when the entire system must change, since it can avoid unecessary
+	heap (de)allocations.
+
+	*/
+	void reset()
+	{
+		m_impl.reset();
+	}
+
+	/* Dump a representation of the solver internals to stdout.
+
+	*/
+	void dump()
+	{
+		debug::dump( m_impl );
+	}
+
+	/* Dump a representation of the solver internals to a stream.
+
+	*/
+	void dump( std::ostream& out )
+	{
+		debug::dump( m_impl, out );
+	}
+
+	/* Dump a representation of the solver internals to a string.
+
+	*/
+	std::string dumps()
+	{
+		return debug::dumps( m_impl );
+	}
+
+private:
+
+	Solver( const Solver& );
+
+	Solver& operator=( const Solver& );
+
+	impl::SolverImpl m_impl;
+};
+
+} // namespace kiwi

thirdparty/include/kiwi/solverimpl.h

@@ -0,0 +1,840 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+#include <algorithm>
+#include <limits>
+#include <memory>
+#include <vector>
+#include "constraint.h"
+#include "errors.h"
+#include "expression.h"
+#include "maptype.h"
+#include "row.h"
+#include "symbol.h"
+#include "term.h"
+#include "util.h"
+#include "variable.h"
+
+
+namespace kiwi
+{
+
+namespace impl
+{
+
+class SolverImpl
+{
+	friend class DebugHelper;
+
+	struct Tag
+	{
+		Symbol marker;
+		Symbol other;
+	};
+
+	struct EditInfo
+	{
+		Tag tag;
+		Constraint constraint;
+		double constant;
+	};
+
+	typedef MapType<Variable, Symbol> VarMap;
+
+	typedef MapType<Symbol, Row*> RowMap;
+
+	typedef MapType<Constraint, Tag> CnMap;
+
+	typedef MapType<Variable, EditInfo> EditMap;
+
+	struct DualOptimizeGuard
+	{
+		DualOptimizeGuard( SolverImpl& impl ) : m_impl( impl ) {}
+		~DualOptimizeGuard() { m_impl.dualOptimize(); }
+		SolverImpl& m_impl;
+	};
+
+public:
+
+	SolverImpl() : m_objective( new Row() ), m_id_tick( 1 ) {}
+
+	~SolverImpl() { clearRows(); }
+
+	/* Add a constraint to the solver.
+
+	Throws
+	------
+	DuplicateConstraint
+		The given constraint has already been added to the solver.
+
+	UnsatisfiableConstraint
+		The given constraint is required and cannot be satisfied.
+
+	*/
+	void addConstraint( const Constraint& constraint )
+	{
+		if( m_cns.find( constraint ) != m_cns.end() )
+			throw DuplicateConstraint( constraint );
+
+		// Creating a row causes symbols to be reserved for the variables
+		// in the constraint. If this method exits with an exception,
+		// then its possible those variables will linger in the var map.
+		// Since its likely that those variables will be used in other
+		// constraints and since exceptional conditions are uncommon,
+		// i'm not too worried about aggressive cleanup of the var map.
+		Tag tag;
+		std::unique_ptr<Row> rowptr( createRow( constraint, tag ) );
+		Symbol subject( chooseSubject( *rowptr, tag ) );
+
+		// If chooseSubject could not find a valid entering symbol, one
+		// last option is available if the entire row is composed of
+		// dummy variables. If the constant of the row is zero, then
+		// this represents redundant constraints and the new dummy
+		// marker can enter the basis. If the constant is non-zero,
+		// then it represents an unsatisfiable constraint.
+		if( subject.type() == Symbol::Invalid && allDummies( *rowptr ) )
+		{
+			if( !nearZero( rowptr->constant() ) )
+				throw UnsatisfiableConstraint( constraint );
+			else
+				subject = tag.marker;
+		}
+
+		// If an entering symbol still isn't found, then the row must
+		// be added using an artificial variable. If that fails, then
+		// the row represents an unsatisfiable constraint.
+		if( subject.type() == Symbol::Invalid )
+		{
+			if( !addWithArtificialVariable( *rowptr ) )
+				throw UnsatisfiableConstraint( constraint );
+		}
+		else
+		{
+			rowptr->solveFor( subject );
+			substitute( subject, *rowptr );
+			m_rows[ subject ] = rowptr.release();
+		}
+
+		m_cns[ constraint ] = tag;
+
+		// Optimizing after each constraint is added performs less
+		// aggregate work due to a smaller average system size. It
+		// also ensures the solver remains in a consistent state.
+		optimize( *m_objective );
+	}
+
+	/* Remove a constraint from the solver.
+
+	Throws
+	------
+	UnknownConstraint
+		The given constraint has not been added to the solver.
+
+	*/
+	void removeConstraint( const Constraint& constraint )
+	{
+		CnMap::iterator cn_it = m_cns.find( constraint );
+		if( cn_it == m_cns.end() )
+			throw UnknownConstraint( constraint );
+
+		Tag tag( cn_it->second );
+		m_cns.erase( cn_it );
+
+		// Remove the error effects from the objective function
+		// *before* pivoting, or substitutions into the objective
+		// will lead to incorrect solver results.
+		removeConstraintEffects( constraint, tag );
+
+		// If the marker is basic, simply drop the row. Otherwise,
+		// pivot the marker into the basis and then drop the row.
+		RowMap::iterator row_it = m_rows.find( tag.marker );
+		if( row_it != m_rows.end() )
+		{
+			std::unique_ptr<Row> rowptr( row_it->second );
+			m_rows.erase( row_it );
+		}
+		else
+		{
+			row_it = getMarkerLeavingRow( tag.marker );
+			if( row_it == m_rows.end() )
+				throw InternalSolverError( "failed to find leaving row" );
+			Symbol leaving( row_it->first );
+			std::unique_ptr<Row> rowptr( row_it->second );
+			m_rows.erase( row_it );
+			rowptr->solveFor( leaving, tag.marker );
+			substitute( tag.marker, *rowptr );
+		}
+
+		// Optimizing after each constraint is removed ensures that the
+		// solver remains consistent. It makes the solver api easier to
+		// use at a small tradeoff for speed.
+		optimize( *m_objective );
+	}
+
+	/* Test whether a constraint has been added to the solver.
+
+	*/
+	bool hasConstraint( const Constraint& constraint ) const
+	{
+		return m_cns.find( constraint ) != m_cns.end();
+	}
+
+	/* Add an edit variable to the solver.
+
+	This method should be called before the `suggestValue` method is
+	used to supply a suggested value for the given edit variable.
+
+	Throws
+	------
+	DuplicateEditVariable
+		The given edit variable has already been added to the solver.
+
+	BadRequiredStrength
+		The given strength is >= required.
+
+	*/
+	void addEditVariable( const Variable& variable, double strength )
+	{
+		if( m_edits.find( variable ) != m_edits.end() )
+			throw DuplicateEditVariable( variable );
+		strength = strength::clip( strength );
+		if( strength == strength::required )
+			throw BadRequiredStrength();
+		Constraint cn( Expression( variable ), OP_EQ, strength );
+		addConstraint( cn );
+		EditInfo info;
+		info.tag = m_cns[ cn ];
+		info.constraint = cn;
+		info.constant = 0.0;
+		m_edits[ variable ] = info;
+	}
+
+	/* Remove an edit variable from the solver.
+
+	Throws
+	------
+	UnknownEditVariable
+		The given edit variable has not been added to the solver.
+
+	*/
+	void removeEditVariable( const Variable& variable )
+	{
+		EditMap::iterator it = m_edits.find( variable );
+		if( it == m_edits.end() )
+			throw UnknownEditVariable( variable );
+		removeConstraint( it->second.constraint );
+		m_edits.erase( it );
+	}
+
+	/* Test whether an edit variable has been added to the solver.
+
+	*/
+	bool hasEditVariable( const Variable& variable ) const
+	{
+		return m_edits.find( variable ) != m_edits.end();
+	}
+
+	/* Suggest a value for the given edit variable.
+
+	This method should be used after an edit variable as been added to
+	the solver in order to suggest the value for that variable.
+
+	Throws
+	------
+	UnknownEditVariable
+		The given edit variable has not been added to the solver.
+
+	*/
+	void suggestValue( const Variable& variable, double value )
+	{
+		EditMap::iterator it = m_edits.find( variable );
+		if( it == m_edits.end() )
+			throw UnknownEditVariable( variable );
+
+		DualOptimizeGuard guard( *this );
+		EditInfo& info = it->second;
+		double delta = value - info.constant;
+		info.constant = value;
+
+		// Check first if the positive error variable is basic.
+		RowMap::iterator row_it = m_rows.find( info.tag.marker );
+		if( row_it != m_rows.end() )
+		{
+			if( row_it->second->add( -delta ) < 0.0 )
+				m_infeasible_rows.push_back( row_it->first );
+			return;
+		}
+
+		// Check next if the negative error variable is basic.
+		row_it = m_rows.find( info.tag.other );
+		if( row_it != m_rows.end() )
+		{
+			if( row_it->second->add( delta ) < 0.0 )
+				m_infeasible_rows.push_back( row_it->first );
+			return;
+		}
+
+		// Otherwise update each row where the error variables exist.
+		RowMap::iterator end = m_rows.end();
+		for( row_it = m_rows.begin(); row_it != end; ++row_it )
+		{
+			double coeff = row_it->second->coefficientFor( info.tag.marker );
+			if( coeff != 0.0 &&
+				row_it->second->add( delta * coeff ) < 0.0 &&
+				row_it->first.type() != Symbol::External )
+				m_infeasible_rows.push_back( row_it->first );
+		}
+	}
+
+	/* Update the values of the external solver variables.
+
+	*/
+	void updateVariables()
+	{
+		typedef RowMap::iterator row_iter_t;
+		typedef VarMap::iterator var_iter_t;
+		row_iter_t row_end = m_rows.end();
+		var_iter_t var_end = m_vars.end();
+		for( var_iter_t var_it = m_vars.begin(); var_it != var_end; ++var_it )
+		{
+			Variable& var( const_cast<Variable&>( var_it->first ) );
+			row_iter_t row_it = m_rows.find( var_it->second );
+			if( row_it == row_end )
+				var.setValue( 0.0 );
+			else
+				var.setValue( row_it->second->constant() );
+		}
+	}
+
+	/* Reset the solver to the empty starting condition.
+
+	This method resets the internal solver state to the empty starting
+	condition, as if no constraints or edit variables have been added.
+	This can be faster than deleting the solver and creating a new one
+	when the entire system must change, since it can avoid unecessary
+	heap (de)allocations.
+
+	*/
+	void reset()
+	{
+		clearRows();
+		m_cns.clear();
+		m_vars.clear();
+		m_edits.clear();
+		m_infeasible_rows.clear();
+		m_objective.reset( new Row() );
+		m_artificial.reset();
+		m_id_tick = 1;
+	}
+
+private:
+
+	SolverImpl( const SolverImpl& );
+
+	SolverImpl& operator=( const SolverImpl& );
+
+	struct RowDeleter
+	{
+		template<typename T>
+		void operator()( T& pair ) { delete pair.second; }
+	};
+
+	void clearRows()
+	{
+		std::for_each( m_rows.begin(), m_rows.end(), RowDeleter() );
+		m_rows.clear();
+	}
+
+	/* Get the symbol for the given variable.
+
+	If a symbol does not exist for the variable, one will be created.
+
+	*/
+	Symbol getVarSymbol( const Variable& variable )
+	{
+		VarMap::iterator it = m_vars.find( variable );
+		if( it != m_vars.end() )
+			return it->second;
+		Symbol symbol( Symbol::External, m_id_tick++ );
+		m_vars[ variable ] = symbol;
+		return symbol;
+	}
+
+	/* Create a new Row object for the given constraint.
+
+	The terms in the constraint will be converted to cells in the row.
+	Any term in the constraint with a coefficient of zero is ignored.
+	This method uses the `getVarSymbol` method to get the symbol for
+	the variables added to the row. If the symbol for a given cell
+	variable is basic, the cell variable will be substituted with the
+	basic row.
+
+	The necessary slack and error variables will be added to the row.
+	If the constant for the row is negative, the sign for the row
+	will be inverted so the constant becomes positive.
+
+	The tag will be updated with the marker and error symbols to use
+	for tracking the movement of the constraint in the tableau.
+
+	*/
+	Row* createRow( const Constraint& constraint, Tag& tag )
+	{
+		typedef std::vector<Term>::const_iterator iter_t;
+		const Expression& expr( constraint.expression() );
+		Row* row = new Row( expr.constant() );
+
+		// Substitute the current basic variables into the row.
+		iter_t end = expr.terms().end();
+		for( iter_t it = expr.terms().begin(); it != end; ++it )
+		{
+			if( !nearZero( it->coefficient() ) )
+			{
+				Symbol symbol( getVarSymbol( it->variable() ) );
+				RowMap::const_iterator row_it = m_rows.find( symbol );
+				if( row_it != m_rows.end() )
+					row->insert( *row_it->second, it->coefficient() );
+				else
+					row->insert( symbol, it->coefficient() );
+			}
+		}
+
+		// Add the necessary slack, error, and dummy variables.
+		switch( constraint.op() )
+		{
+			case OP_LE:
+			case OP_GE:
+			{
+				double coeff = constraint.op() == OP_LE ? 1.0 : -1.0;
+				Symbol slack( Symbol::Slack, m_id_tick++ );
+				tag.marker = slack;
+				row->insert( slack, coeff );
+				if( constraint.strength() < strength::required )
+				{
+					Symbol error( Symbol::Error, m_id_tick++ );
+					tag.other = error;
+					row->insert( error, -coeff );
+					m_objective->insert( error, constraint.strength() );
+				}
+				break;
+			}
+			case OP_EQ:
+			{
+				if( constraint.strength() < strength::required )
+				{
+					Symbol errplus( Symbol::Error, m_id_tick++ );
+					Symbol errminus( Symbol::Error, m_id_tick++ );
+					tag.marker = errplus;
+					tag.other = errminus;
+					row->insert( errplus, -1.0 ); // v = eplus - eminus
+					row->insert( errminus, 1.0 ); // v - eplus + eminus = 0
+					m_objective->insert( errplus, constraint.strength() );
+					m_objective->insert( errminus, constraint.strength() );
+				}
+				else
+				{
+					Symbol dummy( Symbol::Dummy, m_id_tick++ );
+					tag.marker = dummy;
+					row->insert( dummy );
+				}
+				break;
+			}
+		}
+
+		// Ensure the row as a positive constant.
+		if( row->constant() < 0.0 )
+			row->reverseSign();
+
+		return row;
+	}
+
+	/* Choose the subject for solving for the row.
+
+	This method will choose the best subject for using as the solve
+	target for the row. An invalid symbol will be returned if there
+	is no valid target.
+
+	The symbols are chosen according to the following precedence:
+
+	1) The first symbol representing an external variable.
+	2) A negative slack or error tag variable.
+
+	If a subject cannot be found, an invalid symbol will be returned.
+
+	*/
+	Symbol chooseSubject( const Row& row, const Tag& tag )
+	{
+		typedef Row::CellMap::const_iterator iter_t;
+		iter_t end = row.cells().end();
+		for( iter_t it = row.cells().begin(); it != end; ++it )
+		{
+			if( it->first.type() == Symbol::External )
+				return it->first;
+		}
+		if( tag.marker.type() == Symbol::Slack || tag.marker.type() == Symbol::Error )
+		{
+			if( row.coefficientFor( tag.marker ) < 0.0 )
+				return tag.marker;
+		}
+		if( tag.other.type() == Symbol::Slack || tag.other.type() == Symbol::Error )
+		{
+			if( row.coefficientFor( tag.other ) < 0.0 )
+				return tag.other;
+		}
+		return Symbol();
+	}
+
+ 	/* Add the row to the tableau using an artificial variable.
+
+	This will return false if the constraint cannot be satisfied.
+
+ 	*/
+ 	bool addWithArtificialVariable( const Row& row )
+ 	{
+		// Create and add the artificial variable to the tableau
+		Symbol art( Symbol::Slack, m_id_tick++ );
+		m_rows[ art ] = new Row( row );
+		m_artificial.reset( new Row( row ) );
+
+		// Optimize the artificial objective. This is successful
+		// only if the artificial objective is optimized to zero.
+		optimize( *m_artificial );
+		bool success = nearZero( m_artificial->constant() );
+		m_artificial.reset();
+
+		// If the artificial variable is not basic, pivot the row so that
+		// it becomes basic. If the row is constant, exit early.
+		RowMap::iterator it = m_rows.find( art );
+		if( it != m_rows.end() )
+		{
+			std::unique_ptr<Row> rowptr( it->second );
+			m_rows.erase( it );
+			if( rowptr->cells().empty() )
+				return success;
+			Symbol entering( anyPivotableSymbol( *rowptr ) );
+			if( entering.type() == Symbol::Invalid )
+				return false;  // unsatisfiable (will this ever happen?)
+			rowptr->solveFor( art, entering );
+			substitute( entering, *rowptr );
+			m_rows[ entering ] = rowptr.release();
+		}
+
+		// Remove the artificial variable from the tableau.
+		RowMap::iterator end = m_rows.end();
+		for( it = m_rows.begin(); it != end; ++it )
+			it->second->remove( art );
+		m_objective->remove( art );
+		return success;
+ 	}
+
+	/* Substitute the parametric symbol with the given row.
+
+	This method will substitute all instances of the parametric symbol
+	in the tableau and the objective function with the given row.
+
+	*/
+	void substitute( const Symbol& symbol, const Row& row )
+	{
+		typedef RowMap::iterator iter_t;
+		iter_t end = m_rows.end();
+		for( iter_t it = m_rows.begin(); it != end; ++it )
+		{
+			it->second->substitute( symbol, row );
+			if( it->first.type() != Symbol::External &&
+				it->second->constant() < 0.0 )
+				m_infeasible_rows.push_back( it->first );
+		}
+		m_objective->substitute( symbol, row );
+		if( m_artificial.get() )
+			m_artificial->substitute( symbol, row );
+	}
+
+	/* Optimize the system for the given objective function.
+
+	This method performs iterations of Phase 2 of the simplex method
+	until the objective function reaches a minimum.
+
+	Throws
+	------
+	InternalSolverError
+		The value of the objective function is unbounded.
+
+	*/
+	void optimize( const Row& objective )
+	{
+		while( true )
+		{
+			Symbol entering( getEnteringSymbol( objective ) );
+			if( entering.type() == Symbol::Invalid )
+				return;
+			RowMap::iterator it = getLeavingRow( entering );
+			if( it == m_rows.end() )
+				throw InternalSolverError( "The objective is unbounded." );
+			// pivot the entering symbol into the basis
+			Symbol leaving( it->first );
+			Row* row = it->second;
+			m_rows.erase( it );
+			row->solveFor( leaving, entering );
+			substitute( entering, *row );
+			m_rows[ entering ] = row;
+		}
+	}
+
+	/* Optimize the system using the dual of the simplex method.
+
+	The current state of the system should be such that the objective
+	function is optimal, but not feasible. This method will perform
+	an iteration of the dual simplex method to make the solution both
+	optimal and feasible.
+
+	Throws
+	------
+	InternalSolverError
+		The system cannot be dual optimized.
+
+	*/
+	void dualOptimize()
+	{
+		while( !m_infeasible_rows.empty() )
+		{
+
+			Symbol leaving( m_infeasible_rows.back() );
+			m_infeasible_rows.pop_back();
+			RowMap::iterator it = m_rows.find( leaving );
+			if( it != m_rows.end() && !nearZero( it->second->constant() ) &&
+				it->second->constant() < 0.0 )
+			{
+				Symbol entering( getDualEnteringSymbol( *it->second ) );
+				if( entering.type() == Symbol::Invalid )
+					throw InternalSolverError( "Dual optimize failed." );
+				// pivot the entering symbol into the basis
+				Row* row = it->second;
+				m_rows.erase( it );
+				row->solveFor( leaving, entering );
+				substitute( entering, *row );
+				m_rows[ entering ] = row;
+			}
+		}
+	}
+
+	/* Compute the entering variable for a pivot operation.
+
+	This method will return first symbol in the objective function which
+	is non-dummy and has a coefficient less than zero. If no symbol meets
+	the criteria, it means the objective function is at a minimum, and an
+	invalid symbol is returned.
+
+	*/
+	Symbol getEnteringSymbol( const Row& objective )
+	{
+		typedef Row::CellMap::const_iterator iter_t;
+		iter_t end = objective.cells().end();
+		for( iter_t it = objective.cells().begin(); it != end; ++it )
+		{
+			if( it->first.type() != Symbol::Dummy && it->second < 0.0 )
+				return it->first;
+		}
+		return Symbol();
+	}
+
+	/* Compute the entering symbol for the dual optimize operation.
+
+	This method will return the symbol in the row which has a positive
+	coefficient and yields the minimum ratio for its respective symbol
+	in the objective function. The provided row *must* be infeasible.
+	If no symbol is found which meats the criteria, an invalid symbol
+	is returned.
+
+	*/
+	Symbol getDualEnteringSymbol( const Row& row )
+	{
+		typedef Row::CellMap::const_iterator iter_t;
+		Symbol entering;
+		double ratio = std::numeric_limits<double>::max();
+		iter_t end = row.cells().end();
+		for( iter_t it = row.cells().begin(); it != end; ++it )
+		{
+			if( it->second > 0.0 && it->first.type() != Symbol::Dummy )
+			{
+				double coeff = m_objective->coefficientFor( it->first );
+				double r = coeff / it->second;
+				if( r < ratio )
+				{
+					ratio = r;
+					entering = it->first;
+				}
+			}
+		}
+		return entering;
+	}
+
+	/* Get the first Slack or Error symbol in the row.
+
+	If no such symbol is present, and Invalid symbol will be returned.
+
+	*/
+	Symbol anyPivotableSymbol( const Row& row )
+	{
+		typedef Row::CellMap::const_iterator iter_t;
+		iter_t end = row.cells().end();
+		for( iter_t it = row.cells().begin(); it != end; ++it )
+		{
+			const Symbol& sym( it->first );
+			if( sym.type() == Symbol::Slack || sym.type() == Symbol::Error )
+				return sym;
+		}
+		return Symbol();
+	}
+
+	/* Compute the row which holds the exit symbol for a pivot.
+
+	This method will return an iterator to the row in the row map
+	which holds the exit symbol. If no appropriate exit symbol is
+	found, the end() iterator will be returned. This indicates that
+	the objective function is unbounded.
+
+	*/
+	RowMap::iterator getLeavingRow( const Symbol& entering )
+	{
+		typedef RowMap::iterator iter_t;
+		double ratio = std::numeric_limits<double>::max();
+		iter_t end = m_rows.end();
+		iter_t found = m_rows.end();
+		for( iter_t it = m_rows.begin(); it != end; ++it )
+		{
+			if( it->first.type() != Symbol::External )
+			{
+				double temp = it->second->coefficientFor( entering );
+				if( temp < 0.0 )
+				{
+					double temp_ratio = -it->second->constant() / temp;
+					if( temp_ratio < ratio )
+					{
+						ratio = temp_ratio;
+						found = it;
+					}
+				}
+			}
+		}
+		return found;
+	}
+
+	/* Compute the leaving row for a marker variable.
+
+	This method will return an iterator to the row in the row map
+	which holds the given marker variable. The row will be chosen
+	according to the following precedence:
+
+	1) The row with a restricted basic varible and a negative coefficient
+	   for the marker with the smallest ratio of -constant / coefficient.
+
+	2) The row with a restricted basic variable and the smallest ratio
+	   of constant / coefficient.
+
+	3) The last unrestricted row which contains the marker.
+
+	If the marker does not exist in any row, the row map end() iterator
+	will be returned. This indicates an internal solver error since
+	the marker *should* exist somewhere in the tableau.
+
+	*/
+	RowMap::iterator getMarkerLeavingRow( const Symbol& marker )
+	{
+		const double dmax = std::numeric_limits<double>::max();
+		typedef RowMap::iterator iter_t;
+		double r1 = dmax;
+		double r2 = dmax;
+		iter_t end = m_rows.end();
+		iter_t first = end;
+		iter_t second = end;
+		iter_t third = end;
+		for( iter_t it = m_rows.begin(); it != end; ++it )
+		{
+			double c = it->second->coefficientFor( marker );
+			if( c == 0.0 )
+				continue;
+			if( it->first.type() == Symbol::External )
+			{
+				third = it;
+			}
+			else if( c < 0.0 )
+			{
+				double r = -it->second->constant() / c;
+				if( r < r1 )
+				{
+					r1 = r;
+					first = it;
+				}
+			}
+			else
+			{
+				double r = it->second->constant() / c;
+				if( r < r2 )
+				{
+					r2 = r;
+					second = it;
+				}
+			}
+		}
+		if( first != end )
+			return first;
+		if( second != end )
+			return second;
+		return third;
+	}
+
+	/* Remove the effects of a constraint on the objective function.
+
+	*/
+	void removeConstraintEffects( const Constraint& cn, const Tag& tag )
+	{
+		if( tag.marker.type() == Symbol::Error )
+			removeMarkerEffects( tag.marker, cn.strength() );
+		if( tag.other.type() == Symbol::Error )
+			removeMarkerEffects( tag.other, cn.strength() );
+	}
+
+	/* Remove the effects of an error marker on the objective function.
+
+	*/
+	void removeMarkerEffects( const Symbol& marker, double strength )
+	{
+		RowMap::iterator row_it = m_rows.find( marker );
+		if( row_it != m_rows.end() )
+			m_objective->insert( *row_it->second, -strength );
+		else
+			m_objective->insert( marker, -strength );
+	}
+
+	/* Test whether a row is composed of all dummy variables.
+
+	*/
+	bool allDummies( const Row& row )
+	{
+		typedef Row::CellMap::const_iterator iter_t;
+		iter_t end = row.cells().end();
+		for( iter_t it = row.cells().begin(); it != end; ++it )
+		{
+			if( it->first.type() != Symbol::Dummy )
+				return false;
+		}
+		return true;
+	}
+
+	CnMap m_cns;
+	RowMap m_rows;
+	VarMap m_vars;
+	EditMap m_edits;
+	std::vector<Symbol> m_infeasible_rows;
+	std::unique_ptr<Row> m_objective;
+	std::unique_ptr<Row> m_artificial;
+	Symbol::Id m_id_tick;
+};
+
+} // namespace impl
+
+} // namespace kiwi

thirdparty/include/kiwi/strength.h

@@ -0,0 +1,44 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+#include <algorithm>
+
+
+namespace kiwi
+{
+
+namespace strength
+{
+
+inline double create( double a, double b, double c, double w = 1.0 )
+{
+	double result = 0.0;
+	result += std::max( 0.0, std::min( 1000.0, a * w ) ) * 1000000.0;
+	result += std::max( 0.0, std::min( 1000.0, b * w ) ) * 1000.0;
+	result += std::max( 0.0, std::min( 1000.0, c * w ) );
+	return result;
+}
+
+
+const double required = create( 1000.0, 1000.0, 1000.0 );
+
+const double strong = create( 1.0, 0.0, 0.0 );
+
+const double medium = create( 0.0, 1.0, 0.0 );
+
+const double weak = create( 0.0, 0.0, 1.0 );
+
+
+inline double clip( double value )
+{
+	return std::max( 0.0, std::min( required, value ) );
+}
+
+} // namespace strength
+
+} // namespace kiwi

thirdparty/include/kiwi/symbol.h

@@ -0,0 +1,68 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+
+
+namespace kiwi
+{
+
+namespace impl
+{
+
+class Symbol
+{
+
+public:
+
+	typedef unsigned long long Id;
+
+	enum Type
+	{
+		Invalid,
+		External,
+		Slack,
+		Error,
+		Dummy
+	};
+
+	Symbol() : m_id( 0 ), m_type( Invalid ) {}
+
+	Symbol( Type type, Id id ) : m_id( id ), m_type( type ) {}
+
+	~Symbol() {}
+
+	Id id() const
+	{
+		return m_id;
+	}
+
+	Type type() const
+	{
+		return m_type;
+	}
+
+private:
+
+	Id m_id;
+	Type m_type;
+
+	friend bool operator<( const Symbol& lhs, const Symbol& rhs )
+	{
+		return lhs.m_id < rhs.m_id;
+	}
+
+	friend bool operator==( const Symbol& lhs, const Symbol& rhs )
+	{
+		return lhs.m_id == rhs.m_id;
+	}
+
+};
+
+} // namespace impl
+
+} // namespace kiwi

thirdparty/include/kiwi/symbolics.h

@@ -0,0 +1,685 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+#include <vector>
+#include "constraint.h"
+#include "expression.h"
+#include "term.h"
+#include "variable.h"
+
+
+namespace kiwi
+{
+
+// Variable multiply, divide, and unary invert
+
+inline
+Term operator*( const Variable& variable, double coefficient )
+{
+	return Term( variable, coefficient );
+}
+
+
+inline
+Term operator/( const Variable& variable, double denominator )
+{
+	return variable * ( 1.0 / denominator );
+}
+
+
+inline
+Term operator-( const Variable& variable )
+{
+	return variable * -1.0;
+}
+
+
+// Term multiply, divide, and unary invert
+
+inline
+Term operator*( const Term& term, double coefficient )
+{
+	return Term( term.variable(), term.coefficient() * coefficient );
+}
+
+
+inline
+Term operator/( const Term& term, double denominator )
+{
+	return term * ( 1.0 / denominator );
+}
+
+
+inline
+Term operator-( const Term& term )
+{
+	return term * -1.0;
+}
+
+
+// Expression multiply, divide, and unary invert
+
+inline
+Expression operator*( const Expression& expression, double coefficient )
+{
+	std::vector<Term> terms;
+	terms.reserve( expression.terms().size() );
+	typedef std::vector<Term>::const_iterator iter_t;
+	iter_t begin = expression.terms().begin();
+	iter_t end = expression.terms().end();
+	for( iter_t it = begin; it != end; ++it )
+		terms.push_back( ( *it ) * coefficient );
+	return Expression( terms, expression.constant() * coefficient );
+}
+
+
+inline
+Expression operator/( const Expression& expression, double denominator )
+{
+	return expression * ( 1.0 / denominator );
+}
+
+
+inline
+Expression operator-( const Expression& expression )
+{
+	return expression * -1.0;
+}
+
+
+// Double multiply
+
+inline
+Expression operator*( double coefficient, const Expression& expression )
+{
+	return expression * coefficient;
+}
+
+
+inline
+Term operator*( double coefficient, const Term& term )
+{
+	return term * coefficient;
+}
+
+
+inline
+Term operator*( double coefficient, const Variable& variable )
+{
+	return variable * coefficient;
+}
+
+
+// Expression add and subtract
+
+inline
+Expression operator+( const Expression& first, const Expression& second )
+{
+	std::vector<Term> terms;
+	terms.reserve( first.terms().size() + second.terms().size() );
+	terms.insert( terms.begin(), first.terms().begin(), first.terms().end() );
+	terms.insert( terms.end(), second.terms().begin(), second.terms().end() );
+	return Expression( terms, first.constant() + second.constant() );
+}
+
+
+inline
+Expression operator+( const Expression& first, const Term& second )
+{
+	std::vector<Term> terms;
+	terms.reserve( first.terms().size() + 1 );
+	terms.insert( terms.begin(), first.terms().begin(), first.terms().end() );
+	terms.push_back( second );
+	return Expression( terms, first.constant() );
+}
+
+
+inline
+Expression operator+( const Expression& expression, const Variable& variable )
+{
+	return expression + Term( variable );
+}
+
+
+inline
+Expression operator+( const Expression& expression, double constant )
+{
+	return Expression( expression.terms(), expression.constant() + constant );
+}
+
+
+inline
+Expression operator-( const Expression& first, const Expression& second )
+{
+	return first + -second;
+}
+
+
+inline
+Expression operator-( const Expression& expression, const Term& term )
+{
+	return expression + -term;
+}
+
+
+inline
+Expression operator-( const Expression& expression, const Variable& variable )
+{
+	return expression + -variable;
+}
+
+
+inline
+Expression operator-( const Expression& expression, double constant )
+{
+	return expression + -constant;
+}
+
+
+// Term add and subtract
+
+inline
+Expression operator+( const Term& term, const Expression& expression )
+{
+	return expression + term;
+}
+
+
+inline
+Expression operator+( const Term& first, const Term& second )
+{
+	std::vector<Term> terms;
+	terms.reserve( 2 );
+	terms.push_back( first );
+	terms.push_back( second );
+	return Expression( terms );
+}
+
+
+inline
+Expression operator+( const Term& term, const Variable& variable )
+{
+	return term + Term( variable );
+}
+
+
+inline
+Expression operator+( const Term& term, double constant )
+{
+	return Expression( term, constant );
+}
+
+
+inline
+Expression operator-( const Term& term, const Expression& expression )
+{
+	return -expression + term;
+}
+
+
+inline
+Expression operator-( const Term& first, const Term& second )
+{
+	return first + -second;
+}
+
+
+inline
+Expression operator-( const Term& term, const Variable& variable )
+{
+	return term + -variable;
+}
+
+
+inline
+Expression operator-( const Term& term, double constant )
+{
+	return term + -constant;
+}
+
+
+// Variable add and subtract
+
+inline
+Expression operator+( const Variable& variable, const Expression& expression )
+{
+	return expression + variable;
+}
+
+
+inline
+Expression operator+( const Variable& variable, const Term& term )
+{
+	return term + variable;
+}
+
+
+inline
+Expression operator+( const Variable& first, const Variable& second )
+{
+	return Term( first ) + second;
+}
+
+
+inline
+Expression operator+( const Variable& variable, double constant )
+{
+	return Term( variable ) + constant;
+}
+
+
+inline
+Expression operator-( const Variable& variable, const Expression& expression )
+{
+	return variable + -expression;
+}
+
+
+inline
+Expression operator-( const Variable& variable, const Term& term )
+{
+	return variable + -term;
+}
+
+
+inline
+Expression operator-( const Variable& first, const Variable& second )
+{
+	return first + -second;
+}
+
+
+inline
+Expression operator-( const Variable& variable, double constant )
+{
+	return variable + -constant;
+}
+
+
+// Double add and subtract
+
+inline
+Expression operator+( double constant, const Expression& expression )
+{
+	return expression + constant;
+}
+
+
+inline
+Expression operator+( double constant, const Term& term )
+{
+	return term + constant;
+}
+
+
+inline
+Expression operator+( double constant, const Variable& variable )
+{
+	return variable + constant;
+}
+
+
+inline
+Expression operator-( double constant, const Expression& expression )
+{
+	return -expression + constant;
+}
+
+
+inline
+Expression operator-( double constant, const Term& term )
+{
+	return -term + constant;
+}
+
+
+inline
+Expression operator-( double constant, const Variable& variable )
+{
+	return -variable + constant;
+}
+
+
+// Expression relations
+
+inline
+Constraint operator==( const Expression& first, const Expression& second )
+{
+	return Constraint( first - second, OP_EQ );
+}
+
+
+inline
+Constraint operator==( const Expression& expression, const Term& term )
+{
+	return expression == Expression( term );
+}
+
+
+inline
+Constraint operator==( const Expression& expression, const Variable& variable )
+{
+	return expression == Term( variable );
+}
+
+
+inline
+Constraint operator==( const Expression& expression, double constant )
+{
+	return expression == Expression( constant );
+}
+
+
+inline
+Constraint operator<=( const Expression& first, const Expression& second )
+{
+	return Constraint( first - second, OP_LE );
+}
+
+
+inline
+Constraint operator<=( const Expression& expression, const Term& term )
+{
+	return expression <= Expression( term );
+}
+
+
+inline
+Constraint operator<=( const Expression& expression, const Variable& variable )
+{
+	return expression <= Term( variable );
+}
+
+
+inline
+Constraint operator<=( const Expression& expression, double constant )
+{
+	return expression <= Expression( constant );
+}
+
+
+inline
+Constraint operator>=( const Expression& first, const Expression& second )
+{
+	return Constraint( first - second, OP_GE );
+}
+
+
+inline
+Constraint operator>=( const Expression& expression, const Term& term )
+{
+	return expression >= Expression( term );
+}
+
+
+inline
+Constraint operator>=( const Expression& expression, const Variable& variable )
+{
+	return expression >= Term( variable );
+}
+
+
+inline
+Constraint operator>=( const Expression& expression, double constant )
+{
+	return expression >= Expression( constant );
+}
+
+
+// Term relations
+
+inline
+Constraint operator==( const Term& term, const Expression& expression )
+{
+	return expression == term;
+}
+
+
+inline
+Constraint operator==( const Term& first, const Term& second )
+{
+	return Expression( first ) == second;
+}
+
+
+inline
+Constraint operator==( const Term& term, const Variable& variable )
+{
+	return Expression( term ) == variable;
+}
+
+
+inline
+Constraint operator==( const Term& term, double constant )
+{
+	return Expression( term ) == constant;
+}
+
+
+inline
+Constraint operator<=( const Term& term, const Expression& expression )
+{
+	return expression >= term;
+}
+
+
+inline
+Constraint operator<=( const Term& first, const Term& second )
+{
+	return Expression( first ) <= second;
+}
+
+
+inline
+Constraint operator<=( const Term& term, const Variable& variable )
+{
+	return Expression( term ) <= variable;
+}
+
+
+inline
+Constraint operator<=( const Term& term, double constant )
+{
+	return Expression( term ) <= constant;
+}
+
+
+inline
+Constraint operator>=( const Term& term, const Expression& expression )
+{
+	return expression <= term;
+}
+
+
+inline
+Constraint operator>=( const Term& first, const Term& second )
+{
+	return Expression( first ) >= second;
+}
+
+
+inline
+Constraint operator>=( const Term& term, const Variable& variable )
+{
+	return Expression( term ) >= variable;
+}
+
+
+inline
+Constraint operator>=( const Term& term, double constant )
+{
+	return Expression( term ) >= constant;
+}
+
+
+// Variable relations
+inline
+Constraint operator==( const Variable& variable, const Expression& expression )
+{
+	return expression == variable;
+}
+
+
+inline
+Constraint operator==( const Variable& variable, const Term& term )
+{
+	return term == variable;
+}
+
+
+inline
+Constraint operator==( const Variable& first, const Variable& second )
+{
+	return Term( first ) == second;
+}
+
+
+inline
+Constraint operator==( const Variable& variable, double constant )
+{
+	return Term( variable ) == constant;
+}
+
+
+inline
+Constraint operator<=( const Variable& variable, const Expression& expression )
+{
+	return expression >= variable;
+}
+
+
+inline
+Constraint operator<=( const Variable& variable, const Term& term )
+{
+	return term >= variable;
+}
+
+
+inline
+Constraint operator<=( const Variable& first, const Variable& second )
+{
+	return Term( first ) <= second;
+}
+
+
+inline
+Constraint operator<=( const Variable& variable, double constant )
+{
+	return Term( variable ) <= constant;
+}
+
+
+inline
+Constraint operator>=( const Variable& variable, const Expression& expression )
+{
+	return expression <= variable;
+}
+
+
+inline
+Constraint operator>=( const Variable& variable, const Term& term )
+{
+	return term <= variable;
+}
+
+
+inline
+Constraint operator>=( const Variable& first, const Variable& second )
+{
+	return Term( first ) >= second;
+}
+
+
+inline
+Constraint operator>=( const Variable& variable, double constant )
+{
+	return Term( variable ) >= constant;
+}
+
+
+// Double relations
+
+inline
+Constraint operator==( double constant, const Expression& expression )
+{
+	return expression == constant;
+}
+
+
+inline
+Constraint operator==( double constant, const Term& term )
+{
+	return term == constant;
+}
+
+
+inline
+Constraint operator==( double constant, const Variable& variable )
+{
+	return variable == constant;
+}
+
+
+inline
+Constraint operator<=( double constant, const Expression& expression )
+{
+	return expression >= constant;
+}
+
+
+inline
+Constraint operator<=( double constant, const Term& term )
+{
+	return term >= constant;
+}
+
+
+inline
+Constraint operator<=( double constant, const Variable& variable )
+{
+	return variable >= constant;
+}
+
+
+inline
+Constraint operator>=( double constant, const Expression& expression )
+{
+	return expression <= constant;
+}
+
+
+inline
+Constraint operator>=( double constant, const Term& term )
+{
+	return term <= constant;
+}
+
+
+inline
+Constraint operator>=( double constant, const Variable& variable )
+{
+	return variable <= constant;
+}
+
+
+// Constraint strength modifier
+
+inline
+Constraint operator|( const Constraint& constraint, double strength )
+{
+	return Constraint( constraint, strength );
+}
+
+
+inline
+Constraint operator|( double strength, const Constraint& constraint )
+{
+	return constraint | strength;
+}
+
+} // namespace kiwi

thirdparty/include/kiwi/term.h

@@ -0,0 +1,51 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+#include <utility>
+#include "variable.h"
+
+
+namespace kiwi
+{
+
+class Term
+{
+
+public:
+
+	Term( const Variable& variable, double coefficient = 1.0 ) :
+		m_variable( variable ), m_coefficient( coefficient ) {}
+
+	// to facilitate efficient map -> vector copies
+	Term( const std::pair<const Variable, double>& pair ) :
+		m_variable( pair.first ), m_coefficient( pair.second ) {}
+
+	~Term() {}
+
+	const Variable& variable() const
+	{
+		return m_variable;
+	}
+
+	double coefficient() const
+	{
+		return m_coefficient;
+	}
+
+	double value() const
+	{
+		return m_coefficient * m_variable.value();
+	}
+
+private:
+
+	Variable m_variable;
+	double m_coefficient;
+};
+
+} // namespace kiwi

thirdparty/include/kiwi/util.h

@@ -0,0 +1,24 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+
+namespace kiwi
+{
+
+namespace impl
+{
+
+inline bool nearZero(double value)
+{
+    const double eps = 1.0e-8;
+    return value < 0.0 ? -value < eps : value < eps;
+}
+
+} // namespace impl
+
+} // namespace kiwi

thirdparty/include/kiwi/variable.h

@@ -0,0 +1,111 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2017, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+#include <memory>
+#include <string>
+#include "shareddata.h"
+
+namespace kiwi
+{
+
+class Variable
+{
+
+public:
+    class Context
+    {
+    public:
+        Context() {}
+        virtual ~Context() {} // LCOV_EXCL_LINE
+    };
+
+    Variable(Context *context = 0) : m_data(new VariableData("", context)) {}
+
+    Variable(const std::string &name, Context *context = 0) : m_data(new VariableData(name, context)) {}
+
+    Variable(const char *name, Context *context = 0) : m_data(new VariableData(name, context)) {}
+
+    ~Variable() {}
+
+    const std::string &name() const
+    {
+        return m_data->m_name;
+    }
+
+    void setName(const char *name)
+    {
+        m_data->m_name = name;
+    }
+
+    void setName(const std::string &name)
+    {
+        m_data->m_name = name;
+    }
+
+    Context *context() const
+    {
+        return m_data->m_context.get();
+    }
+
+    void setContext(Context *context)
+    {
+        m_data->m_context.reset(context);
+    }
+
+    double value() const
+    {
+        return m_data->m_value;
+    }
+
+    void setValue(double value)
+    {
+        m_data->m_value = value;
+    }
+
+    // operator== is used for symbolics
+    bool equals(const Variable &other)
+    {
+        return m_data == other.m_data;
+    }
+
+private:
+    class VariableData : public SharedData
+    {
+
+    public:
+        VariableData(const std::string &name, Context *context) : SharedData(),
+                                                                  m_name(name),
+                                                                  m_context(context),
+                                                                  m_value(0.0) {}
+
+        VariableData(const char *name, Context *context) : SharedData(),
+                                                           m_name(name),
+                                                           m_context(context),
+                                                           m_value(0.0) {}
+
+        ~VariableData() {}
+
+        std::string m_name;
+        std::unique_ptr<Context> m_context;
+        double m_value;
+
+    private:
+        VariableData(const VariableData &other);
+
+        VariableData &operator=(const VariableData &other);
+    };
+
+    SharedDataPtr<VariableData> m_data;
+
+    friend bool operator<(const Variable &lhs, const Variable &rhs)
+    {
+        return lhs.m_data < rhs.m_data;
+    }
+};
+
+} // namespace kiwi

thirdparty/include/kiwi/version.h

@@ -0,0 +1,14 @@
+/*-----------------------------------------------------------------------------
+| Copyright (c) 2013-2020, Nucleic Development Team.
+|
+| Distributed under the terms of the Modified BSD License.
+|
+| The full license is in the file LICENSE, distributed with this software.
+|----------------------------------------------------------------------------*/
+#pragma once
+
+#define KIWI_MAJOR_VERSION 1
+#define KIWI_MINOR_VERSION 2
+#define KIWI_MICRO_VERSION 0
+#define KIWI_VERSION_HEX 0x010200
+#define KIWI_VERSION "1.2.0"

thirdparty/lib/common/x64/Newton_d.dll.REMOVED.git-id

@@ -1 +0,0 @@
-0248a702d5e35c14dc8adfb6f0d4ba7d18d90c8b

thirdparty/lib/common/x86/Newton_d.dll.REMOVED.git-id

@@ -1 +0,0 @@
-c1629ecc8c31a6a8f6648448bc655824e35435b0

thirdparty/lib/gmake/x64/libNewton.a.REMOVED.git-id

@@ -1 +0,0 @@
-f79ffcc0c10c1bffafba367583e3675a15fa9167

thirdparty/lib/gmake/x86/libNewton.a.REMOVED.git-id

@@ -1 +0,0 @@
-50c86d0a541fd1757e41ef7f8a368d969395fdf6

thirdparty/src/newton/dContainers/CMakeLists.txt

@@ -0,0 +1,61 @@
+# Copyright (c) <2014-2017> <Newton Game Dynamics>
+#
+# This software is provided 'as-is', without any express or implied
+# warranty. In no event will the authors be held liable for any damages
+# arising from the use of this software.
+#
+# Permission is granted to anyone to use this software for any purpose,
+# including commercial applications, and to alter it and redistribute it
+# freely.
+
+cmake_minimum_required(VERSION 3.4.0)
+
+set (projectName "dContainers")
+message (${projectName})
+
+#source and header files
+file(GLOB CPP_SOURCE *.h *.cpp)
+file(GLOB HEADERS *.h)
+
+source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}/" FILES ${CPP_SOURCE})
+
+if(NEWTON_BUILD_SHARED_LIBS)
+	if(MSVC OR MINGW)
+        add_definitions(-D_DCONTAINERS_DLL)
+        add_definitions(-D_DCONTAINERS_EXPORT)
+    endif()
+    add_library(${projectName} SHARED ${CPP_SOURCE})
+else(NEWTON_BUILD_SHARED_LIBS)
+	if (MSVC OR MINGW)
+        add_definitions(-D_NEWTON_STATIC_LIB)
+    endif()
+
+    add_library(${projectName} STATIC ${CPP_SOURCE})
+endif(NEWTON_BUILD_SHARED_LIBS)
+
+target_include_directories(${projectName} PUBLIC . ../dMath ../dgCore)
+if (NEWTON_BUILD_PROFILER)
+    target_link_libraries (${projectName} dProfiler)
+endif()
+
+if (MSVC)
+    if(CMAKE_VS_MSBUILD_COMMAND OR CMAKE_VS_DEVENV_COMMAND)
+	    set_target_properties(${projectName} PROPERTIES COMPILE_FLAGS "/YudContainersStdAfx.h")
+        set_source_files_properties(dContainersStdAfx.cpp PROPERTIES COMPILE_FLAGS "/YcdContainersStdAfx.h")
+    endif()
+
+	if (NEWTON_BUILD_SANDBOX_DEMOS)
+		add_custom_command(
+			TARGET ${projectName} POST_BUILD
+			COMMAND ${CMAKE_COMMAND}
+			ARGS -E copy $<TARGET_FILE:${projectName}> ${PROJECT_BINARY_DIR}/applications/demosSandbox/${CMAKE_CFG_INTDIR}/$<TARGET_FILE_NAME:${projectName}>)
+	endif ()
+endif(MSVC)
+
+install(TARGETS ${projectName} 
+		LIBRARY DESTINATION lib
+		ARCHIVE DESTINATION lib
+		RUNTIME DESTINATION bin)
+
+install(FILES ${HEADERS} DESTINATION include/${projectName})
+

thirdparty/src/newton/dContainers/dArray.h

@@ -0,0 +1,114 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#ifndef __D_ARRAY__
+#define __D_ARRAY__
+
+#include "dContainersAlloc.h"
+
+template<class T>
+class dArray: public dContainersAlloc
+{
+	public:
+	dArray();
+	dArray(int size);
+	~dArray();
+
+	T& operator[] (int i);
+	const T& operator[] (int i) const;
+
+	int GetSize() const;
+	void Resize(int size) const;
+
+	protected:
+	mutable int m_capacity;
+	mutable T* m_data;
+};
+
+template<class T>
+dArray<T>::dArray()
+	:m_capacity(0)
+	,m_data(NULL)
+{
+}
+
+template<class T>
+dArray<T>::dArray(int size)
+	:dContainersAlloc()
+	,m_capacity(0)
+	,m_data(NULL)
+{
+	Resize(size);
+}
+
+
+template<class T>
+dArray<T>::~dArray()
+{
+	if (m_data) {
+		delete[] m_data;
+	}
+}
+
+
+template<class T>
+T& dArray<T>::operator[] (int i)
+{
+	dAssert(i >= 0);
+	while (i >= m_capacity) {
+		Resize(dMax (i * 2, 1));
+	}
+	return m_data[i];
+}
+
+template<class T>
+const T& dArray<T>::operator[] (int i) const
+{
+	dAssert(i >= 0);
+	while (i >= m_capacity) {
+		Resize(dMax (i * 2, 1));
+	}
+	return m_data[i];
+}
+
+template<class T>
+int dArray <T>::GetSize() const
+{
+	return m_capacity;
+}
+
+template<class T>
+void dArray <T>::Resize(int size) const
+{
+	if (size >= m_capacity) {
+		T* const newArray = new T[size];
+		if (m_data) {
+			for (int i = 0; i < m_capacity; i++) {
+				newArray[i] = m_data[i];
+			}
+			delete[] m_data;
+		}
+		m_data = newArray;
+		m_capacity = size;
+	} else if (size < m_capacity) {
+		T* const newArray = new T[size];
+		if (m_data) {
+			for (int i = 0; i < size; i++) {
+				newArray[i] = m_data[i];
+			}
+			delete[] m_data;
+		}
+		m_data = newArray;
+		m_capacity = size;
+	}
+}
+
+#endif

thirdparty/src/newton/dContainers/dBaseHierarchy.cpp

@@ -0,0 +1,148 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dContainersStdAfx.h"
+#include "dBaseHierarchy.h"
+
+
+dBaseHierarchy::dBaseHierarchy (const dBaseHierarchy &clone)
+{
+	Clear ();
+	SetNameID (clone.m_name.GetStr());
+	for (dBaseHierarchy* obj = clone.m_child; obj; obj = obj->m_sibling) {
+		dBaseHierarchy* const newObj = obj->CreateClone ();
+		newObj->Attach (this);
+	}
+}
+
+
+dBaseHierarchy::~dBaseHierarchy () 
+{
+	Detach();
+	while (m_child) {
+		delete m_child;
+	}
+}
+
+
+
+void dBaseHierarchy::Attach (dBaseHierarchy* const parentArg, bool addFirst)
+{
+	m_parent = parentArg;
+	if (m_parent->m_child) {
+		if (addFirst) {
+			m_sibling = m_parent->m_child;
+			m_parent->m_child = this;
+		} else {
+			dBaseHierarchy* obj = m_parent->m_child;
+			for (; obj->m_sibling; obj = obj->m_sibling);
+			obj->m_sibling = this;
+		}
+	} else {
+		m_parent->m_child = this;
+	}
+}
+
+
+void dBaseHierarchy::Detach ()
+{
+ 	if (m_parent) {
+		if (m_parent->m_child == this) {
+			m_parent->m_child = m_sibling;
+		} else {
+			dBaseHierarchy* ptr = m_parent->m_child;
+			for (; ptr->m_sibling != this; ptr = ptr->m_sibling);
+			ptr->m_sibling = m_sibling;
+		}
+		m_parent = NULL;
+		m_sibling = NULL;
+	}
+}
+	
+
+dBaseHierarchy* dBaseHierarchy::GetRoot() const
+{
+	const dBaseHierarchy* root = this;
+	for (; root->m_parent; root = root->m_parent);
+	return (dBaseHierarchy*)root;
+}
+
+
+dBaseHierarchy* dBaseHierarchy::GetFirst() const
+{
+	dBaseHierarchy* ptr = (dBaseHierarchy*) this;
+	for (; ptr->m_child; ptr = ptr->m_child);
+	return ptr;
+}
+
+dBaseHierarchy* dBaseHierarchy::GetNext() const
+{
+	if (m_sibling) {
+		return m_sibling->GetFirst();
+	}
+
+	dBaseHierarchy* ptr = m_parent;
+	dBaseHierarchy* x = (dBaseHierarchy *)this;
+	for (; ptr && (x == ptr->m_sibling); ptr = ptr->m_parent) {
+		x = ptr;
+	}
+	return ptr;
+}
+
+
+
+dBaseHierarchy* dBaseHierarchy::GetLast() const
+{
+	dBaseHierarchy* ptr = (dBaseHierarchy*) this;
+	for (; ptr->m_sibling; ptr = ptr->m_sibling);
+	return ptr;
+}
+
+
+dBaseHierarchy* dBaseHierarchy::GetPrev() const
+{
+
+	if (m_child) {
+		return m_child->GetNext();
+	}
+
+	dBaseHierarchy* ptr = m_parent;
+	dBaseHierarchy* x = (dBaseHierarchy *)this;
+	for (; ptr && (x == ptr->m_child); ptr = ptr->m_child) {
+		x = ptr;
+	}
+	return ptr;
+}
+
+
+dBaseHierarchy* dBaseHierarchy::Find (dCRCTYPE nameCRC) const 
+{
+	if (nameCRC == GetNameID()) {
+		return (dBaseHierarchy*)this;
+	} else {
+		for (dBaseHierarchy* ptr = GetFirst(); ptr && (ptr != this); ptr = ptr->GetNext()) {
+			if (nameCRC == ptr->GetNameID()) {
+				return ptr;
+			}
+		}
+	}
+
+	return NULL;
+}
+
+
+
+
+
+
+
+
+

thirdparty/src/newton/dContainers/dBaseHierarchy.h

@@ -0,0 +1,256 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+
+#ifndef __Hierarchy__
+#define __Hierarchy__
+
+#include "dCRC.h"
+#include "dString.h"
+#include "dContainersAlloc.h"
+
+
+
+class dBaseHierarchy: public dContainersAlloc
+{
+	public:
+	DCONTAINERS_API dBaseHierarchy* GetChild () const;
+	DCONTAINERS_API dBaseHierarchy* GetParent () const;
+	DCONTAINERS_API dBaseHierarchy* GetSibling () const;
+
+	DCONTAINERS_API void Detach ();
+	DCONTAINERS_API void Attach (dBaseHierarchy* const parent, bool addFirst = false);
+	
+	DCONTAINERS_API dBaseHierarchy* GetRoot () const;
+	DCONTAINERS_API dBaseHierarchy* GetFirst() const;
+	DCONTAINERS_API dBaseHierarchy* GetLast() const;
+	DCONTAINERS_API dBaseHierarchy* GetNext() const;
+	DCONTAINERS_API dBaseHierarchy* GetPrev() const;
+
+	DCONTAINERS_API dBaseHierarchy* Find (dCRCTYPE nameCRC) const; 
+	DCONTAINERS_API dBaseHierarchy* Find (const char* const name) const;
+
+	DCONTAINERS_API long long GetNameID() const;
+	DCONTAINERS_API const dString& GetName() const;
+	void SetNameID(const char* const name);
+	
+	protected:
+	DCONTAINERS_API dBaseHierarchy ();
+	DCONTAINERS_API dBaseHierarchy (const char* const name);
+	DCONTAINERS_API dBaseHierarchy (const dBaseHierarchy &clone);
+	virtual DCONTAINERS_API ~dBaseHierarchy ();
+
+	virtual DCONTAINERS_API dBaseHierarchy* CreateClone () const = 0;
+
+	private:
+	inline void Clear();
+
+	dString m_name;
+	long long m_nameID;
+	dBaseHierarchy* m_parent;
+	dBaseHierarchy* m_child;
+	dBaseHierarchy* m_sibling;
+};
+
+template<class T>
+class dHierarchy: public dBaseHierarchy
+{
+	public:
+	dHierarchy ();
+	dHierarchy (const char* const name);
+	void Attach (T* const parent, bool addFirst = false);
+	void Detach ();
+	T* GetChild () const;
+	T* GetParent () const;
+	T* GetSibling () const;
+	T* GetRoot () const;
+	T* GetFirst() const;
+	T* GetLast() const;
+	T* GetNext() const;
+	T* GetPrev() const;
+	T* Find (long long nameCRC) const;
+	T* Find (const char* const name) const;
+
+	protected:
+	dHierarchy (const T &clone);
+	virtual ~dHierarchy ();
+};
+
+
+inline dBaseHierarchy::dBaseHierarchy ()
+{
+	Clear ();
+}
+
+inline dBaseHierarchy::dBaseHierarchy (const char* const name)
+{
+	Clear ();
+	SetNameID (name);
+}
+
+
+inline void dBaseHierarchy::Clear()
+{
+	m_child = NULL;
+	m_parent = NULL;
+	m_sibling = NULL;
+	m_nameID = 0;
+	m_name = (char*)NULL;
+}
+
+
+inline dBaseHierarchy* dBaseHierarchy::GetChild () const
+{
+	return m_child;
+}
+
+inline dBaseHierarchy* dBaseHierarchy::GetSibling () const
+{
+	return m_sibling;
+}
+
+inline dBaseHierarchy* dBaseHierarchy::GetParent () const
+{
+	return m_parent;
+}
+
+
+inline dBaseHierarchy* dBaseHierarchy::Find (const char* const name) const
+{
+	return Find (dCRC64 (name)); 
+} 
+
+inline void dBaseHierarchy::SetNameID(const char* const name)
+{
+	m_nameID = dCRC64 (name);
+	m_name = name;
+}
+
+inline long long  dBaseHierarchy::GetNameID() const
+{
+	return m_nameID;
+}
+
+inline const dString& dBaseHierarchy::GetName() const
+{
+	return m_name;
+}
+
+
+template<class T>
+dHierarchy<T>::dHierarchy ()
+	:dBaseHierarchy ()
+{
+}
+
+template<class T>
+dHierarchy<T>::dHierarchy (const T &clone)
+	:dBaseHierarchy (clone)
+{
+}
+
+template<class T>
+dHierarchy<T>::dHierarchy (const char* const name)
+	:dBaseHierarchy (name)
+{
+}
+
+template<class T>
+dHierarchy<T>::~dHierarchy () 
+{
+}
+
+
+//template<class T>
+//dBaseHierarchy* dHierarchy<T>::CreateClone () const
+//{
+//	return new T (*(T*)this);
+//}
+
+template<class T>
+void dHierarchy<T>::Attach (T* const parent, bool addFirst)
+{
+	dBaseHierarchy::Attach(parent, addFirst);
+}
+
+template<class T>
+void dHierarchy<T>::Detach ()
+{
+	dBaseHierarchy::Detach ();
+}
+
+template<class T>
+T* dHierarchy<T>::GetChild () const
+{
+	return (T*) dBaseHierarchy::GetChild();
+}
+
+template<class T>
+T* dHierarchy<T>::GetSibling () const
+{
+	return (T*) dBaseHierarchy::GetSibling ();
+}
+
+template<class T>
+T* dHierarchy<T>::GetParent () const
+{
+	return (T*) dBaseHierarchy::GetParent ();
+}
+
+
+template<class T>
+T* dHierarchy<T>::GetRoot () const
+{
+	return (T*) dBaseHierarchy::GetRoot ();
+}
+
+
+template<class T>
+T* dHierarchy<T>::GetFirst() const
+{
+	return (T*) dBaseHierarchy::GetFirst ();
+}
+
+template<class T>
+T* dHierarchy<T>::GetLast() const
+{
+	return (T*) dBaseHierarchy::GetLast ();
+}
+
+
+template<class T>
+T* dHierarchy<T>::GetNext() const
+{
+	return (T*) dBaseHierarchy::GetNext ();
+}
+
+template<class T>
+T* dHierarchy<T>::GetPrev() const
+{
+	return (T*) dBaseHierarchy::GetPrev ();
+}
+
+
+template<class T>
+T* dHierarchy<T>::Find (dCRCTYPE nameCRC) const 
+{
+	return (T*) dBaseHierarchy::Find (nameCRC);
+}
+
+template<class T>
+T* dHierarchy<T>::Find (const char* const name) const
+{
+	return (T*) dBaseHierarchy::Find (name);
+} 
+
+
+#endif
+

thirdparty/src/newton/dContainers/dBezierSpline.cpp

@@ -0,0 +1,641 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dContainersStdAfx.h"
+#include "dBezierSpline.h"
+
+dBezierSpline::dBezierSpline ()
+	:dContainersAlloc()
+	,m_knotVector(16)
+	,m_controlPoints(16)
+	,m_degree(0)
+	,m_knotsCount(0)
+	,m_controlPointsCount(0)
+{
+}
+
+dBezierSpline::dBezierSpline (const dBezierSpline& src)
+	:m_knotVector(src.m_knotsCount)
+	,m_controlPoints(src.m_controlPointsCount)
+	,m_degree(0)
+	,m_knotsCount(0)
+	,m_controlPointsCount(0)
+{
+	if (src.m_knotsCount) {
+		CreateFromKnotVectorAndControlPoints (src.m_degree, src.m_knotsCount - 2 * src.m_degree, &src.m_knotVector[src.m_degree], &src.m_controlPoints[0]);
+	}
+}
+
+dBezierSpline::~dBezierSpline ()
+{
+	Clear();
+}
+
+void dBezierSpline::Clear()
+{
+	m_degree = 0;
+	m_knotsCount = 0;
+	m_controlPointsCount = 0;
+}
+
+dBezierSpline& dBezierSpline::operator = (const dBezierSpline &copy)
+{
+	Clear();
+	if (copy.m_knotsCount) {
+		CreateFromKnotVectorAndControlPoints (copy.m_degree, copy.m_knotsCount - 2 * copy.m_degree, &copy.m_knotVector[copy.m_degree], &copy.m_controlPoints[0]);
+	}
+	return *this;
+}
+
+int dBezierSpline::GetDegree () const
+{
+	return m_degree;
+}
+
+void dBezierSpline::CreateFromKnotVectorAndControlPoints (int degree, int knotCount, const dFloat64* const knotVector, const dBigVector* const controlPoints)
+{
+	Clear();
+	dAssert (knotCount);
+	dAssert (knotVector[0] == 0.0f);
+	dAssert (knotVector[knotCount - 1] == 1.0f);
+
+	m_degree = degree;
+	m_knotsCount = knotCount + 2 * degree;
+	m_controlPointsCount = knotCount + m_degree - 1;
+
+	for (int i = 0; i < m_controlPointsCount; i++) {
+		m_controlPoints[i] = controlPoints[i];
+	}
+
+	for (int i = 0; i < m_degree; i ++) {
+		m_knotVector[i] = dFloat64(0.0f);
+		m_knotVector[i + m_knotsCount - m_degree] = dFloat64(1.0f);
+	}
+
+	for (int i = 0; i < knotCount; i ++) {
+		m_knotVector[i + m_degree] = knotVector[i];
+		dAssert (m_knotVector[i + m_degree] >= m_knotVector[i + m_degree - 1]);
+	}
+}
+
+int dBezierSpline::GetKnotCount() const
+{
+	return m_knotsCount;
+}
+
+dArray<dFloat64>& dBezierSpline::GetKnotArray()
+{
+	return m_knotVector;
+}
+
+const dArray<dFloat64>& dBezierSpline::GetKnotArray() const
+{
+	return m_knotVector;
+}
+
+dFloat64 dBezierSpline::GetKnot(int i) const
+{
+	return m_knotVector[i];
+}
+
+int dBezierSpline::GetControlPointCount() const
+{
+	return m_controlPointsCount;
+}
+
+dBigVector dBezierSpline::GetControlPoint(int i) const
+{
+	return m_controlPoints[i];
+}
+
+void dBezierSpline::SetControlPoint(int i, const dBigVector& point)
+{
+	m_controlPoints[i] = point;
+}
+
+dArray<dBigVector>& dBezierSpline::GetControlPointArray()
+{
+	return m_controlPoints;
+}
+
+const dArray<dBigVector>& dBezierSpline::GetControlPointArray() const
+{
+	return m_controlPoints;
+}
+
+int dBezierSpline::GetSpan(dFloat64 u) const
+{
+	int low = m_degree;
+	int high = m_knotsCount - m_degree - 1;
+
+	dAssert (u >= 0.0f);
+	dAssert (u <= 1.0f);
+	while ((high - low) >= 4) {
+		int mid = (low + high) >> 1;
+		if (u > m_knotVector[mid]) {
+			low = mid;
+		} else {
+			high = mid;
+		}
+	}
+
+	dAssert (m_knotVector[low] <= u);
+	for (int i = low; i < m_degree + m_knotsCount + 1; i ++) {
+		if (m_knotVector[i + 1] >= u) {
+			return i;
+		}
+	}
+	dAssert (0);
+	return 0;
+}
+
+void dBezierSpline::BasicsFunctions (dFloat64 u, int span, dFloat64* const BasicFunctionsOut) const
+{
+	BasicFunctionsOut[0] = 1.0f;
+
+	dFloat64* const left = dAlloca(dFloat64, m_knotsCount + 32);
+	dFloat64* const right = dAlloca(dFloat64, m_knotsCount + 32);
+
+	for (int j = 1; j <= m_degree; j ++) {
+		left[j] = u - m_knotVector[span + 1 - j]; 
+		right[j] = m_knotVector[span + j] - u;
+
+		dFloat64 saved = 0.0f;
+		for (int r = 0; r < j; r ++) {
+			dFloat64 temp = BasicFunctionsOut[r] / (right[r + 1] + left[j - r]);
+			BasicFunctionsOut[r] = saved + temp * right[r + 1];
+			saved = temp * left[j - r];
+		}
+		BasicFunctionsOut[j] = saved;
+	}
+}
+
+void dBezierSpline::BasicsFunctionsDerivatives (dFloat64 u, int span, dFloat64* const derivativesOut) const
+{
+	dFloat64* const a = dAlloca(dFloat64, m_knotsCount + 32);
+	dFloat64* const ndu = dAlloca(dFloat64, m_knotsCount + 32);
+	dFloat64* const left = dAlloca(dFloat64, m_knotsCount + 32);
+	dFloat64* const right = dAlloca(dFloat64, m_knotsCount + 32);
+
+	const int width = m_degree + 1;
+	ndu[0] = 1.0f;
+	for (int j = 1; j <= m_degree; j ++) {
+		left[j] = u - m_knotVector[span + 1 - j];
+		right[j] = m_knotVector[span + j] - u;
+		dFloat64 saved = 0.0f;
+		for (int r = 0; r < j; r ++) {
+			ndu[j * width + r] = right[r + 1] + left[j - r];
+
+			dFloat64 temp = ndu[r * width + j - 1] / ndu[j * width + r];
+			ndu[r * width + j] = saved + temp * right[r + 1];
+			saved = temp * left[j - r];
+		}
+		ndu[j * width + j] = saved;
+	}
+
+
+	for (int j = 0; j <= m_degree; j ++) {
+		derivativesOut[width * 0 + j] = ndu [width * j + m_degree];
+	}
+
+	for (int r = 0; r <= m_degree; r ++) {
+		int s1 = 0;
+		int s2 = 1;
+		a[0] = 1.0f;
+		for (int k = 1; k <= m_degree; k ++) {
+			dFloat64 d = 0.0f;
+			int rk = r - k;
+			int pk = m_degree - k;
+			if (r >= k)  {
+				a[width * s2 + 0] = a[width * s1 + 0] / ndu[width * (pk + 1) + rk];
+				d = a[width * s2 + 0] * ndu[width * rk + pk];
+			}
+			int j1 = 0;
+			int j2 = 0;
+			if (rk >= -1) {
+				j1 = 1;
+			} else {
+				j1 = -rk;
+			}
+
+			if ((r - 1) <= pk) {
+				j2 = k-1;
+			} else {
+				j2 = m_degree-r;
+			}
+			for (int j = j1; j <= j2; j ++) {
+				a[width * s2 + j] = (a[width * s1 + j] - a[width * s1 + j - 1]) / ndu[width * (pk + 1) + rk + j];
+				d += a[width * s2 + j] * ndu[width * (rk + j) + pk];
+			}
+			if (r <= pk) {
+				a[width * s2 + k] = -a[width * s1 + k - 1] / ndu[width * (pk + 1) + r];
+				d += a[width * s2 + k] * ndu[width * r + pk];
+			}
+			derivativesOut[width * k + r] = d;
+			dSwap(s1, s2);
+		}
+	}
+
+	int s = m_degree;
+	for (int k = 1; k <= m_degree; k ++) {
+		for (int j = 0; j <= m_degree; j ++) {
+			derivativesOut[width * k + j] *= s;
+		}
+		s *= (m_degree - k);
+	}
+}
+
+dBigVector dBezierSpline::CurvePoint (dFloat64 u, int span) const
+{
+	dBigVector point (0.0f);
+	dFloat64* const basicFunctions = dAlloca(dFloat64, m_knotsCount + 32);
+	BasicsFunctions (u, span, basicFunctions);
+	for (int i = 0; i <= m_degree; i ++) {
+		point += m_controlPoints[span - m_degree + i].Scale (basicFunctions[i]);
+	}
+	return point;
+}
+
+dBigVector dBezierSpline::CurvePoint (dFloat64 u) const
+{
+	u = dClamp (u, dFloat64 (0.0f), dFloat64 (1.0f));
+	int span = GetSpan(u);
+	return CurvePoint (u, span);
+}
+
+dBigVector dBezierSpline::CurveDerivative (dFloat64 u, int index) const
+{
+	u = dClamp (u, dFloat64 (0.0f), dFloat64 (1.0f));
+	dAssert (index <= m_degree);
+	
+	dFloat64* const basicsFuncDerivatives = dAlloca(dFloat64, m_knotsCount + 32);
+	int span = GetSpan(u);
+	BasicsFunctionsDerivatives (u, span, basicsFuncDerivatives);
+
+	const int with = m_degree + 1;
+	dBigVector point (0.0f);
+	for (int i = 0; i <= m_degree; i ++) {
+		point += m_controlPoints[span - m_degree + i].Scale (basicsFuncDerivatives[with * index + i]);
+	}
+	return point;
+}
+
+int dBezierSpline::CurveAllDerivatives (dFloat64 u, dBigVector* const derivatives) const
+{
+	u = dMod (u, dFloat64(1.0f));
+	dFloat64* const basicsFuncDerivatives = dAlloca(dFloat64, m_knotsCount + 32);
+	int span = GetSpan(u);
+	BasicsFunctionsDerivatives (u, span, basicsFuncDerivatives);
+
+	const int with = m_degree + 1;
+	dBigVector point (0.0f);
+	for (int j = 0; j <= m_degree; j ++) {
+		dBigVector ck (0.0f);
+		for (int i = 0; i <= m_degree; i ++) {
+			ck += m_controlPoints[span - m_degree + i].Scale (basicsFuncDerivatives[with * j + i]);
+		}
+		derivatives[j] = ck;
+	}
+
+	return m_degree + 1;
+}
+
+void dBezierSpline::GlobalCubicInterpolation (int count, const dBigVector* const points, const dBigVector& firstTangent, const dBigVector& lastTangent)
+{
+	CreateCubicKnotVector (count, points);
+	CreateCubicControlPoints (count, points, firstTangent, lastTangent);
+}
+
+void dBezierSpline::CreateCubicKnotVector(int count, const dBigVector* const points)
+{
+	dFloat64 d = dFloat64(0.0f);
+	dAssert (count >= 2);
+
+	dFloat64* const u = dAlloca(dFloat64, m_knotsCount + 32);
+	u[0] = dFloat64(0.0f);
+	for (int i = 1; i < count; i ++) {
+		dBigVector step (points[i] - points[i - 1]);
+		dFloat64 len = dSqrt (step.DotProduct3(step));
+		u[i] = dSqrt (len); 
+		d += u[i];
+	}
+
+	for (int i = 1; i < count; i ++) {
+		u[i] = u[i-1] + u[i] / d;
+	}
+	u[0] = dFloat64 (0.0f);
+	u[count - 1] = dFloat64(1.0f);
+
+	m_degree = 3;
+	m_knotsCount = count + 2 * m_degree;
+
+	for (int i = 0; i < (m_degree + 1); i ++) {
+		m_knotVector[i] = dFloat64(0.0f);
+		m_knotVector[i + m_knotsCount - m_degree - 1] = dFloat64(1.0f);
+	}
+
+	for (int i = 1; i < (count - 1); i ++) {
+		dFloat64 acc = dFloat64 (0.0f);
+		for (int j = 0; j < m_degree; j ++) {
+			acc += u[j + i - 1];
+		}
+		m_knotVector[m_degree + i] = acc / dFloat64 (3.0f);
+	}
+}
+
+void dBezierSpline::CreateCubicControlPoints(int count, const dBigVector* const points, const dBigVector& firstTangent, const dBigVector& lastTangent)
+{
+	dFloat64 abc[4];
+	if ((m_knotsCount - 2 * (m_degree - 1)) != m_controlPointsCount) {
+		m_controlPointsCount = m_knotsCount - 2 * (m_degree - 1);
+	}
+
+	m_controlPoints[0] = points[0];
+	m_controlPoints[m_controlPointsCount - 1] = points[count - 1];
+
+	m_controlPoints[1] = m_controlPoints[0] + firstTangent.Scale (m_knotVector[m_degree + 1] / 3.0f);
+	m_controlPoints[m_controlPointsCount - 2] = m_controlPoints[m_controlPointsCount - 1] - lastTangent.Scale ((1.0f - m_knotVector[m_knotsCount - m_degree - 2]) / 3.0f);
+	if (count == 3) {
+		BasicsFunctions (m_knotVector[m_degree + 1], m_degree + 1, abc);
+		m_controlPoints[2]  = points[1] - m_controlPoints[1].Scale (abc[0]) - m_controlPoints[3].Scale (abc[2]);
+		m_controlPoints[2] = m_controlPoints[2].Scale (1.0f / abc[1]);
+	} else {
+		dFloat64* const dd = dAlloca(dFloat64, m_knotsCount + 32);
+		BasicsFunctions (m_knotVector[m_degree + 1], m_degree + 1, abc);
+		dFloat64 den = abc[1];
+		m_controlPoints[2]  = (points[1] - m_controlPoints[1].Scale (abc[0])).Scale (1.0f / den);
+		for (int i = 3; i < (count - 1); i ++) {
+			dd[i + 1] = abc[2] / den;
+			BasicsFunctions (m_knotVector[i + 2], i + 2, abc);
+			den = abc[1] - abc[0] * dd[i + 1];
+			m_controlPoints[i]  = (points[i - 1] - m_controlPoints[i - 1].Scale (abc[0])).Scale (1.0f / den);
+		}
+
+		dd[count] = abc[2] / den;
+		BasicsFunctions (m_knotVector[count + 1], count + 1, abc);
+		den = abc[1] - abc[0] * dd[count];
+		m_controlPoints[count - 1] = (points[count - 2] - m_controlPoints[count].Scale (abc[2]) - m_controlPoints[count - 2].Scale (abc[0])).Scale (1.0f / den);
+
+		for (int i = count - 2; i >= 2; i --) {
+			m_controlPoints[i] -= m_controlPoints[i + 1].Scale (dd[i + 2]);
+		}
+	}
+}
+
+dFloat64 dBezierSpline::CalculateLength (dFloat64 tol) const
+{
+	dBigVector stackPool[32][3];
+	int stack = 0;
+
+	dFloat64 length = 0.0f;
+	dFloat64 tol2 = tol * tol;
+	dFloat64 u0 = m_knotVector[m_degree];
+	dBigVector p0 (CurvePoint (u0));
+
+	for (int i = m_degree; i < (m_knotsCount - m_degree - 1); i ++) {
+		dFloat64 u1 = m_knotVector[i + 1];
+		dBigVector p1 (CurvePoint (u1));
+		stackPool[stack][0] = p0;
+		stackPool[stack][1] = p1;
+		stackPool[stack][2] = dBigVector (u0, u1, dFloat64(0.0f), dFloat64(0.0f));
+		stack ++;
+		while (stack) {
+			stack --;
+			dBigVector q0 (stackPool[stack][0]);
+			dBigVector q1 (stackPool[stack][1]);
+			dFloat64 t0 = stackPool[stack][2][0];
+			dFloat64 t1 = stackPool[stack][2][1];
+			dFloat64 t01 = (t1 + t0) * 0.5f;
+
+			dBigVector p01 ((q1 + q0).Scale (0.5f));
+			dBigVector q01 (CurvePoint (t01));
+			dBigVector err (q01 - p01);
+
+			dFloat64 err2 = err.DotProduct3(err);
+			if (err2 < tol2) {
+				dBigVector step (q1 - q0);
+				length += dSqrt (step.DotProduct3(step));
+			} else {
+				stackPool[stack][0] = q01;
+				stackPool[stack][1] = q1;
+				stackPool[stack][2] = dBigVector (t01, t1, dFloat64(0.0f), dFloat64(0.0f));
+				stack ++;
+
+				stackPool[stack][0] = q0;
+				stackPool[stack][1] = q01;
+				stackPool[stack][2] = dBigVector (t0, t01, dFloat64(0.0f), dFloat64(0.0f));
+				stack ++;
+			}
+		}
+		u0 = u1;
+		p0 = p1;
+	}
+
+	return length;
+}
+
+void dBezierSpline::InsertKnot (dFloat64 u)
+{
+	const int k = GetSpan(u);
+	int multiplicity = 0;
+	for (int i = 0; i < m_degree; i ++) {
+		multiplicity += (dAbs (m_knotVector[k + i + 1] - u) < dFloat64 (1.0e-5f)) ? 1 : 0;
+	}
+	if (multiplicity == m_degree) {
+		return;
+	}
+
+	for (int i = m_knotsCount; i > (k + 1); i --) {
+		m_knotVector[i] = m_knotVector[i - 1];
+	}
+	m_knotVector[k + 1] = u;
+
+	dBigVector Rw[16];
+	for (int i = 0; i <= m_degree; i ++) {
+		Rw[i] = m_controlPoints[k - m_degree + i];
+	}
+
+	const int m = k - m_degree + 1;
+	dAssert(m >= 0);
+	dAssert((k + 1 - 1 - 0) >= 0);
+	dAssert((m_degree - 1 - 0) >= 0);
+
+	for (int i = 0; i <= (m_degree - 1); i ++) {
+		dFloat64 alpha = (u  - m_knotVector[m + i]) / (m_knotVector[i + k + 1] - m_knotVector[m + i]);
+		Rw[i] = Rw[i + 1].Scale (alpha) + Rw[i].Scale (dFloat64 (1.0f) - alpha);
+	}
+
+	for (int i = m_controlPointsCount; i > k; i--) {
+		m_controlPoints[i] = m_controlPoints[i - 1];
+	}
+	m_controlPoints[m] = Rw[0];
+	m_controlPoints[k + 1 - 1 - 0] = Rw[m_degree - 1 - 0];
+	for (int i = m + 1; i < k; i++) {
+		dAssert((i - m) >= 0);
+		m_controlPoints[i] = Rw[i - m];
+	}
+
+	m_knotsCount ++;
+	m_controlPointsCount ++;
+}
+
+bool dBezierSpline::RemoveKnot (dFloat64 u, dFloat64 tol)
+{
+	int r = GetSpan(u) + 1;
+	dAssert (m_knotVector[r - 1] < u);
+	if (dAbs (m_knotVector[r] - u) > 1.0e-5f) {
+		return false;
+	}
+
+	int s = 1;
+	int last = r - s;
+	int first = r - m_degree;
+	int ord = m_degree + 1;
+	dBigVector temp[16];
+
+	bool removableFlag = false;
+	int t = 0;
+	for ( ; t < m_degree; t ++) {
+		int off = first - 1;
+		temp[0] = m_controlPoints[off];
+		temp[last + 1 - off] = m_controlPoints[last + 1];
+		int i = first;
+		int j = last;
+		int ii = 1;
+		int jj = last - off;
+
+		while ((j - i) > t) {
+			dFloat64 alpha_i = (u - m_knotVector[i]) / (m_knotVector[i + ord + t] - m_knotVector[i]);
+			dFloat64 alpha_j = (u - m_knotVector[j - t]) / (m_knotVector[j + ord] - m_knotVector[j - t]);
+			temp[ii] = (m_controlPoints[i] - temp[ii - 1].Scale (dFloat64 (1.0f) - alpha_i)).Scale (dFloat64 (1.0f) / alpha_i);
+			temp[jj] = (m_controlPoints[j] - temp[jj + 1].Scale (alpha_j)).Scale (dFloat64 (1.0f) / (dFloat64 (1.0f) - alpha_j));
+			i ++;
+			j --;
+			ii ++;
+			jj --;
+		}
+		if ((j - i) < t) {
+			dBigVector diff (temp[ii - 1] - temp[jj + 1]);
+			removableFlag = diff.DotProduct3(diff) < (tol * tol);
+		} else {
+			dFloat64 alpha_i = (u - m_knotVector[i]) / (m_knotVector[i + ord + t] - m_knotVector[i]);
+			dBigVector p (temp[ii + t + 1].Scale (alpha_i) + temp[ii - 1].Scale (dFloat64 (1.0f) - alpha_i));
+			dBigVector diff (m_controlPoints[i] - p);
+			removableFlag = diff.DotProduct3(diff) < (tol * tol);
+		}
+		if (!removableFlag) {
+			break;
+		}
+
+		i = first;
+		j = last;
+		while ((j - 1) > t) {
+			m_controlPoints[i] = temp[i - off];
+			m_controlPoints[j] = temp[j - off];
+			i ++;
+			j --;
+		}
+		first --;
+		last ++;
+	}
+
+	if (t) {
+		for (int k = r + t; k < m_knotsCount; k ++) {
+			m_knotVector[k - t] = m_knotVector[k];
+		}
+
+		int fOut = (2 * r - s - m_degree) / 2;
+		int j = fOut;
+		int i = j;
+		for (int k = 1; k < t; k ++) {
+			if ((k % 2) == 1) {
+				i ++;
+			} else {
+				j = j - 1;
+			}
+		}
+		for (int k = i + 1; k < m_controlPointsCount; k ++) {
+			m_controlPoints[j] = m_controlPoints[k];
+			j ++;
+		}
+
+		m_knotsCount -= t;
+		m_controlPointsCount -= t;
+	}
+
+
+	return removableFlag;
+}
+
+dFloat64 dBezierSpline::FindClosestKnot(dBigVector& closestPoint, const dBigVector& point, int subdivitionSteps) const
+{
+	int startSpan = m_degree;
+	dFloat64 bestU = 0.0f;
+	dFloat64 distance2 = 1.0e10f;
+	dBigVector closestControlPoint(m_controlPoints[0]);
+	subdivitionSteps = dMax(subdivitionSteps, 1);
+	dFloat64 scale = 1.0f / subdivitionSteps;
+	for (int span = m_degree; span < (m_knotsCount - m_degree - 1); span++) {
+		dFloat64 param = 0.0f;
+		for (int i = 0; i < subdivitionSteps; i++) {
+			dFloat64 u = m_knotVector[span] + (m_knotVector[span + 1] - m_knotVector[span]) * param;
+			param += scale;
+			dBigVector p(CurvePoint(u, span));
+			dBigVector dp(p - point);
+			dFloat64 dist2 = dp.DotProduct3(dp);
+			if (dist2 < distance2) {
+				bestU = u;
+				startSpan = span;
+				distance2 = dist2;
+				closestControlPoint = p;
+			}
+		}
+	}
+
+	dBigVector p(CurvePoint(0.999f));
+	dBigVector dp(p - point);
+	dFloat64 dist2 = dp.DotProduct3(dp);
+	if (dist2 < distance2) {
+		bestU = dFloat64(0.999f);
+		startSpan = m_knotsCount - m_degree - 2;
+		closestControlPoint = p;
+	}
+
+	dBigVector derivatives[32];
+	dFloat64 u0 = bestU;
+
+	bool stop = false;
+	for (int i = 0; (i < 20) && !stop; i++) {
+		CurveAllDerivatives(u0, derivatives);
+
+		dBigVector dist(closestControlPoint - point);
+		dFloat64 num = derivatives[1].DotProduct3(dist);
+		dFloat64 den = derivatives[2].DotProduct3(dist) + derivatives[1].DotProduct3(derivatives[1]);
+		dFloat64 u1 = dClamp(u0 - num / den, dFloat64(0.0), dFloat64(1.0));
+		if (u1 < m_knotVector[startSpan]) {
+			startSpan--;
+			dAssert(startSpan >= 0);
+		} else if (u1 >= m_knotVector[startSpan + 1]) {
+			startSpan++;
+			dAssert(startSpan < (m_knotsCount - m_degree));
+		}
+
+		dAssert(startSpan >= m_degree);
+		dAssert(startSpan <= (m_knotsCount - m_degree - 1));
+		closestControlPoint = CurvePoint(u1, startSpan);
+
+		stop |= (dAbs(u1 - u0) < 1.0e-10) || (num * num < ((dist.DotProduct3(dist)) * (derivatives[1].DotProduct3(derivatives[1])) * 1.0e-10));
+		u0 = u1;
+	}
+
+	closestPoint = closestControlPoint;
+	return u0;
+}

thirdparty/src/newton/dContainers/dBezierSpline.h

@@ -0,0 +1,79 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dContainersStdAfx.h"
+#include "dArray.h"
+#include "dContainersAlloc.h"
+
+#ifndef __D_BEZIER_SPLINE_H__
+#define __D_BEZIER_SPLINE_H__
+
+class dBezierSpline: public dContainersAlloc
+{
+	public:
+
+	// empty spline
+	DCONTAINERS_API dBezierSpline ();
+	DCONTAINERS_API dBezierSpline (const dBezierSpline& src);
+
+	// create from knot vector and control points
+	DCONTAINERS_API virtual ~dBezierSpline ();
+
+	DCONTAINERS_API dBezierSpline& operator = (const dBezierSpline &copy) ;
+
+	DCONTAINERS_API int GetDegree () const;
+
+	DCONTAINERS_API dBigVector CurvePoint (dFloat64 u) const;
+	DCONTAINERS_API dBigVector CurveDerivative (dFloat64 u, int index = 1) const;
+	DCONTAINERS_API int CurveAllDerivatives (dFloat64 u, dBigVector* const defivatives) const;
+
+	DCONTAINERS_API dFloat64 CalculateLength (dFloat64 tol) const;
+
+	DCONTAINERS_API void GlobalCubicInterpolation (int count, const dBigVector* const points, const dBigVector& firstTangent, const dBigVector& lastTangent);
+	DCONTAINERS_API void CreateFromKnotVectorAndControlPoints (int degree, int knotCount, const dFloat64* const knotVector, const dBigVector* const controlPoints);
+
+	DCONTAINERS_API void InsertKnot (dFloat64 u);
+	DCONTAINERS_API bool RemoveKnot (dFloat64 u, dFloat64 tol);
+
+	DCONTAINERS_API int GetControlPointCount() const;
+	DCONTAINERS_API dArray<dBigVector>& GetControlPointArray();
+	DCONTAINERS_API const dArray<dBigVector>& GetControlPointArray() const;
+
+	DCONTAINERS_API dBigVector GetControlPoint(int i) const;
+	DCONTAINERS_API void SetControlPoint(int i, const dBigVector& point);
+	
+	DCONTAINERS_API int GetKnotCount() const;
+	DCONTAINERS_API dArray<dFloat64>& GetKnotArray();
+	DCONTAINERS_API const dArray<dFloat64>& GetKnotArray() const;
+
+	DCONTAINERS_API dFloat64 GetKnot(int i) const;
+	DCONTAINERS_API dFloat64 FindClosestKnot (dBigVector& closestPointOnCurve, const dBigVector& point, int subdivitionSteps = 2) const;
+
+	private:
+	void Clear();
+	int GetSpan(dFloat64 u) const;
+
+	dBigVector CurvePoint (dFloat64 u, int span) const;
+	void CreateCubicKnotVector(int count, const dBigVector* const points);
+	void CreateCubicControlPoints(int count, const dBigVector* const points, const dBigVector& firstTangent, const dBigVector& lastTangent);
+
+	void BasicsFunctions (dFloat64 u, int span, dFloat64* const functionOut) const;
+	void BasicsFunctionsDerivatives (dFloat64 u, int span, dFloat64* const derivatyivesOut) const;
+	
+	dArray<dFloat64> m_knotVector;
+	dArray<dBigVector> m_controlPoints;
+
+	int m_degree;
+	int m_knotsCount;
+	int m_controlPointsCount;
+};
+#endif
+

thirdparty/src/newton/dContainers/dCRC.cpp

@@ -0,0 +1,135 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dContainersStdAfx.h"
+#include "dCRC.h"
+
+
+static dCRCTYPE randBits0[] =
+{
+    static_cast<long long>(7266447313870364031ULL), static_cast<long long>(4946485549665804864ULL), static_cast<long long>(16945909448695747420ULL), static_cast<long long>(16394063075524226720ULL),
+    static_cast<long long>(4873882236456199058ULL), static_cast<long long>(14877448043947020171ULL), static_cast<long long>(6740343660852211943ULL), static_cast<long long>(13857871200353263164ULL),
+    static_cast<long long>(5249110015610582907ULL), static_cast<long long>(10205081126064480383ULL), static_cast<long long>(1235879089597390050ULL), static_cast<long long>(17320312680810499042ULL),
+    static_cast<long long>(16489141110565194782ULL), static_cast<long long>(8942268601720066061ULL), static_cast<long long>(13520575722002588570ULL), static_cast<long long>(14226945236717732373ULL),
+
+    static_cast<long long>(9383926873555417063ULL), static_cast<long long>(15690281668532552105ULL), static_cast<long long>(11510704754157191257ULL), static_cast<long long>(15864264574919463609ULL), 
+    static_cast<long long>(6489677788245343319ULL), static_cast<long long>(5112602299894754389ULL), static_cast<long long>(10828930062652518694ULL), static_cast<long long>(15942305434158995996ULL),
+    static_cast<long long>(15445717675088218264ULL), static_cast<long long>(4764500002345775851ULL), static_cast<long long>(14673753115101942098ULL), static_cast<long long>(236502320419669032ULL),
+    static_cast<long long>(13670483975188204088ULL), static_cast<long long>(14931360615268175698ULL), static_cast<long long>(8904234204977263924ULL), static_cast<long long>(12836915408046564963ULL),
+
+    static_cast<long long>(12120302420213647524ULL), static_cast<long long>(15755110976537356441ULL), static_cast<long long>(5405758943702519480ULL), static_cast<long long>(10951858968426898805ULL),
+    static_cast<long long>(17251681303478610375ULL), static_cast<long long>(4144140664012008120ULL), static_cast<long long>(18286145806977825275ULL), static_cast<long long>(13075804672185204371ULL),
+    static_cast<long long>(10831805955733617705ULL), static_cast<long long>(6172975950399619139ULL), static_cast<long long>(12837097014497293886ULL), static_cast<long long>(12903857913610213846ULL),
+    static_cast<long long>(560691676108914154ULL), static_cast<long long>(1074659097419704618ULL), static_cast<long long>(14266121283820281686ULL), static_cast<long long>(11696403736022963346ULL),
+
+    static_cast<long long>(13383246710985227247ULL), static_cast<long long>(7132746073714321322ULL), static_cast<long long>(10608108217231874211ULL), static_cast<long long>(9027884570906061560ULL),
+    static_cast<long long>(12893913769120703138ULL), static_cast<long long>(15675160838921962454ULL), static_cast<long long>(2511068401785704737ULL), static_cast<long long>(14483183001716371453ULL),
+    static_cast<long long>(3774730664208216065ULL), static_cast<long long>(5083371700846102796ULL), static_cast<long long>(9583498264570933637ULL), static_cast<long long>(17119870085051257224ULL),
+    static_cast<long long>(5217910858257235075ULL), static_cast<long long>(10612176809475689857ULL), static_cast<long long>(1924700483125896976ULL), static_cast<long long>(7171619684536160599ULL),
+
+
+    static_cast<long long>(10949279256701751503ULL), static_cast<long long>(15596196964072664893ULL), static_cast<long long>(14097948002655599357ULL), static_cast<long long>(615821766635933047ULL),
+    static_cast<long long>(5636498760852923045ULL), static_cast<long long>(17618792803942051220ULL), static_cast<long long>(580805356741162327ULL), static_cast<long long>(425267967796817241ULL),
+    static_cast<long long>(8381470634608387938ULL), static_cast<long long>(13212228678420887626ULL), static_cast<long long>(16993060308636741960ULL), static_cast<long long>(957923366004347591ULL),
+    static_cast<long long>(6210242862396777185ULL), static_cast<long long>(1012818702180800310ULL), static_cast<long long>(15299383925974515757ULL), static_cast<long long>(17501832009465945633ULL),
+
+    static_cast<long long>(17453794942891241229ULL), static_cast<long long>(15807805462076484491ULL), static_cast<long long>(8407189590930420827ULL), static_cast<long long>(974125122787311712ULL),
+    static_cast<long long>(1861591264068118966ULL), static_cast<long long>(997568339582634050ULL), static_cast<long long>(18046771844467391493ULL), static_cast<long long>(17981867688435687790ULL),
+    static_cast<long long>(3809841506498447207ULL), static_cast<long long>(9460108917638135678ULL), static_cast<long long>(16172980638639374310ULL), static_cast<long long>(958022432077424298ULL),
+    static_cast<long long>(4393365126459778813ULL), static_cast<long long>(13408683141069553686ULL), static_cast<long long>(13900005529547645957ULL), static_cast<long long>(15773550354402817866ULL),
+
+    static_cast<long long>(16475327524349230602ULL), static_cast<long long>(6260298154874769264ULL), static_cast<long long>(12224576659776460914ULL), static_cast<long long>(6405294864092763507ULL),
+    static_cast<long long>(7585484664713203306ULL), static_cast<long long>(5187641382818981381ULL), static_cast<long long>(12435998400285353380ULL), static_cast<long long>(13554353441017344755ULL),
+    static_cast<long long>(646091557254529188ULL), static_cast<long long>(11393747116974949255ULL), static_cast<long long>(16797249248413342857ULL), static_cast<long long>(15713519023537495495ULL),
+    static_cast<long long>(12823504709579858843ULL), static_cast<long long>(4738086532119935073ULL), static_cast<long long>(4429068783387643752ULL), static_cast<long long>(585582692562183870ULL),
+
+    static_cast<long long>(1048280754023674130ULL), static_cast<long long>(6788940719869959076ULL), static_cast<long long>(11670856244972073775ULL), static_cast<long long>(2488756775360218862ULL),
+    static_cast<long long>(2061695363573180185ULL), static_cast<long long>(6884655301895085032ULL), static_cast<long long>(3566345954323888697ULL), static_cast<long long>(12784319933059041817ULL),
+    static_cast<long long>(4772468691551857254ULL), static_cast<long long>(6864898938209826895ULL), static_cast<long long>(7198730565322227090ULL), static_cast<long long>(2452224231472687253ULL),
+    static_cast<long long>(13424792606032445807ULL), static_cast<long long>(10827695224855383989ULL), static_cast<long long>(11016608897122070904ULL), static_cast<long long>(14683280565151378358ULL),
+
+    static_cast<long long>(7077866519618824360ULL), static_cast<long long>(17487079941198422333ULL), static_cast<long long>(3956319990205097495ULL), static_cast<long long>(5804870313319323478ULL),
+    static_cast<long long>(8017203611194497730ULL), static_cast<long long>(3310931575584983808ULL), static_cast<long long>(5009341981771541845ULL), static_cast<long long>(11772020174577005930ULL),
+    static_cast<long long>(3537640779967351792ULL), static_cast<long long>(6801855569284252424ULL), static_cast<long long>(17687268231192623388ULL), static_cast<long long>(12968358613633237218ULL),
+    static_cast<long long>(1429775571144180123ULL), static_cast<long long>(10427377732172208413ULL), static_cast<long long>(12155566091986788996ULL), static_cast<long long>(16465954421598296115ULL),
+
+    static_cast<long long>(12710429690464359999ULL), static_cast<long long>(9547226351541565595ULL), static_cast<long long>(12156624891403410342ULL), static_cast<long long>(2985938688676214686ULL),
+    static_cast<long long>(18066917785985010959ULL), static_cast<long long>(5975570403614438776ULL), static_cast<long long>(11541343163022500560ULL), static_cast<long long>(11115388652389704592ULL),
+    static_cast<long long>(9499328389494710074ULL), static_cast<long long>(9247163036769651820ULL), static_cast<long long>(3688303938005101774ULL), static_cast<long long>(2210483654336887556ULL),
+    static_cast<long long>(15458161910089693228ULL), static_cast<long long>(6558785204455557683ULL), static_cast<long long>(1288373156735958118ULL), static_cast<long long>(18433986059948829624ULL),
+
+    static_cast<long long>(3435082195390932486ULL), static_cast<long long>(16822351800343061990ULL), static_cast<long long>(3120532877336962310ULL), static_cast<long long>(16681785111062885568ULL),
+    static_cast<long long>(7835551710041302304ULL), static_cast<long long>(2612798015018627203ULL), static_cast<long long>(15083279177152657491ULL), static_cast<long long>(6591467229462292195ULL),
+    static_cast<long long>(10592706450534565444ULL), static_cast<long long>(7438147750787157163ULL), static_cast<long long>(323186165595851698ULL), static_cast<long long>(7444710627467609883ULL),
+    static_cast<long long>(8473714411329896576ULL), static_cast<long long>(2782675857700189492ULL), static_cast<long long>(3383567662400128329ULL), static_cast<long long>(3200233909833521327ULL),
+
+    static_cast<long long>(12897601280285604448ULL), static_cast<long long>(3612068790453735040ULL), static_cast<long long>(8324209243736219497ULL), static_cast<long long>(15789570356497723463ULL),
+    static_cast<long long>(1083312926512215996ULL), static_cast<long long>(4797349136059339390ULL), static_cast<long long>(5556729349871544986ULL), static_cast<long long>(18266943104929747076ULL),
+    static_cast<long long>(1620389818516182276ULL), static_cast<long long>(172225355691600141ULL), static_cast<long long>(3034352936522087096ULL), static_cast<long long>(1266779576738385285ULL),
+    static_cast<long long>(3906668377244742888ULL), static_cast<long long>(6961783143042492788ULL), static_cast<long long>(17159706887321247572ULL), static_cast<long long>(4676208075243319061ULL),
+
+    static_cast<long long>(10315634697142985816ULL), static_cast<long long>(13435140047933251189ULL), static_cast<long long>(716076639492622016ULL), static_cast<long long>(13847954035438697558ULL),
+    static_cast<long long>(7195811275139178570ULL), static_cast<long long>(10815312636510328870ULL), static_cast<long long>(6214164734784158515ULL), static_cast<long long>(16412194511839921544ULL),
+    static_cast<long long>(3862249798930641332ULL), static_cast<long long>(1005482699535576005ULL), static_cast<long long>(4644542796609371301ULL), static_cast<long long>(17600091057367987283ULL),
+    static_cast<long long>(4209958422564632034ULL), static_cast<long long>(5419285945389823940ULL), static_cast<long long>(11453701547564354601ULL), static_cast<long long>(9951588026679380114ULL),
+
+    static_cast<long long>(7425168333159839689ULL), static_cast<long long>(8436306210125134906ULL), static_cast<long long>(11216615872596820107ULL), static_cast<long long>(3681345096403933680ULL),
+    static_cast<long long>(5770016989916553752ULL), static_cast<long long>(11102855936150871733ULL), static_cast<long long>(11187980892339693935ULL), static_cast<long long>(396336430216428875ULL),
+    static_cast<long long>(6384853777489155236ULL), static_cast<long long>(7551613839184151117ULL), static_cast<long long>(16527062023276943109ULL), static_cast<long long>(13429850429024956898ULL),
+    static_cast<long long>(9901753960477271766ULL), static_cast<long long>(9731501992702612259ULL), static_cast<long long>(5217575797614661659ULL), static_cast<long long>(10311708346636548706ULL),
+
+    static_cast<long long>(15111747519735330483ULL), static_cast<long long>(4353415295139137513ULL), static_cast<long long>(1845293119018433391ULL), static_cast<long long>(11952006873430493561ULL),
+    static_cast<long long>(3531972641585683893ULL), static_cast<long long>(16852246477648409827ULL), static_cast<long long>(15956854822143321380ULL), static_cast<long long>(12314609993579474774ULL),
+    static_cast<long long>(16763911684844598963ULL), static_cast<long long>(16392145690385382634ULL), static_cast<long long>(1545507136970403756ULL), static_cast<long long>(17771199061862790062ULL),
+    static_cast<long long>(12121348462972638971ULL), static_cast<long long>(12613068545148305776ULL), static_cast<long long>(954203144844315208ULL), static_cast<long long>(1257976447679270605ULL),
+
+    static_cast<long long>(3664184785462160180ULL), static_cast<long long>(2747964788443845091ULL), static_cast<long long>(15895917007470512307ULL), static_cast<long long>(15552935765724302120ULL),
+    static_cast<long long>(16366915862261682626ULL), static_cast<long long>(8385468783684865323ULL), static_cast<long long>(10745343827145102946ULL), static_cast<long long>(2485742734157099909ULL),
+    static_cast<long long>(916246281077683950ULL), static_cast<long long>(15214206653637466707ULL), static_cast<long long>(12895483149474345798ULL), static_cast<long long>(1079510114301747843ULL),
+    static_cast<long long>(10718876134480663664ULL), static_cast<long long>(1259990987526807294ULL), static_cast<long long>(8326303777037206221ULL), static_cast<long long>(14104661172014248293ULL),
+};
+
+dCRCTYPE dCombineCRC (dCRCTYPE a, dCRCTYPE b)
+{
+	return (a << 8) ^ b;
+}
+
+// calculate a 32 bit crc of a string
+dCRCTYPE dCRC64 (const char* const name, dCRCTYPE crcAcc)
+{
+	if (name) {
+		const int bitshift = (sizeof (dCRCTYPE)<<3) - 8;
+		for (int i = 0; name[i]; i ++) {
+			char c = name[i];
+			dCRCTYPE val = randBits0[((crcAcc >> bitshift) ^ c) & 0xff];
+			crcAcc = (crcAcc << 8) ^ val;
+		}
+	}
+	return crcAcc;
+}
+
+
+dCRCTYPE dCRC64 (const void* const buffer, int size, dCRCTYPE crcAcc)
+{
+	const unsigned char* const ptr = (unsigned char*)buffer;
+
+	const int bitshift = (sizeof (dCRCTYPE)<<3) - 8;
+	for (int i = 0; i < size; i ++) {
+		char c = ptr[i];
+		dCRCTYPE  val = randBits0[((crcAcc >> bitshift) ^ c) & 0xff];
+		crcAcc = (crcAcc << 8) ^ val;
+	}
+	return crcAcc;
+}
+
+
+
+

thirdparty/src/newton/dContainers/dCRC.h

@@ -0,0 +1,25 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#ifndef __dCRC__
+#define __dCRC__
+
+#include "dContainersAlloc.h"
+
+#define dCRCTYPE long long
+
+DCONTAINERS_API dCRCTYPE dCRC64 (const char* const string, dCRCTYPE  crcAcc = 0);
+DCONTAINERS_API dCRCTYPE dCRC64 (const void* const buffer, int size, dCRCTYPE crcAcc);
+
+DCONTAINERS_API dCRCTYPE dCombineCRC (dCRCTYPE a, dCRCTYPE b);
+
+#endif
+

thirdparty/src/newton/dContainers/dClassInfo.cpp

@@ -0,0 +1,18 @@
+
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+
+#include "dContainersStdAfx.h"
+#include "dClassInfo.h"
+
+//dRtti dClassInfo::m_rtti ("dClassInfo");
+dRttiRootClassSupportImplement(dClassInfo);

thirdparty/src/newton/dContainers/dClassInfo.h

@@ -0,0 +1,32 @@
+
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#ifndef __DCLASS_INFO_H__
+#define __DCLASS_INFO_H__
+
+#include "dRtti.h"
+#include "dRefCounter.h"
+
+class dClassInfo: public dRefCounter
+{
+	public:
+	dClassInfo(void)
+	{
+	}
+	virtual ~dClassInfo()
+	{
+	}
+
+	dRttiRootClassSupportDeclare(dClassInfo,DCONTAINERS_API);
+};
+
+#endif

thirdparty/src/newton/dContainers/dContainersAlloc.cpp

@@ -0,0 +1,215 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dContainersStdAfx.h"
+#include "dContainersAlloc.h"
+
+
+#pragma warning (disable: 4100) //warning C4100: 'lpReserved' : unreferenced formal parameter
+
+#ifdef _DCONTAINERS_DLL
+	void* operator new (size_t size)
+	{ 
+		void* const ptr = malloc (size);
+		dAssert (ptr);
+		return ptr;
+	}
+
+	void operator delete (void* ptr) 
+	{ 
+		free (ptr);
+	}
+
+	BOOL APIENTRY DllMain( HMODULE hModule, DWORD  ul_reason_for_call, LPVOID lpReserved)
+	{
+		switch (ul_reason_for_call)
+		{
+			case DLL_PROCESS_ATTACH:
+			case DLL_THREAD_ATTACH:
+				// check for memory leaks
+				#if defined(_DEBUG) && defined(_MSC_VER)
+					// Track all memory leaks at the operating system level.
+					// make sure no Newton tool or utility leaves leaks behind.
+					_CrtSetDbgFlag(_CRTDBG_LEAK_CHECK_DF|_CrtSetDbgFlag(_CRTDBG_LEAK_CHECK_DF));
+				#endif
+
+			case DLL_THREAD_DETACH:
+			case DLL_PROCESS_DETACH:
+				break;
+		}
+		return TRUE;
+	}
+#endif
+
+
+void* dContainersAlloc::operator new (size_t size)
+{
+	//return ::new char[size];
+	return Alloc (size);
+}
+
+void dContainersAlloc::operator delete (void* ptr)
+{
+//	delete[] (char*) ptr;
+	Free(ptr);
+}
+
+void* dContainersAlloc::Alloc (size_t size)
+{
+	char* const ptr = ::new char[size];
+	return ptr;
+}
+
+void dContainersAlloc::Free(void* const ptr)
+{
+	delete[] (char*) ptr;
+}
+
+
+
+
+dContainerFixSizeAllocator::dContainerFixSizeAllocator(int size, int poolSize)
+	:m_freeListNode(NULL)
+	,m_size(size)
+	,m_poolSize(poolSize)
+{
+	Prefetch ();
+}
+
+
+dContainerFixSizeAllocator::~dContainerFixSizeAllocator()
+{
+	Flush();
+}
+
+
+dContainerFixSizeAllocator* dContainerFixSizeAllocator::Create (int size, int poolSize)
+{
+	class AllocatorsFactory
+	{
+		public:
+		AllocatorsFactory()
+			:m_count(0)
+			,m_maxSize(0)
+			,m_pool (NULL)
+		{
+			m_maxSize = 1;
+			m_pool = new dContainerFixSizeAllocator*[1];
+		}
+
+		~AllocatorsFactory()
+		{
+			for (int i = 0; i < m_count; i ++) {
+				delete m_pool[i];
+			}
+			delete[] m_pool; 
+		}
+
+		dContainerFixSizeAllocator* FindCreate (int size, int poolSize)
+		{
+			int i0 = 0;
+			int i2 = m_count -1;
+			while ((i2 - i0) > 4) {
+				int i1 = (i0 + i2) >> 1;
+				if (size < m_pool[i1]->m_size) {
+					i2 = i1;
+				} else {
+					i0 = i1;
+				}
+			}
+
+			for (int i = i0; (i < m_count) && (m_pool[i]->m_size <= size); i ++) {
+				if (m_pool[i]->m_size == size) {
+					return m_pool[i];
+				}
+			}
+
+			if (m_count == m_maxSize) {
+				m_maxSize *= 2;
+				dContainerFixSizeAllocator** pool = new dContainerFixSizeAllocator*[m_maxSize];
+				memcpy (pool, m_pool, m_count * sizeof (dContainerFixSizeAllocator*));
+				delete[] m_pool;
+				m_pool = pool;
+			}
+
+			dContainerFixSizeAllocator* const allocator = new dContainerFixSizeAllocator(size, poolSize);
+
+			int entry = m_count;
+			for (; entry && (m_pool[entry - 1]->m_size > size); entry --) {
+				m_pool[entry] = m_pool[entry - 1];
+			}
+			m_pool[entry] = allocator;
+			m_count ++;
+			return allocator;
+		}
+
+		int m_count;
+		int m_maxSize;
+		dContainerFixSizeAllocator** m_pool;
+	};
+
+	static AllocatorsFactory factories;
+	return factories.FindCreate(size, poolSize);
+}
+
+
+void dContainerFixSizeAllocator::Prefetch ()
+{
+	for (int i = 0; i < m_poolSize; i ++) {
+		dFreeListNode* const data = (dFreeListNode*) dContainersAlloc::Alloc (m_size);
+		data->m_count = i + 1; 
+		data->m_next = m_freeListNode; 
+		m_freeListNode = data;
+	}
+}
+
+
+void dContainerFixSizeAllocator::Flush ()
+{
+	for (int i = 0; m_freeListNode && (i < m_poolSize); i ++) {
+		dFreeListNode* const ptr = m_freeListNode;
+		m_freeListNode = m_freeListNode->m_next;
+		dContainersAlloc::Free (ptr);
+	}
+}
+
+
+void* dContainerFixSizeAllocator::Alloc() 
+{
+	if (!m_freeListNode) {
+		Prefetch ();
+	}
+	dFreeListNode* const data = m_freeListNode;
+	m_freeListNode = m_freeListNode->m_next;
+	return data;
+}
+
+
+void dContainerFixSizeAllocator::Free(void* const ptr) 
+{
+	dFreeListNode* const data = (dFreeListNode*) ptr;
+	data->m_count = m_freeListNode ? m_freeListNode->m_count + 1 : 1;
+	data->m_next = m_freeListNode;
+	m_freeListNode = data;
+	if (data->m_count >= 2 * m_poolSize) {
+		Flush();
+	}
+}

thirdparty/src/newton/dContainers/dContainersAlloc.h

@@ -0,0 +1,75 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef _D_CONTAINERS_ALLOC_H_
+#define _D_CONTAINERS_ALLOC_H_
+#include "dContainersStdAfx.h"
+
+#define D_MAX_ENTRIES_IN_FREELIST	32
+
+class dContainersAlloc  
+{
+	public:
+	DCONTAINERS_API void *operator new (size_t size);
+	DCONTAINERS_API void operator delete (void* ptr);
+
+	dContainersAlloc()
+	{
+	}
+
+	virtual ~dContainersAlloc() 
+	{
+	}
+
+	static DCONTAINERS_API void* Alloc (size_t size);
+	static DCONTAINERS_API void Free (void* const ptr);
+};
+
+
+class dContainerFixSizeAllocator
+{
+	public:
+	static DCONTAINERS_API dContainerFixSizeAllocator* Create (int size, int poolSize);
+	DCONTAINERS_API ~dContainerFixSizeAllocator();
+	DCONTAINERS_API void* Alloc();
+	DCONTAINERS_API void Free(void* const ptr);
+//	DCONTAINERS_API bool IsAlive() const;
+	DCONTAINERS_API void Flush ();
+
+	private:
+	DCONTAINERS_API dContainerFixSizeAllocator(int size, int poolSize);
+
+	class dFreeListNode
+	{
+		public:
+		int m_count;
+		dFreeListNode* m_next;
+	};
+
+	DCONTAINERS_API void Prefetch ();
+
+	dFreeListNode* m_freeListNode;
+	int m_size;
+	int m_poolSize;
+};
+
+
+#endif

thirdparty/src/newton/dContainers/dContainersStdAfx.cpp

@@ -0,0 +1,21 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+// stdafx.cpp : source file that includes just the standard includes
+// containers.pch will be the pre-compiled header
+// stdafx.obj will contain the pre-compiled type information
+
+#include "dContainersStdAfx.h"
+
+// TODO: reference any additional headers you need in STDAFX.H
+// and not in this file
+
+

thirdparty/src/newton/dContainers/dContainersStdAfx.h

@@ -0,0 +1,55 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+// stdafx.h : include file for standard system include files,
+// or project specific include files that are used frequently, but
+// are changed infrequently
+//
+
+#ifndef __D_CONTAINERS_STDAFX__
+#define __D_CONTAINERS_STDAFX__
+
+#include <new>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include <dMathDefines.h>
+#include <dVector.h>
+#include <dMatrix.h>
+
+#include <dgTypes.h>
+
+#ifdef _DCONTAINERS_DLL
+	#ifdef _DCONTAINERS_EXPORT
+		#define DCONTAINERS_API DG_LIBRARY_EXPORT
+	#else
+		#define DCONTAINERS_API DG_LIBRARY_IMPORT
+	#endif
+#else
+	#define DCONTAINERS_API DG_LIBRARY_STATIC
+#endif
+
+
+#ifdef _WIN32
+	#include <windows.h>
+	#include <crtdbg.h>
+#endif
+
+#ifdef _MACOSX_VER
+	#include <unistd.h>
+	#include <libkern/OSAtomic.h>
+	#include <sys/sysctl.h>
+#endif
+
+#include <dMathDefines.h>
+
+#endif

thirdparty/src/newton/dContainers/dHeap.h

@@ -0,0 +1,421 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+*
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+*
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+*
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+*
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dHeapBase__
+#define __dHeapBase__
+
+
+#include "dContainersStdAfx.h"
+#include "dContainersAlloc.h"
+
+//#define D_CHECK_HEAP
+
+
+template <class OBJECT, class KEY>
+class dHeapBase: public dContainersAlloc
+{
+	protected:
+	struct dHeapRecord 
+	{
+		dHeapRecord ()
+            :m_key(), m_obj()
+		{
+		}
+
+		dHeapRecord (KEY key, const OBJECT& obj)
+			:m_key(key), m_obj(obj)
+		{
+		}
+
+		~dHeapRecord ()
+		{
+		}
+
+		KEY m_key;
+		OBJECT m_obj;
+	};
+
+	dHeapBase (int maxElements);
+	virtual ~dHeapBase ();
+
+	public:
+	void Flush (); 
+	KEY MaxValue() const; 
+	KEY Value(int i = 0) const;
+	int GetCount() const;
+	int GetMaxCount() const;
+	const OBJECT& operator[] (int i) const;
+	int Find (OBJECT &obj);
+	int Find (KEY key);
+
+	int m_curCount;
+	int m_maxCount;
+	bool m_allocated;
+	dHeapRecord* m_pool;
+};
+
+template <class OBJECT, class KEY>
+class dDownHeap: public dHeapBase<OBJECT, KEY>
+{
+	public:
+	dDownHeap (int maxElements);
+
+	void Pop () {Remove (0);}
+	void Push (OBJECT &obj, KEY key);
+	void Sort ();
+	void Remove (int Index);
+	bool SanityCheck();
+};
+
+template <class OBJECT, class KEY>
+class dUpHeap: public dHeapBase<OBJECT, KEY>
+{
+	public:
+	dUpHeap (int maxElements);
+
+	void Pop () {Remove (0);}
+	void Push (OBJECT &obj, KEY key);
+	void Sort ();
+	void Remove (int Index);
+	bool SanityCheck();
+};
+
+template <class OBJECT, class KEY>
+dHeapBase<OBJECT,KEY>::dHeapBase (int maxElements)
+	:m_curCount(0)
+	,m_maxCount(maxElements)
+	,m_allocated(true)
+	,m_pool(new dHeapRecord[maxElements])
+{
+	Flush();
+}
+
+
+template <class OBJECT, class KEY>
+dHeapBase<OBJECT,KEY>::~dHeapBase ()
+{   
+	if (m_allocated == true) {
+		delete[] m_pool;
+	}
+}
+
+
+template <class OBJECT, class KEY>
+KEY dHeapBase<OBJECT,KEY>::Value(int i) const
+{
+	return m_pool[i].m_key;
+}
+
+
+template <class OBJECT, class KEY>
+int dHeapBase<OBJECT,KEY>::GetCount() const
+{ 
+	return m_curCount;
+}
+
+
+template <class OBJECT, class KEY>
+void dHeapBase<OBJECT,KEY>::Flush () 
+{
+	m_curCount = 0;
+
+	#ifdef _DEBUG
+//	dHeapBase<OBJECT,KEY>::m_pool[dHeapBase<OBJECT,KEY>::m_curCount].m_key = KEY (0);
+	#endif
+}
+
+
+template <class OBJECT, class KEY>
+KEY dHeapBase<OBJECT,KEY>::MaxValue() const 
+{
+	return m_pool[0].m_key;
+}
+
+
+template <class OBJECT, class KEY>
+int dHeapBase<OBJECT,KEY>::GetMaxCount() const
+{ 
+	return m_maxCount;
+}
+
+
+template <class OBJECT, class KEY>
+int dHeapBase<OBJECT,KEY>::Find (OBJECT &obj)
+{
+	// For now let perform a linear search
+	// this is efficient if the size of the heap is small
+	// ex: m_curCount < 32
+	// this will be change to a binary search in the heap should the 
+	// the size of the heap get larger than 32
+	//	dAssert (m_curCount <= 32);
+	for (int i = 0; i < m_curCount; i ++) {
+		if (m_pool[i].obj == obj) {
+			return i;
+		}
+	}
+	return - 1;
+}
+
+template <class OBJECT, class KEY>
+int dHeapBase<OBJECT,KEY>::Find (KEY key)
+{
+	// ex: m_curCount < 32
+	// this will be change to a binary search in the heap shoud the 
+	// the size of the heap get larger than 32
+	dAssert (m_curCount <= 32);
+	for (int i = 0; i < m_curCount; i ++)	{
+		if (m_pool[i].m_key == key) {
+			return i;
+		}
+	}
+	return - 1;
+}
+
+
+template <class OBJECT, class KEY>
+const OBJECT& dHeapBase<OBJECT,KEY>::operator[] (int i) const
+{ 
+	dAssert (i<= m_curCount);
+	return m_pool[i].m_obj;
+}
+
+
+// **************************************************************************
+//
+// down Heap
+//
+// **************************************************************************
+template <class OBJECT, class KEY>
+dDownHeap<OBJECT,KEY>::dDownHeap (int maxElements)
+	:dHeapBase<OBJECT, KEY> (maxElements)
+{
+}
+
+template <class OBJECT, class KEY>
+void dDownHeap<OBJECT,KEY>::Push (OBJECT &obj, KEY key)
+{
+	dAssert ((dHeapBase<OBJECT,KEY>::m_curCount < dHeapBase<OBJECT,KEY>::m_maxCount));
+	dHeapBase<OBJECT,KEY>::m_curCount ++;
+
+	int j;
+	int i = dHeapBase<OBJECT,KEY>::m_curCount;
+	for (; i; i = j) {
+		j = i >> 1;
+		if (!j || (dHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key > key)) {
+			break;
+		}
+		dHeapBase<OBJECT,KEY>::m_pool[i - 1] = dHeapBase<OBJECT,KEY>::m_pool[j - 1];
+	}
+	dAssert (i);
+	dHeapBase<OBJECT,KEY>::m_pool[i - 1].m_key = key;
+	dHeapBase<OBJECT,KEY>::m_pool[i - 1].m_obj = obj;
+	dAssert (SanityCheck());
+}
+
+template <class OBJECT, class KEY>
+void dDownHeap<OBJECT,KEY>::Remove (int index)
+{
+	dHeapBase<OBJECT, KEY>::m_curCount--;
+	dHeapBase<OBJECT, KEY>::m_pool[index] = dHeapBase<OBJECT, KEY>::m_pool[dHeapBase<OBJECT, KEY>::m_curCount];
+	while (index && dHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1].m_key < dHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
+		dSwap(dHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1], dHeapBase<OBJECT, KEY>::m_pool[index]);
+		index = (index - 1) >> 1;
+	}
+
+	while ((2 * index + 1) < dHeapBase<OBJECT, KEY>::m_curCount) {
+		int i0 = 2 * index + 1;
+		int i1 = 2 * index + 2;
+		if (i1 < dHeapBase<OBJECT, KEY>::m_curCount) {
+			i0 = (dHeapBase<OBJECT, KEY>::m_pool[i0].m_key > dHeapBase<OBJECT, KEY>::m_pool[i1].m_key) ? i0 : i1;
+			if (dHeapBase<OBJECT, KEY>::m_pool[i0].m_key <= dHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
+				break;
+			}
+			dSwap(dHeapBase<OBJECT, KEY>::m_pool[i0], dHeapBase<OBJECT, KEY>::m_pool[index]);
+			index = i0;
+		} else {
+			if (dHeapBase<OBJECT, KEY>::m_pool[i0].m_key > dHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
+				dSwap(dHeapBase<OBJECT, KEY>::m_pool[i0], dHeapBase<OBJECT, KEY>::m_pool[index]);
+			}
+			index = i0;
+		}
+	}
+	dAssert(SanityCheck());
+}
+
+template <class OBJECT, class KEY>
+void dDownHeap<OBJECT,KEY>::Sort ()
+{
+	int count = dHeapBase<OBJECT,KEY>::m_curCount;
+	for (int i = 1; i < count; i ++) {
+		KEY key (dHeapBase<OBJECT,KEY>::m_pool[0].m_key);
+		OBJECT obj (dHeapBase<OBJECT,KEY>::m_pool[0].m_obj);
+
+		Pop();
+
+		dHeapBase<OBJECT,KEY>::m_pool[dHeapBase<OBJECT,KEY>::m_curCount].m_key = key;
+		dHeapBase<OBJECT,KEY>::m_pool[dHeapBase<OBJECT,KEY>::m_curCount].m_obj = obj;
+	}
+
+	dHeapBase<OBJECT,KEY>::m_curCount = count;
+	for (int i = 0; i < count / 2; i ++) {
+		KEY key (dHeapBase<OBJECT,KEY>::m_pool[i].m_key);
+		OBJECT obj (dHeapBase<OBJECT,KEY>::m_pool[i].m_obj);
+
+		dHeapBase<OBJECT,KEY>::m_pool[i].m_key = dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key;
+		dHeapBase<OBJECT,KEY>::m_pool[i].m_obj = dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj;
+
+		dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key = key;
+		dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj = obj;
+	}
+	dAssert (SanityCheck());
+}
+
+template <class OBJECT, class KEY>
+bool dDownHeap<OBJECT, KEY>::SanityCheck()
+{
+#ifdef D_CHECK_HEAP
+	for (int i = 0; i < this->m_curCount; i++) {
+		int i1 = 2 * i + 1;
+		int i2 = 2 * i + 2;
+		if ((i1 < this->m_curCount) && (dHeapBase<OBJECT, KEY>::m_pool[i].m_key < dHeapBase<OBJECT, KEY>::m_pool[i1].m_key)) {
+			return false;
+		}
+		if ((i2 < this->m_curCount) && (dHeapBase<OBJECT, KEY>::m_pool[i].m_key < dHeapBase<OBJECT, KEY>::m_pool[i2].m_key)) {
+			return false;
+		}
+	}
+#endif
+	return true;
+}
+
+
+// **************************************************************************
+//
+// Up Heap
+//
+// **************************************************************************
+template <class OBJECT, class KEY>
+dUpHeap<OBJECT,KEY>::dUpHeap (int maxElements)
+	:dHeapBase<OBJECT, KEY> (maxElements)
+{
+}
+
+template <class OBJECT, class KEY>
+void dUpHeap<OBJECT,KEY>::Push (OBJECT &obj, KEY key)
+{
+	dAssert ((dHeapBase<OBJECT,KEY>::m_curCount < dHeapBase<OBJECT,KEY>::m_maxCount));
+	dHeapBase<OBJECT,KEY>::m_curCount ++;
+	
+	int j;
+	int i = dHeapBase<OBJECT,KEY>::m_curCount;
+	for (; i; i = j) {
+		j = i >> 1;
+		if (!j || (dHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key < key)) {
+			break;
+		}
+		dHeapBase<OBJECT,KEY>::m_pool[i - 1] = dHeapBase<OBJECT,KEY>::m_pool[j - 1];
+	}
+	dAssert (i);
+	dHeapBase<OBJECT,KEY>::m_pool[i - 1].m_key = key;
+	dHeapBase<OBJECT,KEY>::m_pool[i - 1].m_obj = obj;
+	dAssert (SanityCheck());
+}
+
+
+template <class OBJECT, class KEY>
+void dUpHeap<OBJECT,KEY>::Sort ()
+{
+	int count = dHeapBase<OBJECT,KEY>::m_curCount;
+	for (int i = 1; i < count; i ++) {
+		KEY key (dHeapBase<OBJECT,KEY>::m_pool[0].m_key);
+		OBJECT obj (dHeapBase<OBJECT,KEY>::m_pool[0].m_obj);
+
+		Pop();
+
+		dHeapBase<OBJECT,KEY>::m_pool[dHeapBase<OBJECT,KEY>::m_curCount].m_key = key;
+		dHeapBase<OBJECT,KEY>::m_pool[dHeapBase<OBJECT,KEY>::m_curCount].m_obj = obj;
+	}
+
+	dHeapBase<OBJECT,KEY>::m_curCount = count;
+	for (int i = 0; i < count / 2; i ++) {
+		KEY key (dHeapBase<OBJECT,KEY>::m_pool[i].m_key);
+		OBJECT obj (dHeapBase<OBJECT,KEY>::m_pool[i].m_obj);
+
+		dHeapBase<OBJECT,KEY>::m_pool[i].m_key = dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key;
+		dHeapBase<OBJECT,KEY>::m_pool[i].m_obj = dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj;
+
+		dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key = key;
+		dHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj = obj;
+	}
+	dAssert (SanityCheck());
+}
+
+template <class OBJECT, class KEY>
+void dUpHeap<OBJECT,KEY>::Remove (int index)
+{
+	dHeapBase<OBJECT, KEY>::m_curCount--;
+	dHeapBase<OBJECT, KEY>::m_pool[index] = dHeapBase<OBJECT, KEY>::m_pool[dHeapBase<OBJECT, KEY>::m_curCount];
+	while (index && dHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1].m_key > dHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
+		dSwap(dHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1], dHeapBase<OBJECT, KEY>::m_pool[index]);
+		index = (index - 1) >> 1;
+	}
+
+	while ((2 * index + 1) < dHeapBase<OBJECT, KEY>::m_curCount) {
+		int i0 = 2 * index + 1;
+		int i1 = 2 * index + 2;
+		if (i1 < dHeapBase<OBJECT, KEY>::m_curCount) {
+			i0 = (dHeapBase<OBJECT, KEY>::m_pool[i0].m_key < dHeapBase<OBJECT, KEY>::m_pool[i1].m_key) ? i0 : i1;
+			if (dHeapBase<OBJECT, KEY>::m_pool[i0].m_key >= dHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
+				break;
+			}
+			dSwap(dHeapBase<OBJECT, KEY>::m_pool[i0], dHeapBase<OBJECT, KEY>::m_pool[index]);
+			index = i0;
+		} else {
+			if (dHeapBase<OBJECT, KEY>::m_pool[i0].m_key < dHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
+				dSwap(dHeapBase<OBJECT, KEY>::m_pool[i0], dHeapBase<OBJECT, KEY>::m_pool[index]);
+			}
+			index = i0;
+		}
+	}
+	dAssert (SanityCheck());
+}
+
+template <class OBJECT, class KEY>
+bool dUpHeap<OBJECT, KEY>::SanityCheck()
+{
+#ifdef D_CHECK_HEAP
+	for (int i = 0; i < this->m_curCount; i++) {
+		int i1 = 2 * i + 1;
+		int i2 = 2 * i + 2;
+		if ((i1 < this->m_curCount) && (dHeapBase<OBJECT, KEY>::m_pool[i].m_key > dHeapBase<OBJECT, KEY>::m_pool[i1].m_key)) {
+			return false;
+		}
+		if ((i2 < this->m_curCount) && (dHeapBase<OBJECT, KEY>::m_pool[i].m_key > dHeapBase<OBJECT, KEY>::m_pool[i2].m_key)) {
+			return false;
+		}
+	}
+#endif
+	return true;
+}
+
+
+#endif
+

thirdparty/src/newton/dContainers/dList.h

@@ -0,0 +1,483 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+
+#ifndef __dList__
+#define __dList__
+
+#include "dContainersStdAfx.h"
+#include "dContainersAlloc.h"
+
+
+// a small double link list container similar to STL, 
+template<class T, int poolSize = D_MAX_ENTRIES_IN_FREELIST>
+class dList: public dContainersAlloc
+{
+	public:
+	class dListNode
+	{
+		dListNode (dListNode* const prev, dListNode* const next) 
+			:m_info () 
+		{
+			m_prev = prev;
+			m_next = next;
+			if (m_prev) {
+				m_prev->m_next = this;
+			}
+			if (m_next) {
+				m_next->m_prev = this;
+			}
+		}
+
+		dListNode (const T &info, dListNode* prev, dListNode* next) 
+			:m_info (info) 
+		{
+			m_prev = prev;
+			m_next = next;
+			if (m_prev) {
+				m_prev->m_next = this;
+			}
+			if (m_next) {
+				m_next->m_prev = this;
+			}
+		}
+
+		virtual ~dListNode()
+		{
+		}
+
+		void Unlink ()
+		{
+			if (m_prev) {
+				m_prev->m_next = m_next;
+			}
+
+			if (m_next) {
+				m_next->m_prev = m_prev;
+			}
+			m_prev = NULL;
+			m_next = NULL;
+		}
+
+		void Remove()
+		{
+			Unlink();
+			this->~dListNode();
+		}
+
+		void AddLast(dListNode* const node) 
+		{
+			m_next = node;
+			node->m_prev = this;
+		}
+
+		void AddFirst(dListNode* const node) 
+		{
+			m_prev = node;
+			node->m_next = this;
+		}
+
+		public:
+		T& GetInfo()
+		{
+			return m_info;
+		}
+
+		const T& GetInfo() const
+		{
+			return m_info;
+		}
+
+
+		dListNode* GetNext() const
+		{
+			return m_next;
+		}
+
+		dListNode* GetPrev() const
+		{
+			return m_prev;
+		}
+
+		private:
+		T m_info;
+		dListNode* m_next;
+		dListNode* m_prev;
+		friend class dList<T, poolSize>;
+	};
+
+	class Iterator
+	{
+		public:
+		Iterator (const dList<T, poolSize> &me)
+		{
+			m_ptr = NULL;
+			m_list = (dList *)&me;
+		}
+
+		~Iterator ()
+		{
+		}
+
+		operator int() const
+		{
+			return m_ptr != NULL;
+		}
+
+		bool operator== (const Iterator &target) const
+		{
+			return (m_ptr == target.m_ptr) && (m_list == target.m_list);
+		}
+
+		void Begin()
+		{
+			m_ptr = m_list->GetFirst();
+		}
+
+		void End()
+		{
+			m_ptr = m_list->GetLast();
+		}
+
+		void Set (dListNode* const node)
+		{
+			m_ptr = node;
+		}
+
+		void operator++ ()
+		{
+			m_ptr = m_ptr->m_next();
+		}
+
+		void operator++ (int)
+		{
+			m_ptr = m_ptr->GetNext();
+		}
+
+		void operator-- () 
+		{
+			m_ptr = m_ptr->GetPrev();
+		}
+
+		void operator-- (int) 
+		{
+			m_ptr = m_ptr->GetPrev();
+		}
+
+		T &operator* () const
+		{
+			return m_ptr->GetInfo();
+		}
+
+		dListNode* GetNode() const
+		{
+			return m_ptr;
+		}
+
+		private:
+		dList *m_list;
+		dListNode* m_ptr;
+	};
+
+	// ***********************************************************
+	// member functions
+	// ***********************************************************
+	public:
+	dList ();
+	virtual ~dList ();
+
+	operator int() const;
+	int GetCount() const;
+	dListNode* GetLast() const;
+	dListNode* GetFirst() const;
+	dListNode* Append ();
+	dListNode* Append (const T &element);
+	dListNode* Addtop ();
+	dListNode* Addtop (const T &element);
+	dListNode* AppendAfter (dListNode* const node);
+
+	void RotateToEnd (dListNode* const node);
+	void RotateToBegin (dListNode* const node);
+	void InsertAfter (dListNode* const root, dListNode* const node);
+
+	dListNode* Find (const T &element) const;
+	dListNode* GetNodeFromInfo (T &m_info) const;
+	void Remove (dListNode* const node);
+	void Remove (const T &element);
+	void RemoveAll ();
+	
+	// special routines
+	// move the data to the target list and set to zero m_count, m_first and m_last
+	void TranferDataToTarget (dList& target);
+
+	// ***********************************************************
+	// member variables
+	// ***********************************************************
+	private:
+/*
+	bool Sanity() const
+	{
+		int count = 0;
+		for (dListNode* ptr = GetFirst(); ptr; ptr = ptr->GetNext())
+			count ++;
+		return count == m_count;
+	}
+*/
+	bool Sanity() const {return true;}
+	dContainerFixSizeAllocator& GetAllocator()
+	{
+		static dContainerFixSizeAllocator* allocator = NULL;
+		if (!allocator) {
+			allocator = dContainerFixSizeAllocator::Create (sizeof (dList<T, poolSize>::dListNode), poolSize);
+		}
+		return *allocator;
+	}
+
+	int m_count;
+	dListNode* m_first;
+	dListNode* m_last;
+	friend class dListNode;
+};
+
+
+template<class T, int poolSize>
+dList<T, poolSize>::dList ()
+{
+	m_count = 0;
+	m_first = NULL;
+	m_last = NULL;
+	GetAllocator();
+}
+
+
+template<class T, int poolSize>
+dList<T, poolSize>::~dList () 
+{
+	RemoveAll ();
+}
+
+
+template<class T, int poolSize>
+int dList<T, poolSize>::GetCount() const
+{
+	return m_count;
+}
+
+template<class T, int poolSize>
+dList<T, poolSize>::operator int() const
+{
+	return m_first != NULL;
+}
+
+template<class T, int poolSize>
+typename dList<T, poolSize>::dListNode* dList<T, poolSize>::GetFirst() const
+{
+	return m_first;
+}
+
+template<class T, int poolSize>
+typename dList<T, poolSize>::dListNode* dList<T, poolSize>::GetLast() const
+{
+	return m_last;
+}
+
+template<class T, int poolSize>
+typename dList<T, poolSize>::dListNode* dList<T, poolSize>::Append ()
+{
+	m_count	++;
+	if (m_first == NULL) {
+		m_first = new (GetAllocator().Alloc()) dListNode(NULL, NULL);
+		m_last = m_first;
+	} else {
+		m_last = new (GetAllocator().Alloc()) dListNode(m_last, NULL);
+	}
+	dAssert (Sanity());
+	return m_last;
+}
+
+template<class T, int poolSize>
+typename dList<T, poolSize>::dListNode* dList<T, poolSize>::AppendAfter (dListNode* const node)
+{
+	dListNode* const ptr = Append ();
+	InsertAfter (node, ptr);
+	return ptr;
+}
+
+template<class T, int poolSize>
+typename dList<T, poolSize>::dListNode* dList<T, poolSize>::Append (const T &element)
+{
+	m_count	++;
+	if (m_first == NULL) {
+		m_first = new (GetAllocator().Alloc()) dListNode(element, NULL, NULL);
+		m_last = m_first;
+	} else {
+		m_last = new (GetAllocator().Alloc()) dListNode(element, m_last, NULL);
+	}
+	dAssert (Sanity());
+	return m_last;
+}
+
+template<class T, int poolSize>
+typename dList<T, poolSize>::dListNode* dList<T, poolSize>::Addtop ()
+{
+	m_count	++;
+	if (m_last == NULL) {
+		m_last = new (GetAllocator().Alloc()) dListNode(NULL, NULL);
+		m_first = m_last;
+	} else {
+		m_first = new (GetAllocator().Alloc()) dListNode(NULL, m_first);
+	}
+	dAssert (Sanity());
+	return m_first;
+}
+
+
+template<class T, int poolSize>
+typename dList<T, poolSize>::dListNode* dList<T, poolSize>::Addtop (const T &element)
+{
+	m_count	++;
+	if (m_last == NULL) {
+		m_last = new (GetAllocator().Alloc()) dListNode(element, NULL, NULL);
+		m_first = m_last;
+	} else {
+		m_first = new (GetAllocator().Alloc()) dListNode(element, NULL, m_first);
+	}
+	dAssert (Sanity());
+	return m_first;
+}
+
+template<class T, int poolSize>
+void dList<T, poolSize>::InsertAfter (dListNode* const root, dListNode* const node)
+{
+	dAssert (root != node);
+	if (node == m_last) {
+		m_last = m_last->GetPrev(); 
+	}
+	node->Unlink();
+
+	node->m_prev = root;
+	node->m_next = root->m_next;
+	if (root->m_next) {
+		root->m_next->m_prev = node;
+	} 
+	root->m_next = node;
+
+	if (root == m_last) {
+		m_last = node;
+	}
+
+	dAssert (Sanity());
+}
+
+template<class T, int poolSize>
+void dList<T, poolSize>::RotateToEnd (dListNode* const node)
+{
+	if (node != m_last) {
+		if (m_last != m_first) {
+			if (node == m_first) {
+				m_first = m_first->GetNext();
+			}
+			node->Unlink();
+			m_last->AddLast(node);
+			m_last = node; 
+		}
+	}
+}
+
+template<class T, int poolSize>
+void dList<T, poolSize>::RotateToBegin (dListNode* const node)
+{
+	if (node != m_first) {
+		if (m_last != m_first) {
+			if (node == m_last) {
+				m_last = m_last->GetPrev();
+			}
+			node->Unlink();
+			m_first->AddFirst(node);
+			m_first = node; 
+		}
+	}
+}
+
+
+template<class T, int poolSize>
+typename dList<T, poolSize>::dListNode* dList<T, poolSize>::Find (const T &element) const
+{
+	dListNode* node;
+	for (node = m_first; node; node = node->GetNext()) {
+		if (element	== node->m_info) {
+			break;
+		}
+	}
+	return node;
+}
+
+
+template<class T, int poolSize>
+typename dList<T, poolSize>::dListNode* dList<T, poolSize>::GetNodeFromInfo (T &info) const
+{
+	dListNode* const node = (dListNode*) &info;
+	long long offset = ((char*) &node->m_info) - ((char*)node);
+	dListNode* const retnode = (dListNode*) (((char *) node) - offset);
+	dAssert (&retnode->GetInfo () == &info);
+	return retnode;
+}
+
+
+template<class T, int poolSize> 
+void dList<T, poolSize>::Remove (const T &element)
+{
+	dListNode* const node = Find (element);
+	if (node) {
+		Remove (node);
+	}
+}
+
+template<class T, int poolSize>
+void dList<T, poolSize>::Remove (dListNode* const node)
+{
+	m_count --;
+	if (node == m_first) {
+		m_first = m_first->GetNext();
+	}
+	if (node == m_last) {
+		m_last = m_last->GetPrev();
+	}
+	node->Remove();
+	GetAllocator().Free (node);
+	dAssert (Sanity());
+}
+
+template<class T, int poolSize>
+void dList<T, poolSize>::RemoveAll ()
+{
+	while (m_first) {
+		Remove(m_first);
+	}
+	dAssert (!m_count);
+}
+
+template<class T, int poolSize>
+void dList<T, poolSize>::TranferDataToTarget (dList& target)
+{
+	dAssert (target.m_count == 0);
+	target.m_count = m_count;
+	target.m_first = m_first;
+	target.m_last = m_last;
+
+	m_count = 0;
+	m_first = NULL;
+	m_last = NULL;
+}
+
+#endif
+
+

thirdparty/src/newton/dContainers/dPointer.h

@@ -0,0 +1,79 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+
+#ifndef __D_POINTER_H__
+#define __D_POINTER_H__
+
+
+template <typename T>
+class dPointer
+{
+	public:
+	dPointer();
+	dPointer(T* const data);
+	~dPointer();
+	T& operator* ();
+	T* operator-> ();
+
+	T* GetData() const;
+	void SetData(T* const data);
+
+	private:
+	T* m_data; 
+};
+
+template<class T>
+dPointer<T>::dPointer()
+	:m_data(NULL)
+{
+}
+
+
+template<class T>
+dPointer<T>::dPointer(T* const data)
+	:m_data(data)
+{
+}
+
+template<class T>
+dPointer<T>::~dPointer()
+{
+	if (m_data) {
+		delete m_data;
+	}
+}
+
+template<class T>
+T& dPointer<T>::operator* ()
+{
+	return *m_data;
+}
+
+template<class T>
+T* dPointer<T>::operator-> ()
+{
+	return m_data;
+}
+
+template<class T>
+T* dPointer<T>::GetData() const
+{
+	return m_data;
+}
+
+template<class T>
+void dPointer<T>::SetData(T* const data)
+{
+	m_data = data;
+}
+
+#endif

thirdparty/src/newton/dContainers/dRefCounter.cpp

@@ -0,0 +1,46 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dContainersStdAfx.h"
+#include "dRefCounter.h"
+
+dRefCounter::dRefCounter(void)
+	:dContainersAlloc()
+{
+	m_refCount = 1;
+}
+
+dRefCounter::~dRefCounter(void)
+{
+}
+
+
+int dRefCounter::GetRef() const
+{
+	return m_refCount;
+}
+
+int dRefCounter::Release()
+{
+	m_refCount --;
+	dAssert (m_refCount >= 0);
+	if (!m_refCount) {
+		delete this;
+		return 0;
+	}
+	return m_refCount;
+}
+
+void dRefCounter::AddRef() const
+{
+	m_refCount ++;
+}
+

thirdparty/src/newton/dContainers/dRefCounter.h

@@ -0,0 +1,34 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+
+#ifndef __DREF_COUNTER_H__
+#define __DREF_COUNTER_H__
+
+#include "dContainersAlloc.h"
+
+
+class dRefCounter: public dContainersAlloc
+{
+	public:
+	DCONTAINERS_API dRefCounter(void);
+	DCONTAINERS_API int GetRef() const;
+	DCONTAINERS_API int Release();
+	DCONTAINERS_API void AddRef() const;
+
+	protected:
+	DCONTAINERS_API virtual ~dRefCounter(void);
+
+	private:
+	mutable int m_refCount;
+};
+
+#endif

thirdparty/src/newton/dContainers/dRtti.h

@@ -0,0 +1,62 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#ifndef __DRTTI_H__
+#define __DRTTI_H__
+
+#include "dCRC.h"
+#include "dContainersAlloc.h"
+
+#define dRttiCommon(className,exportType)					\
+	private:												\
+	exportType static dCRCTYPE m_rtti;						\
+	public:													\
+	static dCRCTYPE GetRttiType()							\
+	{														\
+		return m_rtti;										\
+	}														\
+	virtual dCRCTYPE GetTypeId () const						\
+	{														\
+		return m_rtti;										\
+	}														\
+
+
+
+// add these macros only to the root base class that you want to have rtti 
+#define dRttiRootClassSupportDeclare(className,exportType)			\
+	dRttiCommon(className,exportType)								\
+	virtual bool IsType (dCRCTYPE typeId) const						\
+	{																\
+		return typeId == m_rtti;									\
+	}												
+
+#define dRttiRootClassSupportImplement(className)					\
+	dCRCTYPE className::m_rtti = dCRC64 (#className);
+
+
+
+// add these macros to every derived class  
+#define dAddRtti(baseClass,exportType)									\
+	dRttiCommon(baseClass,exportType)									\
+	virtual bool IsType (dCRCTYPE typeId) const							\
+	{																	\
+		if (typeId == m_rtti) {											\
+			return true;												\
+		}																\
+		return baseClass::IsType (typeId);								\
+	}													
+
+
+#define dInitRtti(className)											\
+	dRttiRootClassSupportImplement(className)
+
+
+#endif

thirdparty/src/newton/dContainers/dString.cpp

@@ -0,0 +1,571 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dContainersStdAfx.h"
+#include "dString.h"
+
+
+#define D_USE_POOL_BUKECT_ALLOCATOR
+#define D_STRING_MEM_GRANULARITY		16
+#define D_STRING_MEM_MAX_BUCKET_SIZE	256
+#define D_STRING_MEM_BUCKETS			(D_STRING_MEM_MAX_BUCKET_SIZE / D_STRING_MEM_GRANULARITY)
+#define D_DSTRING_ENTRIES_IN_FREELIST	32
+
+
+
+
+class dString::dStringAllocator
+{
+	public:
+	#ifdef D_USE_POOL_BUKECT_ALLOCATOR
+		class dMemBucket
+		{
+			public:
+
+			class dDataChunk
+			{
+				public: 
+				int m_size;
+				int m_count;
+				dDataChunk* m_next;
+				
+			};
+
+			dMemBucket()
+				:m_freeListDataChunk(NULL)
+			{
+			}
+			~dMemBucket()
+			{
+			}
+
+			void Prefetch (int chunckSize)
+			{
+				for (int i = 0; i < D_DSTRING_ENTRIES_IN_FREELIST; i ++) {
+					//dDataChunk* const data = (dDataChunk*) new char[chunckSize + sizeof (int)];
+					dDataChunk* const data = (dDataChunk*) dContainersAlloc::Alloc (chunckSize + sizeof (int));
+					data->m_count = i + 1; 
+					data->m_size = chunckSize;
+					data->m_next = m_freeListDataChunk; 
+					m_freeListDataChunk = data;
+				}
+			}
+
+			void Flush ()
+			{
+				for (int i = 0; m_freeListDataChunk && (i < D_DSTRING_ENTRIES_IN_FREELIST); i ++) {
+					dDataChunk* const ptr = m_freeListDataChunk;
+					m_freeListDataChunk = m_freeListDataChunk->m_next;
+					//delete[] (char*) ptr;
+					dContainersAlloc::Free (ptr);
+				}
+			}
+
+			char* Alloc(int size)
+			{
+				dAssert (size < 1024 * 4);
+				if (!m_freeListDataChunk) {
+					Prefetch (size);
+				}
+				dDataChunk* const data = m_freeListDataChunk;
+				dAssert (size == data->m_size);
+				m_freeListDataChunk = m_freeListDataChunk->m_next;
+				return ((char*)data) + sizeof (int);
+			}
+
+			void Free(char * const ptr)
+			{
+				char* const realPtr = ptr - sizeof (int);
+				dMemBucket::dDataChunk* const dataChunck = (dMemBucket::dDataChunk*) (realPtr);
+
+				dataChunck->m_count = m_freeListDataChunk ? m_freeListDataChunk->m_count + 1 : 1;
+				dataChunck->m_next = m_freeListDataChunk;
+				m_freeListDataChunk = dataChunck;
+				if (dataChunck->m_count >= 2 * D_DSTRING_ENTRIES_IN_FREELIST) {
+					Flush();
+				}
+			}
+
+			dDataChunk* m_freeListDataChunk;
+		};
+
+	
+		dStringAllocator()
+		{
+			for (int i = 0; i < int (sizeof (m_buckects) / sizeof (m_buckects[0])); i ++) {
+				m_buckects[i].Prefetch ((i + 1)* D_STRING_MEM_GRANULARITY);
+			}
+		}
+		~dStringAllocator()
+		{
+			for (int i = 0; i < int (sizeof (m_buckects) / sizeof (m_buckects[0])); i ++) {
+				m_buckects[i].Flush();
+			}
+		}
+
+		char* Alloc(int size)
+		{
+			dAssert (size >= 1);
+			if (size <= D_STRING_MEM_MAX_BUCKET_SIZE) {
+				int buckectEntry = (size - 1) / D_STRING_MEM_GRANULARITY;
+				int buckectSize = (buckectEntry + 1) * D_STRING_MEM_GRANULARITY;
+				return m_buckects[buckectEntry].Alloc(buckectSize);
+			}
+			dMemBucket::dDataChunk* const ptr = (dMemBucket::dDataChunk*) dContainersAlloc::Alloc (size + sizeof (int));
+			ptr->m_size = size;
+			return ((char*)ptr) + sizeof (int);
+		}
+
+		void Free(char* const ptr)
+		{
+			char* const realPtr = ptr-sizeof (int);
+			dMemBucket::dDataChunk* const dataChunck = (dMemBucket::dDataChunk*) (realPtr);
+			if (dataChunck->m_size <= D_STRING_MEM_MAX_BUCKET_SIZE) {
+				int buckectEntry = dataChunck->m_size / D_STRING_MEM_GRANULARITY - 1;
+				m_buckects[buckectEntry].Free(ptr);
+			} else {
+				void* const ptr1 = ((char*)ptr) - sizeof (int);
+				dContainersAlloc::Free (ptr1);
+			}
+		}
+
+		dMemBucket m_buckects [D_STRING_MEM_BUCKETS];
+
+	#else 
+		char* Alloc(int size)
+		{
+			//return new char[size];
+			return (char*) dContainersAlloc::Alloc (size);
+		}
+
+		void Free(char* const ptr)
+		{
+			//delete[] ptr;
+			dContainersAlloc::Free (ptr);
+		}
+	#endif
+};
+
+//dString::dStringAllocator dString::m_allocator;
+
+dString::dString ()
+	:m_string(NULL)
+	,m_size(0)
+	,m_capacity(0)
+{
+}
+
+dString::dString (const dString& src)
+	:m_string(NULL)
+	,m_size(0)
+	,m_capacity(0)
+{
+	if (src.m_string) {
+		m_size = src.m_size;
+		m_capacity = m_size + 1;
+
+		m_string = AllocMem (src.m_size + 1);
+		CopyData (m_string, src.m_string, src.m_size + 1);
+		m_string[m_size] = 0;
+	}
+}
+
+dString::dString (const char* const data)
+	:m_string(NULL)
+	,m_size(0)
+	,m_capacity(0)
+{
+	if (data) {
+		m_size = CalculateSize (data);
+		m_capacity = m_size + 1; 
+
+		m_string = AllocMem (m_size + 1);
+		CopyData (m_string, data, m_size + 1);
+		m_string[m_size] = 0;
+	}
+}
+
+dString::dString (const char* const data, int maxSize)
+	:m_string(NULL)
+	,m_size(0)
+	,m_capacity(0)
+{
+	if (data) {
+		m_size = dMin (CalculateSize (data), maxSize);
+		m_capacity = m_size + 1; 
+		m_string = AllocMem (m_size + 1);
+		CopyData (m_string, data, m_size + 1);
+		m_string[m_size] = 0;
+	}
+}
+
+dString::dString (const dString& src, const char* const concatenate, int concatenateSize)
+	:m_string(NULL)
+	,m_size(0)
+	,m_capacity(0)
+{
+	m_string = AllocMem (src.m_size + concatenateSize + 1);
+	memcpy (m_string, src.m_string, src.m_size);
+	memcpy (&m_string[src.m_size], concatenate, concatenateSize);
+	m_size = src.m_size + concatenateSize;
+	m_string[m_size] = 0;
+	m_capacity = m_size + 1;
+}
+
+
+
+dString::dString (char chr)
+	:m_string(NULL)
+	,m_size(0)
+	,m_capacity(0)
+{
+	m_string = AllocMem (2);
+	m_string[0] = chr;
+	m_string[1] = 0;
+	m_size = 1;
+	m_capacity = m_size + 1;
+}
+
+dString::dString (int val)
+	:m_string(NULL)
+	,m_size(0)
+	,m_capacity(0)
+{
+	char tmp[256];
+
+	int count = 0;
+	unsigned mag = abs (val);
+	do {
+		unsigned digit = mag % 10;
+		mag /= 10;  
+		tmp[count] = '0' + char(digit);
+		count ++;
+	} while (mag > 0);
+
+	int offset = (val >= 0) ? 0: 1;
+	m_string = AllocMem (count + offset + 1);
+	if (offset) {
+		m_string[0] = '-';
+	}
+	for (int i = 0; i < count; i ++) {
+		m_string[i + offset] = tmp[count - i - 1];
+	}
+
+	m_string[count + offset] = 0;
+	m_size = count + offset;
+	m_capacity = m_size + 1;
+}
+
+dString::dString (long long val)
+	:m_string(NULL)
+	,m_size(0)
+	,m_capacity(0)
+{
+	char tmp[256];
+
+	int count = 0;
+	unsigned long long mag = (val > 0ll) ? val : -val;
+	do {
+		unsigned long long digit = mag % 10ll;
+		mag /= 10ll;  
+		tmp[count] = '0' + char(digit);
+		count ++;
+	} while (mag > 0);
+
+	int offset = (val >= 0ll) ? 0: 1;
+	m_string = AllocMem (count + offset + 1);
+	if (offset) {
+		m_string[0] = '-';
+	}
+	for (int i = 0; i < count; i ++) {
+		m_string[i + offset] = tmp[count - i - 1];
+	}
+
+	m_string[count + offset] = 0;
+	m_size = count + offset;
+	m_capacity = m_size + 1;
+}
+
+
+dString::~dString ()
+{
+	Empty();
+}
+
+void dString::Empty()
+{
+	if (m_capacity && m_string) {
+		FreeMem (m_string);
+	}
+	m_size = 0;
+	m_capacity = 0;
+	m_string = NULL;
+}
+
+void dString::LoadFile (FILE* const file)
+{
+	Empty();
+//	fseek (file, 0, SEEK_END);
+//	int size = ftell (file);
+	int size = 0;
+	fseek (file, 0, SEEK_SET);
+	for (;!feof(file); size ++) {
+		fgetc (file);
+	}
+	fseek (file, 0, SEEK_SET);
+	Expand (size);
+	size_t ret = fread (m_string, 1, size, file);
+	ret = 0;
+	m_string[size-1] = 0;
+	m_size = size-1;
+	m_capacity = m_size + 1;
+}
+
+
+
+void dString::operator+= (const char* const src)
+{
+	char* const oldData = m_string;
+	int size = CalculateSize (src);
+	m_string = AllocMem (m_size + size + 1);
+	memcpy (m_string, oldData, m_size);
+	memcpy (&m_string[m_size], src, size);
+	m_size = m_size + size;
+	m_string[m_size] = 0;
+	m_capacity = m_size + 1;
+	FreeMem(oldData);
+}
+
+
+int dString::ToInteger() const
+{
+	int value = 0;
+	if (m_size) {
+		int base = (m_string[0] == '-') ? 1 : 0;
+		for (int i = base; i < m_size; i ++) {
+			char ch = m_string[i]; 		
+			if ((ch >= '0') && (ch <= '9')) {
+				value = value * 10 + ch - '0';
+			} else {
+				break;
+			}
+		}
+		value *= base ? -1 : 1;
+	}
+	return value;
+}
+
+
+long long dString::ToInteger64() const
+{
+	long long value = 0;
+	if (m_size) {
+		int base = (m_string[0] == '-') ? 1 : 0;
+		for (int i = base; i < m_size; i ++) {
+			char ch = m_string[i]; 		
+			if ((ch >= '0') && (ch <= '9')) {
+				value = value * 10ll + ch - '0';
+			} else {
+				break;
+			}
+		}
+		value *= base ? -1 : 1;
+	}
+	return value;
+}
+
+
+double dString::ToFloat() const
+{
+	double value = 0.0;
+	double power = 1.0;
+	double decimalBase = 1.0;
+	if (m_size) {
+		int base = (m_string[0] == '-') ? 1 : 0;
+		for (int i = base; i < m_size; i ++) {
+			char ch = m_string[i]; 		
+			if ((ch >= '0') && (ch <= '9')) {
+				value = value * 10ll + ch - '0';
+				power *= decimalBase;
+			} else if (ch == '.') {
+				decimalBase = 10.0;
+			} else {
+				break;
+			}
+		}
+		value *= base ? -1 : 1;
+	}
+	value /= power;
+
+	return value;
+}
+
+dString& dString::operator= (const dString& src)
+{
+	if (m_capacity && m_string) {
+		FreeMem (m_string);
+	}
+	m_string = NULL;
+	m_capacity = 0;
+	m_size = src.m_size;
+	if (src.m_string) {
+		m_capacity = src.m_size + 1;
+		m_string = AllocMem (src.m_size + 1);
+		CopyData (m_string, src.m_string, src.m_size + 1);
+	}
+	return *this;
+}
+
+int dString::CalculateSize (const char* const data) const
+{
+	int size = 0;
+	if (data) {
+		for (int i = 0; data[i]; i ++) {
+			size ++;
+		}
+	}
+	return size;
+}
+
+void dString::ToUpper()
+{
+	if (m_string) {
+		for (char * cp = m_string; *cp; ++cp) {
+			if ((*cp >= 'a') && (*cp <= 'z') )
+				*cp += 'A' - 'a';
+		}
+	}
+}
+
+void dString::ToLower()
+{
+	if (m_string) {
+		for (char * cp = m_string; *cp; ++cp) {
+			if ((*cp >= 'A') && (*cp <= 'Z') )
+				*cp += 'a' - 'A';
+		}
+	}
+}
+
+int dString::Find (char ch, int from) const
+{
+	for (int i = from; i < m_size; i ++) {
+		if (m_string[i] == ch) {
+			return i;
+		}
+	}
+	return -1;
+}
+
+//int dString::Find (const dString& subStream, int from) const
+int dString::Find (const char* const subString, int subStringLength, int from, int lenght) const
+{
+	dAssert (from >= 0);
+	//dAssert (subStream.m_size >= 0);
+	dAssert (subStringLength >= 1);
+
+	int location = -1;
+	if (m_size) {
+		const int str2Size = dMin (subStringLength, lenght);
+		if (str2Size == 1) {
+			char ch = subString[0];
+			const char* const ptr1 = m_string;
+			for (int i = 0; i < m_size; i ++) {
+				if (ch == ptr1[i]) {
+					return i;
+				}
+			}
+		} else if ((str2Size < 4) || (m_size < 64)) {
+			const int size = m_size - str2Size;
+			for (int j = from; j <= size; j ++) {
+				const char* const ptr1 = &m_string[j];
+				int i = 0;
+				while (subString[i] && (ptr1[i] == subString[i])) {
+					i ++;
+				}
+				if (!subString[i]) {
+					return j;
+				}
+			}
+		} else {
+			// for large strings smart search
+			short frequency[256];
+			memset (frequency, -1, sizeof (frequency));
+			for (int i = 0; i < str2Size; i ++) {
+				frequency[int (subString[i])] = short(i);
+			}
+
+			int j = from;
+			const int size = m_size - str2Size;
+			while (j <= size) {
+				const char* const ptr1 = &m_string[j];
+				int i = str2Size - 1;
+				while ((i >= 0) && (ptr1[i] == subString[i])) {
+					i --;
+				}
+				if (i < 0) {
+					return j;
+					
+				}
+				j += dMax(i - frequency[int (ptr1[i])], 1);
+			}
+		}
+	}
+	return location;
+}
+
+
+void dString::Replace (int start, int size, const char* const str, int strSize)
+{
+	char* const oldData = m_string;
+	m_string = AllocMem (m_size - size + strSize + 1);
+	memcpy (m_string, oldData, start);
+	memcpy (&m_string[start], str, strSize);
+	memcpy (&m_string[start + strSize], &oldData[start + size], m_size - (start + size));
+	m_size = m_size - size + strSize;
+	m_capacity = m_size - size + strSize + 1;
+	m_string[m_size] = 0;
+	FreeMem(oldData);
+}
+
+
+void dString::Expand (int size)
+{
+	char* const oldData = m_string;
+	m_string = AllocMem (m_size + size + 1);
+	
+	if (m_capacity) {
+		memcpy (m_string, oldData, m_size);
+		FreeMem(oldData);
+	}
+	m_string[m_size] = 0;
+	m_capacity = m_size + size + 1;
+}
+
+
+dString::dStringAllocator& dString::GetAllocator() const
+{
+	static dStringAllocator allocator;
+	return allocator;
+}
+
+
+char* dString::AllocMem(int size)
+{
+	return GetAllocator().Alloc(size);
+}
+
+void dString::FreeMem (char* const ptr)
+{
+	if (ptr) {
+		GetAllocator().Free(ptr);
+	}
+}

thirdparty/src/newton/dContainers/dString.h

@@ -0,0 +1,214 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+
+#ifndef __DSTRING_H_
+#define __DSTRING_H_
+
+#include "dContainersStdAfx.h"
+#include "dContainersAlloc.h"
+
+
+class dString: public dContainersAlloc
+{
+	class dStringAllocator;
+	public:
+	DCONTAINERS_API dString ();
+	DCONTAINERS_API dString (char chr);
+	DCONTAINERS_API dString (const dString& src);
+	DCONTAINERS_API dString (const char* const data);
+	DCONTAINERS_API dString (const char* const data, int maxSize);
+	DCONTAINERS_API dString (int val);
+	DCONTAINERS_API dString (long long val);
+	DCONTAINERS_API ~dString ();
+
+	char& operator[] (int index);
+	char operator[] (int index) const;
+	
+	DCONTAINERS_API dString& operator= (const dString& src);
+	bool operator== (const dString& src) const;
+	bool operator!= (const dString& src) const;
+	bool operator< (const dString& src) const;
+	bool operator> (const dString& src) const;
+	bool operator<= (const dString& src) const;
+	bool operator>= (const dString& src) const;
+
+	DCONTAINERS_API void operator+= (const char* const src);
+	void operator+= (const dString& src);
+
+	dString operator+ (const char* const src) const;
+	dString operator+ (const dString& src) const;
+
+	DCONTAINERS_API int Find (char ch, int from = 0) const;
+	int Find (const dString& subString, int from = 0) const;
+	DCONTAINERS_API int Find (const char* const subString, int from = 0, int lenght = 0x7ffffff) const;
+
+	DCONTAINERS_API void Replace (int start, int size, const char* const str, int strSize);
+	void Replace (int start, int size, const dString& str);
+	void Empty();
+
+	DCONTAINERS_API void ToUpper();
+	DCONTAINERS_API void ToLower();
+	DCONTAINERS_API int ToInteger() const;
+	DCONTAINERS_API double ToFloat() const;
+	DCONTAINERS_API long long ToInteger64() const;
+
+	int Size() const;
+	int Capacity() const;
+	DCONTAINERS_API void Expand (int size);
+
+	DCONTAINERS_API void LoadFile (FILE* const file);
+	dString SubString(int start = 0, int size = 0x7fffffff) const;
+
+	const char* GetStr () const;
+
+	private:
+	DCONTAINERS_API int CalculateSize (const char* const data) const;
+	int Compare (const char* const str0, const char* const str1) const;
+	void CopyData (char* const dst, const char* const src, int size) const;
+
+	DCONTAINERS_API int Find (const char* const subString, int stringSize, int from, int lenght) const;
+
+
+	protected:
+	char* AllocMem(int size);
+	void FreeMem (char* const ptr);
+	DCONTAINERS_API dString (const dString& src, const char* const concatenate, int maxSize);
+	
+	char* m_string;
+	int m_size;
+	int m_capacity;
+
+	private:
+	dStringAllocator& GetAllocator() const;
+};
+
+
+inline char& dString::operator[] (int index)
+{
+	dAssert (m_string);
+	dAssert (index >= 0);
+	dAssert (index < m_size);
+	return m_string[index];
+}
+
+inline char dString::operator[] (int index) const
+{
+	dAssert (m_string);
+	dAssert (index >= 0);
+	dAssert (index < m_size);
+	return m_string[index];
+}
+
+inline const char* dString::GetStr () const
+{
+	return m_string;
+}
+
+inline int dString::Size() const
+{
+	return m_size;
+}
+
+
+inline int dString::Find (const char* const subString, int from, int lenght) const
+{
+	return Find (subString, CalculateSize(subString), from, lenght);
+}
+
+inline int dString::Find (const dString& subStream, int from) const
+{
+	dAssert (subStream.m_string);
+	return Find (subStream.m_string, subStream.m_size, from, subStream.m_size);
+}
+
+inline void dString::Replace (int start, int size, const dString& str)
+{
+	Replace(start, size, str.m_string, str.m_size);
+}
+
+inline void dString::operator+= (const dString& src)
+{
+	*this += src.m_string;
+}
+
+inline dString dString::operator+ (const dString& src) const
+{
+	return dString (*this, src.m_string, src.m_size);
+}
+
+inline dString dString::operator+ (const char* const copy) const
+{
+	return dString (*this, copy, CalculateSize (copy));
+}
+
+
+inline int dString::Capacity() const
+{
+	return m_capacity;
+}
+
+inline void dString::CopyData (char* const dst, const char* const src, int size) const
+{
+	dAssert (dst);
+	dAssert (src);
+	memcpy (dst, src, size);
+}
+
+inline int dString::Compare (const char* const str0, const char* const str1) const
+{
+	dAssert (str0);
+	dAssert (str1);
+	return strcmp (str0, str1);
+}
+
+
+inline bool dString::operator== (const dString& src) const
+{
+	return Compare (m_string, src.m_string) == 0;
+}
+
+inline bool dString::operator!= (const dString& src) const
+{
+	return Compare (m_string, src.m_string) != 0;
+}
+
+
+inline bool dString::operator< (const dString& src) const
+{
+	return Compare (m_string, src.m_string) < 0;
+}
+
+inline bool dString::operator> (const dString& src) const
+{
+	return Compare (m_string, src.m_string) > 0;
+}
+
+inline bool dString::operator<= (const dString& src) const
+{
+	return Compare (m_string, src.m_string) <= 0;
+}
+
+inline bool dString::operator>= (const dString& src) const
+{
+	return Compare (m_string, src.m_string) >= 0;
+}
+
+inline dString dString::SubString(int start, int size) const
+{
+	dAssert (m_string);
+	return dString (&m_string[start], size);
+}
+
+
+#endif
+
+

thirdparty/src/newton/dContainers/dTree.cpp

@@ -0,0 +1,369 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dContainersStdAfx.h"
+#include "dTree.h"
+
+
+dRedBackNode* dRedBackNode::Minimum () const
+{
+	dRedBackNode* ptr = (dRedBackNode*) this;
+	for (; ptr->m_left; ptr = ptr->m_left);
+	return ptr;
+}
+
+dRedBackNode* dRedBackNode::Maximum () const
+{
+	dRedBackNode* ptr = (dRedBackNode*) this;
+	for (; ptr->m_right; ptr = ptr->m_right);
+	return ptr;
+}
+
+
+dRedBackNode* dRedBackNode::Prev () const
+{
+	if (m_left) {
+		return m_left->Maximum ();
+	}
+
+	dRedBackNode* me = (dRedBackNode*) this;
+	dRedBackNode* ptr = m_parent;
+	for (; ptr && me == ptr->m_left; ptr = ptr->m_parent) {
+		me = ptr;
+	}
+	return ptr;
+
+}
+
+dRedBackNode* dRedBackNode::Next () const
+{
+	if (m_right) {
+		return m_right->Minimum ();
+	}
+
+	dRedBackNode* node = (dRedBackNode*) this;
+	dRedBackNode* ptr = m_parent;
+	for (; ptr && node == ptr->m_right; ptr = ptr->m_parent) {
+		node = ptr;
+	}
+	return ptr;
+}
+
+// rotate node me to left 
+void dRedBackNode::RotateLeft(dRedBackNode** const head) 
+{
+	dRedBackNode* const me = this;
+	dRedBackNode* const child = me->m_right;
+	
+	dAssert (child);
+	//establish me->m_right link 
+	me->m_right = child->m_left;
+	if (child->m_left != NULL) {
+		child->m_left->m_parent = me;
+	}
+	
+	// establish child->m_parent link 
+	if (child != NULL) {
+		child->m_parent = me->m_parent;
+	}
+	if (me->m_parent) {
+		if (me == me->m_parent->m_left) {
+			me->m_parent->m_left = child;
+		} else {
+			me->m_parent->m_right = child;
+		}
+	} else {
+		*head = child;
+	}
+	
+	// link child and me 
+	child->m_left = me;
+	if (me != NULL) {
+		me->m_parent = child;
+	}
+}
+
+
+// rotate node me to right  *
+void dRedBackNode::RotateRight(dRedBackNode** const head) 
+{
+	dRedBackNode* const me = this;
+	dRedBackNode* const child = me->m_left;
+
+	dAssert (child);
+	// establish me->m_left link 
+	me->m_left = child->m_right;
+	if (child->m_right != NULL) {
+		child->m_right->m_parent = me;
+	}
+
+	// establish child->m_parent link 
+	if (child != NULL) {
+		child->m_parent = me->m_parent;
+	}
+	if (me->m_parent) {
+		if (me == me->m_parent->m_right) {
+			me->m_parent->m_right = child;
+		} else {
+			me->m_parent->m_left = child;
+		}
+	} else {
+		*head = child;
+	}
+
+	// link me and child 
+	child->m_right = me;
+	if (me != NULL) {
+		me->m_parent = child;
+	}
+}
+
+
+// maintain Red-Black tree balance after inserting node ptr   
+void dRedBackNode::InsertFixup(dRedBackNode** const head) 
+{
+	dRedBackNode* ptr = this;
+	// check Red-Black properties 
+	dAssert ((ptr == *head) || ptr->m_parent);
+	while ((ptr != *head) && (ptr->m_parent->GetColor() == RED)) {
+		// we have a violation 
+		dAssert (ptr->m_parent);
+		dAssert (ptr->m_parent->m_parent);
+		if (ptr->m_parent == ptr->m_parent->m_parent->m_left) {
+			dRedBackNode* const tmp = ptr->m_parent->m_parent->m_right;
+			if (tmp && (tmp->GetColor() == RED)) {
+				// uncle is RED 
+				ptr->m_parent->SetColor(BLACK);
+				tmp->SetColor(BLACK) ;
+				ptr->m_parent->m_parent->SetColor(RED) ;
+				ptr = ptr->m_parent->m_parent;
+			} else {
+				// uncle is BLACK 
+				if (ptr == ptr->m_parent->m_right) {
+					// make ptr a left child 
+					ptr = ptr->m_parent;
+					ptr->RotateLeft(head);
+				}
+
+				ptr->m_parent->SetColor(BLACK);
+				if (ptr->m_parent->m_parent) {
+					ptr->m_parent->m_parent->SetColor(RED);
+					ptr->m_parent->m_parent->RotateRight(head);
+				}
+			}
+		} else {
+			//dAssert (ptr->m_parent == ptr->m_parent->m_parent->m_right);
+			// mirror image of above code 
+			dRedBackNode* const tmp = ptr->m_parent->m_parent->m_left;
+			if (tmp && (tmp->GetColor() == RED)) {
+				//uncle is RED 
+				ptr->m_parent->SetColor(BLACK);
+				tmp->SetColor(BLACK) ;
+				ptr->m_parent->m_parent->SetColor(RED) ;
+				ptr = ptr->m_parent->m_parent;
+			} else {
+				// uncle is BLACK 
+				if (ptr == ptr->m_parent->m_left) {
+					ptr = ptr->m_parent;
+					ptr->RotateRight(head);
+				}
+				ptr->m_parent->SetColor(BLACK);
+				if (ptr->m_parent->m_parent->GetColor() == BLACK) {
+					ptr->m_parent->m_parent->SetColor(RED) ;
+				   ptr->m_parent->m_parent->RotateLeft (head); 
+				}
+			}
+		}
+	}
+	(*head)->SetColor(BLACK);
+}
+
+
+//maintain Red-Black tree balance after deleting node x 
+void dRedBackNode::RemoveFixup (dRedBackNode* const me, dRedBackNode** const head) 
+{
+	dRedBackNode* ptr = this;
+	dRedBackNode* node = me;
+	while ((node != *head) && (!node || node->GetColor() == BLACK)) {
+		if (node == ptr->m_left) {
+			if (!ptr) {
+				return;
+			}
+			dRedBackNode* tmp = ptr->m_right;
+			if (!tmp) {
+				return;
+			}
+			if (tmp->GetColor() == RED) {
+				tmp->SetColor(BLACK) ;
+				ptr->SetColor(RED) ;
+				ptr->RotateLeft (head);
+				tmp = ptr->m_right;
+				//if (!ptr || !tmp) {
+				if (!tmp) {
+					return;
+				}
+			}
+			if ((!tmp->m_left  || (tmp->m_left->GetColor() == BLACK)) && 
+				 (!tmp->m_right || (tmp->m_right->GetColor() == BLACK))) {
+				tmp->SetColor(RED);
+				node = ptr;
+				ptr = ptr->m_parent;
+				continue;
+			} else if (!tmp->m_right || (tmp->m_right->GetColor() == BLACK)) {
+				tmp->m_left->SetColor(BLACK);
+				tmp->SetColor(RED);
+				tmp->RotateRight (head);
+				tmp = ptr->m_right;
+				//if (!ptr || !tmp) {
+				if (!tmp) {
+					return;
+				}
+			}
+			tmp->SetColor (ptr->GetColor());
+			if (tmp->m_right) {
+				tmp->m_right->SetColor(BLACK) ;
+			}
+			if (ptr) {
+				ptr->SetColor(BLACK) ;
+				ptr->RotateLeft (head);
+			}
+			node = *head;
+
+		} else {
+		  	if (!ptr) {
+				return;
+		  	}
+			dRedBackNode* tmp = ptr->m_left;
+			if (!tmp) {
+				return;
+			}
+			if (tmp->GetColor() == RED) {
+				tmp->SetColor(BLACK) ;
+				ptr->SetColor(RED) ;
+				ptr->RotateRight (head);
+				tmp = ptr->m_left;
+				//if (!ptr || !tmp) {
+				if (!tmp) {
+					return;
+				}
+			}
+
+			if ((!tmp->m_right || (tmp->m_right->GetColor() == BLACK)) && 
+				 (!tmp->m_left  || (tmp->m_left->GetColor() == BLACK))) {
+				tmp->SetColor(RED) ;
+				node = ptr;
+				ptr = ptr->m_parent;
+				continue;
+			} else if (!tmp->m_left || (tmp->m_left->GetColor() == BLACK)) {
+				tmp->m_right->SetColor(BLACK) ;
+				tmp->SetColor(RED) ;
+				tmp->RotateLeft (head);
+				tmp = ptr->m_left;
+				//if (!ptr || !tmp) {
+				if (!tmp) {
+					return;
+				}
+			}
+			tmp->SetColor (ptr->GetColor());
+			if (tmp->m_left) {
+				tmp->m_left->SetColor(BLACK);
+			}
+			if (ptr) {
+				ptr->SetColor(BLACK) ;
+				ptr->RotateRight (head);
+			}
+			node = *head;
+		}
+	}
+	if (node) {
+		node->SetColor(BLACK);
+	}
+}
+
+void dRedBackNode::Unlink (dRedBackNode** const head)
+{
+	dRedBackNode* const node = this;
+	node->SetInTreeFlag(false);
+
+	if (!node->m_left || !node->m_right) {
+		// y has a NULL node as a child 
+		dRedBackNode* const endNode = node;
+
+		// x is y's only child 
+		dRedBackNode* child = endNode->m_right;
+		if (endNode->m_left) {
+			child = endNode->m_left;
+		}
+
+		// remove y from the parent chain 
+		if (child) {
+			child->m_parent = endNode->m_parent;
+		}
+
+		if (endNode->m_parent)	{
+			if (endNode == endNode->m_parent->m_left) {
+				endNode->m_parent->m_left = child;
+			} else {
+				endNode->m_parent->m_right = child;
+			}
+		} else {
+			*head = child;
+		}
+
+		if (endNode->GetColor() == BLACK) {
+			endNode->m_parent->RemoveFixup (child, head);
+		}
+	} else {
+
+		// find tree successor with a NULL node as a child 
+		dRedBackNode* endNode = node->m_right;
+		while (endNode->m_left != NULL) {
+			endNode = endNode->m_left;
+		}
+
+		// x is y's only child 
+		dRedBackNode* const child = endNode->m_right;
+
+		endNode->m_left = node->m_left;
+		node->m_left->m_parent = endNode;
+
+		dRedBackNode* endNodeParent = endNode;
+		if (endNode	!= node->m_right) {
+			if (child) {
+				child->m_parent = endNode->m_parent;
+			}
+			endNode->m_parent->m_left = child;
+			endNode->m_right = node->m_right;
+			node->m_right->m_parent = endNode;
+			endNodeParent = endNode->m_parent;
+		}
+
+
+		if (node == *head) {
+			*head = endNode;
+		} else if (node == node->m_parent->m_left) {
+			node->m_parent->m_left = endNode;
+		} else {
+			node->m_parent->m_right = endNode;
+		}
+		endNode->m_parent = node->m_parent;
+
+		bool oldColor = endNode->GetColor();
+		endNode->SetColor (node->GetColor());
+		node->SetColor	(oldColor);
+
+		if (oldColor == BLACK) {
+			endNodeParent->RemoveFixup (child, head);
+		}
+	}
+}
+
+

thirdparty/src/newton/dContainers/dTree.h

@@ -0,0 +1,796 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+
+#ifndef __dTree__
+#define __dTree__
+
+#include "dContainersStdAfx.h"
+#include "dContainersAlloc.h"
+
+
+// Note: this is a low level class for dTree use only
+// unpredictable result will happen if you attempt to manipulate
+// any member of this class
+class dRedBackNode
+{
+	public:
+	enum REDBLACK_COLOR
+	{
+		RED = true,
+		BLACK = false
+	};
+
+	public:
+	dRedBackNode* GetLeft() const;
+	dRedBackNode* GetRight() const;
+	dRedBackNode* GetParent() const;
+	dRedBackNode (dRedBackNode* const parent);
+	DCONTAINERS_API dRedBackNode* Prev() const;
+	DCONTAINERS_API dRedBackNode* Next() const;
+	DCONTAINERS_API dRedBackNode* Minimum() const;
+	DCONTAINERS_API dRedBackNode* Maximum() const;
+
+	protected:
+	virtual ~dRedBackNode () 
+	{
+	}
+
+	void Initdata (dRedBackNode* const parent);
+	void SetColor (bool color);
+	bool GetColor () const;
+	bool IsInTree () const;
+	void SetInTreeFlag (bool flag);
+	DCONTAINERS_API void RotateLeft(dRedBackNode** const head); 
+	DCONTAINERS_API void RotateRight(dRedBackNode** const head); 
+	DCONTAINERS_API void RemoveFixup (dRedBackNode* const node, dRedBackNode** const head); 
+	DCONTAINERS_API void Unlink (dRedBackNode** const head);
+	DCONTAINERS_API void InsertFixup(dRedBackNode** const head); 
+	
+	bool m_color;
+	bool m_inTree;
+	dRedBackNode* m_left;
+	dRedBackNode* m_right;
+	dRedBackNode* m_parent;
+};
+
+
+
+template<class OBJECT, class KEY, int poolSize = D_MAX_ENTRIES_IN_FREELIST>
+class dTree: public dContainersAlloc 
+{
+	public:
+	class dTreeNode: public dRedBackNode
+	{
+		dTreeNode (
+			const KEY &key, 
+			dTreeNode* parentNode)
+			:dRedBackNode(parentNode), m_info (), m_key (key)
+		{
+		}
+	
+		dTreeNode (
+			const OBJECT &info, 
+			const KEY &key, 
+			dTreeNode* parentNode)
+			:dRedBackNode(parentNode), m_info (info), m_key (key)
+		{
+		}
+
+		virtual ~dTreeNode () 
+		{
+		}
+
+		dTreeNode& operator= (dTreeNode& src)
+		{
+			dAssert (0);
+			return* this;
+		}
+
+		dTreeNode* GetLeft () const
+		{
+			return (dTreeNode*) dRedBackNode::m_left;
+		}
+
+		dTreeNode* GetRight () const
+		{
+			return (dTreeNode*) dRedBackNode::m_right;
+		}
+
+		dTreeNode* GetParent ()
+		{
+			return (dTreeNode*) dRedBackNode::m_parent;
+		}
+
+		void SetLeft (dTreeNode* const node)
+		{
+			dRedBackNode::m_left = node;
+		}
+
+		void SetRight (dTreeNode* const node)
+		{
+			dRedBackNode::m_right = node;
+		}
+
+		void SetParent (dTreeNode* const node)
+		{
+			dRedBackNode::m_parent = node;
+		}
+
+		public:
+		const KEY& GetKey() const
+		{
+			return m_key;
+		}
+
+		OBJECT& GetInfo()
+		{
+			return m_info;
+		}
+
+		private:
+		OBJECT m_info;
+		KEY m_key; 
+		friend class dTree<OBJECT, KEY>;
+	};
+
+	class Iterator
+	{
+		public:
+		Iterator(const dTree<OBJECT,KEY> &me)
+		{
+			m_ptr = NULL;
+			m_tree = &me;
+		}
+
+		~Iterator()
+		{
+		}
+
+		void Begin() 
+		{
+			m_ptr = m_tree->Minimum();
+		}
+
+		void End()  
+		{
+			m_ptr = m_tree->Maximum();
+		}
+
+		void Set (dTreeNode* const node)
+		{
+			m_ptr = node;
+		}
+
+		operator int() const 
+		{
+			return m_ptr != NULL;
+		}
+
+		void operator++ ()
+		{
+			//dAssert (m_ptr);
+			m_ptr = m_ptr->Next();
+		}
+
+		void operator++ (int)
+		{
+			//dAssert (m_ptr);
+			m_ptr = m_ptr->Next();
+		}
+
+		void operator-- () 
+		{
+			//dAssert (m_ptr);
+			m_ptr = m_ptr->Prev();
+		}
+
+		void operator-- (int) 
+		{
+			//dAssert (m_ptr);
+			m_ptr = m_ptr->Prev();
+		}
+
+		OBJECT &operator* () const 
+		{
+			return ((dTreeNode*)m_ptr)->GetInfo();
+		}
+
+		dTreeNode* GetNode() const
+		{
+			return (dTreeNode*)m_ptr;
+		}
+
+		KEY GetKey () const
+		{
+			dTreeNode* const tmp = (dTreeNode*)m_ptr;
+			return tmp ? tmp->GetKey() : KEY(0);
+		}
+
+		private:
+		dRedBackNode* m_ptr;
+		const dTree* m_tree;
+	};
+
+
+	// ***********************************************************
+	// member functions
+	// ***********************************************************
+	public:
+	dTree ();
+	virtual ~dTree (); 
+
+	operator int() const;
+	int GetCount() const;
+
+	dTreeNode* GetRoot () const;
+	dTreeNode* Minimum () const;
+	dTreeNode* Maximum () const;
+
+	dTreeNode* Find (KEY key) const;
+	dTreeNode* FindGreater (KEY key) const;
+	dTreeNode* FindGreaterEqual (KEY key) const;
+	dTreeNode* FindLessEqual (KEY key) const;
+
+	dTreeNode* GetNodeFromInfo (OBJECT &info) const;
+
+	dTreeNode* Insert (KEY key);
+	dTreeNode* Insert (const OBJECT &element, KEY key);
+	dTreeNode* Insert (const OBJECT &element, KEY key, bool& elementWasInTree);
+	dTreeNode* Insert (dTreeNode* const node, KEY key);
+
+	dTreeNode* Replace (OBJECT &element, KEY key);
+	dTreeNode* ReplaceKey (KEY oldKey, KEY newKey);
+	dTreeNode* ReplaceKey (dTreeNode* const node, KEY key);
+
+	void Unlink (dTreeNode* const node);
+
+	void Remove (KEY key);
+	void Remove (dTreeNode* const node);
+	void RemoveAll (); 
+
+	bool SanityCheck () const;
+
+
+	// ***********************************************************
+	// member variables
+	// ***********************************************************
+	private:
+	dContainerFixSizeAllocator& GetAllocator()
+	{
+		static dContainerFixSizeAllocator* allocator = NULL;
+		if (!allocator) {
+			allocator = dContainerFixSizeAllocator::Create (sizeof (dTree<OBJECT, KEY, poolSize>::dTreeNode), poolSize);
+		}
+		return *allocator;
+	}
+
+
+	void RemoveAllLow (dTreeNode* const root);
+	int CompareKeys (const KEY &key0, const KEY &key1) const;
+	bool SanityCheck (dTreeNode* const ptr, int height) const;
+
+	int m_count;
+	dTreeNode* m_head;
+	friend class dTreeNode;
+};
+
+
+inline dRedBackNode::dRedBackNode (dRedBackNode* const parent)
+{
+	Initdata (parent);
+}
+
+inline void dRedBackNode::Initdata (dRedBackNode* const parent)
+{
+	SetColor (RED);
+	SetInTreeFlag (true);
+	m_left = NULL;
+	m_right = NULL;
+	m_parent = parent;
+}
+
+inline void dRedBackNode::SetColor (bool color)
+{
+	m_color = color;
+}
+
+inline bool dRedBackNode::GetColor () const
+{
+	return m_color;
+}
+
+inline bool dRedBackNode::IsInTree () const
+{
+	return m_inTree;
+}
+
+inline void dRedBackNode::SetInTreeFlag (bool flag)
+{
+	m_inTree = flag;
+}
+
+
+
+template<class OBJECT, class KEY, int poolSize>
+dTree<OBJECT, KEY, poolSize>::dTree ()
+{
+	m_count	= 0;
+	m_head = NULL;
+	GetAllocator();
+}
+
+
+template<class OBJECT, class KEY, int poolSize>
+dTree<OBJECT, KEY, poolSize>::~dTree () 
+{
+	RemoveAll();
+}
+
+
+template<class OBJECT, class KEY, int poolSize>
+dTree<OBJECT, KEY, poolSize>::operator int() const
+{
+	return m_head != NULL;
+}
+
+template<class OBJECT, class KEY, int poolSize>
+int dTree<OBJECT, KEY, poolSize>::GetCount() const
+{
+	return m_count;
+}
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Minimum () const
+{
+	return m_head ? (dTreeNode*) m_head->Minimum() : NULL;
+}
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Maximum () const
+{
+	return m_head ? (dTreeNode*) m_head->Maximum() : NULL;
+}
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::GetRoot () const
+{
+	return m_head;
+}
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Find (KEY key) const
+{
+	if (m_head == NULL) {
+		return NULL;
+	}
+
+	dTreeNode* ptr = m_head;
+	while (ptr != NULL) {
+		int val = CompareKeys (ptr->m_key, key);
+		if (!val) {
+			break;
+		}
+		if (val < 0) {
+			ptr = ptr->GetLeft();
+		} else {
+			ptr = ptr->GetRight();
+		}
+	}
+	return ptr;
+}
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::GetNodeFromInfo (OBJECT &info) const
+{
+	dTreeNode* node = (dTreeNode*) &info;
+	int offset = ((char*) &node->m_info) - ((char *) node);
+	node = (dTreeNode*) (((char *) node) - offset);
+
+//	dAssert (node->IsInTree ());
+	dAssert (&node->GetInfo () == &info);
+	return (node->IsInTree ()) ? node : NULL;
+}
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::FindGreater (KEY key) const
+{
+	if (m_head == NULL) {
+		return NULL;
+	}
+
+	dTreeNode* prev = NULL;
+	dTreeNode* ptr = m_head;
+	int val = 0;
+	while (ptr != NULL) {
+		val = CompareKeys (ptr->m_key, key);
+		if (!val) {
+			return (dTreeNode*) ptr->Next();
+		}
+		prev = ptr;
+		if (val < 0) {
+			ptr = ptr->GetLeft();
+		} else {
+			ptr = ptr->GetRight();
+		}
+	}
+
+	if (val > 0) {
+		while (prev->m_parent && (prev->m_parent->m_right == prev)) {
+			prev = prev->GetParent(); 
+		}
+		prev = prev->GetParent(); 
+	}
+	return (dTreeNode*) prev; 
+}
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::FindGreaterEqual (KEY key) const
+{
+	if (m_head == NULL) {
+		return NULL;
+	}
+
+	dTreeNode* prev = NULL;
+	dTreeNode* ptr = m_head;
+	int val = 0;
+	while (ptr != NULL) {
+		val = CompareKeys (ptr->m_key, key);
+		if (!val) {
+			return ptr;
+		}
+		prev = ptr;
+		if (val < 0) {
+			ptr = ptr->GetLeft();
+		} else {
+			ptr = ptr->GetRight();
+		}
+	}
+
+	if (val > 0) {
+		while (prev->m_parent && (prev->m_parent->m_right == prev)) {
+			prev = prev->GetParent(); 
+		}
+		prev = prev->GetParent(); 
+	}
+	return (dTreeNode*) prev; 
+}
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::FindLessEqual (KEY key) const
+{
+	if (m_head == NULL) {
+		return NULL;
+	}
+
+	dTreeNode* prev = NULL;
+	dTreeNode* ptr = m_head;
+	int val = 0;
+	while (ptr != NULL) {
+		val = CompareKeys (ptr->m_key, key);
+		if (!val) {
+			return ptr;
+		}
+		prev = ptr;
+		if (val < 0) {
+			ptr = ptr->GetLeft();
+		} else {
+			ptr = ptr->GetRight();
+		}
+	}
+
+	if (val < 0) {
+		while (prev->m_parent && (prev->m_parent->m_left == prev)) {
+			prev = prev->GetParent(); 
+		}
+		prev = prev->GetParent(); 
+	}
+	return (dTreeNode*) prev; 
+}
+
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Insert (KEY key)
+{
+	dTreeNode* parent = NULL;
+	dTreeNode* ptr = m_head;
+	int val = 0;
+	while (ptr != NULL) {
+		parent = ptr;
+		val = CompareKeys (ptr->m_key, key);
+
+		if (val < 0) {
+			ptr = ptr->GetLeft();
+		} else if (val > 0) {
+			ptr = ptr->GetRight();
+		} else {
+			return ptr;
+		}
+	}
+	m_count	++;
+
+	ptr = new (GetAllocator().Alloc()) dTreeNode (key, parent);
+	if (!parent) {
+		m_head = ptr;
+	} else {
+		if (val < 0) {
+			parent->m_left = ptr; 
+		} else {
+			parent->m_right = ptr;
+		}
+	}
+	ptr->InsertFixup ((dRedBackNode**)&m_head);
+	return ptr;
+}
+
+
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Insert (const OBJECT &element, KEY key, bool& elementWasInTree)
+{
+	dTreeNode* parent = NULL;
+	dTreeNode* ptr = m_head;
+	int val = 0;
+	elementWasInTree = false;
+	while (ptr != NULL) {
+		parent = ptr;
+		val = CompareKeys (ptr->m_key, key);
+
+		if (val < 0) {
+			ptr = ptr->GetLeft();
+		} else if (val > 0) {
+			ptr = ptr->GetRight();
+		} else {
+			elementWasInTree = true;
+			return ptr;
+		}
+	}
+	m_count	++;
+
+	ptr = new (GetAllocator().Alloc()) dTreeNode (element, key, parent);
+	if (!parent) {
+		m_head = ptr;
+	} else {
+		if (val < 0) {
+			parent->m_left = ptr; 
+		} else {
+			parent->m_right = ptr;
+		}
+	}
+	ptr->InsertFixup ((dRedBackNode**)&m_head);
+	return ptr;
+}
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Insert (const OBJECT &element, KEY key)
+{
+	bool foundState;
+	dTreeNode* node = Insert (element, key, foundState);
+	if (foundState) {
+		node = NULL;
+	}
+	return node;
+}
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Insert (typename dTree<OBJECT, KEY, poolSize>::dTreeNode* const node, KEY key)
+{
+	int val = 0;
+	dTreeNode* ptr = m_head;
+	dTreeNode* parent = NULL;
+	while (ptr != NULL) {
+		parent = ptr;
+		val = CompareKeys (ptr->m_key, key);
+
+		if (val < 0) {
+			ptr = ptr->GetLeft();
+		} else if (val > 0) {
+			ptr = ptr->GetRight();
+		} else {
+			return NULL;
+		}
+	}
+
+	m_count	++;
+
+	ptr = node;
+	ptr->m_key = key;
+	ptr->Initdata (parent);
+
+	if (!parent) {
+		m_head = ptr;
+	} else {
+		if (val < 0) {
+			parent->m_left = ptr; 
+		} else {
+			parent->m_right = ptr;
+		}
+	}
+	ptr->InsertFixup ((dRedBackNode**)&m_head);
+	return ptr;
+}
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::Replace (OBJECT &element, KEY key)
+{
+	dTreeNode* parent = NULL;
+	dTreeNode* ptr = m_head;
+	int val = 0;
+	while (ptr != NULL) {
+		parent = ptr;
+		val = CompareKeys (ptr->m_key, key);
+		if (val == 0) {
+			ptr->m_info = element;
+			return ptr;
+		}
+		if (val < 0) {
+			ptr = ptr->GetLeft();
+		} else {
+			ptr = ptr->GetRight();
+		}
+	}
+
+	ptr = new (GetAllocator().Alloc()) dTreeNode (element, key, parent);
+	if (!parent) {
+		m_head = ptr;
+	} else {
+		if (val < 0) {
+			parent->m_left = ptr; 
+		} else {
+			parent->m_right = ptr;
+		}
+	}
+	ptr->InsertFixup ((dRedBackNode**)&m_head);
+	return ptr;
+}
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::ReplaceKey (typename dTree<OBJECT, KEY, poolSize>::dTreeNode* node, KEY key)
+{
+	Unlink(node);
+	dTreeNode* const ptr = Insert (node, key);
+
+	dAssert (ptr);
+	return ptr;
+}
+
+template<class OBJECT, class KEY, int poolSize>
+typename dTree<OBJECT, KEY, poolSize>::dTreeNode* dTree<OBJECT, KEY, poolSize>::ReplaceKey (KEY oldKey, KEY newKey)
+{
+	dTreeNode* const node = Find (oldKey);
+	return node ? ReplaceKey (node, newKey) : NULL;
+}
+
+template<class OBJECT, class KEY, int poolSize>
+void dTree<OBJECT, KEY, poolSize>::Unlink (typename dTree<OBJECT, KEY, poolSize>::dTreeNode* const node)
+{
+	m_count	--;
+	node->Unlink((dRedBackNode** )&m_head);
+}
+
+
+template<class OBJECT, class KEY, int poolSize>
+void dTree<OBJECT, KEY, poolSize>::Remove (typename dTree<OBJECT, KEY, poolSize>::dTreeNode* const node)
+{
+	m_count	--;
+	node->Unlink ((dRedBackNode** )&m_head);
+	node->~dTreeNode();
+	GetAllocator().Free (node);
+}
+
+template<class OBJECT, class KEY, int poolSize>
+void dTree<OBJECT, KEY, poolSize>::Remove (KEY key) 
+{
+	// find node in tree 
+	dTreeNode* const node = Find (key);
+	if (node) {
+		Remove (node);
+	}
+}
+
+
+template<class OBJECT, class KEY, int poolSize>
+void dTree<OBJECT, KEY, poolSize>::RemoveAllLow (dTreeNode* const root) 
+{
+	if (root->m_left) {
+		RemoveAllLow((dTreeNode*)root->m_left);
+	}
+	if (root->m_right) {
+		RemoveAllLow ((dTreeNode*)root->m_right);
+	}
+	root->SetInTreeFlag(false);
+	root->~dTreeNode();
+	GetAllocator().Free (root);
+}
+
+
+template<class OBJECT, class KEY, int poolSize>
+void dTree<OBJECT, KEY, poolSize>::RemoveAll () 
+{
+	if (m_head) {
+		m_count	 = 0;
+		dTreeNode* root;
+		for (root = m_head; root->m_parent; root = (dTreeNode*)root->m_parent);
+		RemoveAllLow(root);
+		m_head = NULL;
+	}
+}
+
+template<class OBJECT, class KEY, int poolSize>
+bool dTree<OBJECT, KEY, poolSize>::SanityCheck () const
+{
+	return SanityCheck (m_head, 0);
+}
+
+
+template<class OBJECT, class KEY, int poolSize>
+bool dTree<OBJECT, KEY, poolSize>::SanityCheck (typename dTree<OBJECT, KEY, poolSize>::dTreeNode* ptr, int height) const
+{
+	if (!ptr) {
+		return true;
+	}
+
+	if (ptr->m_left) {
+		if (CompareKeys (ptr->m_key, ptr->GetLeft()->m_key) > 0) {
+			return false;
+		}
+	}
+
+	if (ptr->m_right) {
+		if (CompareKeys (ptr->m_key, ptr->GetRight()->m_key) < 0) {
+			return false;
+		}
+	}
+
+	if (ptr->GetColor() == dTreeNode::BLACK) {
+		height ++;
+	} else if (!((!ptr->m_left  || (ptr->m_left->GetColor() == dTreeNode::BLACK)) &&
+		       (!ptr->m_right || (ptr->m_right->GetColor() == dTreeNode::BLACK)))) {
+	  	return false;
+	}
+
+	if (!ptr->m_left && !ptr->m_right) {
+		int bh = 0;
+		for (dTreeNode* x = ptr; x; x = x->GetParent()) {
+	 		if (x->GetColor() == dTreeNode::BLACK) {
+				bh ++;
+			}
+		}
+		if (bh != height) {
+			return false;
+		}
+	}
+
+	if (ptr->m_left && !SanityCheck (ptr->GetLeft(), height)) {
+		return false;
+	}
+
+	if (ptr->m_right && !SanityCheck (ptr->GetRight(), height)) {
+		return false;
+	}
+	return true;
+}
+
+template<class OBJECT, class KEY, int poolSize>
+int dTree<OBJECT, KEY, poolSize>::CompareKeys (const KEY &key0, const KEY &key1) const
+{
+	if (key1 < key0) {
+		return - 1;
+	}
+	if (key1 > key0) {
+		return 1;
+	}
+	return 0;
+}
+
+#endif
+
+

thirdparty/src/newton/dMath/CMakeLists.txt

@@ -0,0 +1,48 @@
+# Copyright (c) <2014-2017> <Newton Game Dynamics>
+#
+# This software is provided 'as-is', without any express or implied
+# warranty. In no event will the authors be held liable for any damages
+# arising from the use of this software.
+#
+# Permission is granted to anyone to use this software for any purpose,
+# including commercial applications, and to alter it and redistribute it
+# freely.
+
+cmake_minimum_required(VERSION 3.4.0)
+
+set (projectName "dMath")
+message (${projectName})
+
+# low level core
+file(GLOB CPP_SOURCE *.h *.cpp)
+file(GLOB HEADERS *.h)
+
+if (UNIX OR MINGW)
+	if(NEWTON_BUILD_SHARED_LIBS)
+	   add_library(${projectName} SHARED ${CPP_SOURCE})
+	else(NEWTON_BUILD_SHARED_LIBS)
+	   add_library(${projectName} STATIC ${CPP_SOURCE})
+	endif(NEWTON_BUILD_SHARED_LIBS)
+endif (UNIX OR MINGW)
+
+if (MSVC)
+   add_library(${projectName} STATIC ${CPP_SOURCE})
+
+   if(CMAKE_VS_MSBUILD_COMMAND OR CMAKE_VS_DEVENV_COMMAND)
+      set_target_properties(${projectName} PROPERTIES COMPILE_FLAGS "/YudStdAfxMath.h")
+      set_source_files_properties(dStdAfxMath.cpp PROPERTIES COMPILE_FLAGS "/YcdStdAfxMath.h")
+   endif()
+endif(MSVC)
+
+target_include_directories(${projectName} PUBLIC .)
+
+if (NEWTON_BUILD_PROFILER)
+    target_link_libraries (${projectName} dProfiler)
+endif()
+
+install(TARGETS ${projectName} 
+		LIBRARY DESTINATION lib
+		ARCHIVE DESTINATION lib
+		RUNTIME DESTINATION bin)
+
+install(FILES ${HEADERS} DESTINATION include/${projectName})

thirdparty/src/newton/dMath/dLinearAlgebra.cpp

@@ -0,0 +1,948 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dStdAfxMath.h"
+#include "dMathDefines.h"
+#include "dLinearAlgebra.h"
+
+#define COMPLEMENTARITY_VEL_DAMP			dFloat(100.0f)
+#define COMPLEMENTARITY_POS_DAMP			dFloat(1500.0f)
+#define COMPLEMENTARITY_PSD_DAMP_TOL		dFloat(1.0e-4f)
+#define COMPLEMENTARITY_STACK_ENTRIES		64
+
+
+void dSymmetricBiconjugateGradientSolve::ScaleAdd (int size, dFloat64* const a, const dFloat64* const b, dFloat64 scale, const dFloat64* const c) const
+{
+	for (int i = 0; i < size; i ++) {
+		a[i] = b[i] + scale * c[i];
+	}
+}
+
+void dSymmetricBiconjugateGradientSolve::Sub (int size, dFloat64* const a, const dFloat64* const b, const dFloat64* const c) const
+{
+	for (int i = 0; i < size; i ++) {
+		a[i] = b[i] - c[i];
+	}
+}
+
+dFloat64 dSymmetricBiconjugateGradientSolve::DotProduct (int size, const dFloat64* const b, const dFloat64* const c) const
+{
+	dFloat64 product = dFloat64 (0.0f);
+	for (int i = 0; i < size; i ++) {
+		product += b[i] * c[i];
+	}
+	return product;
+}
+
+dFloat64 dSymmetricBiconjugateGradientSolve::Solve (int size, dFloat64 tolerance, dFloat64* const x, const dFloat64* const b) const
+{
+	dFloat64* const r0 = new dFloat64 [size];
+	dFloat64* const p0 = new dFloat64 [size];
+	dFloat64* const MinvR0 = new dFloat64 [size];
+	dFloat64* const matrixP0 = new dFloat64 [size];
+
+	MatrixTimeVector (matrixP0, x);
+	Sub(size, r0, b, matrixP0);
+	bool continueExecution = InversePrecoditionerTimeVector (p0, r0);
+
+	int iter = 0;
+	dFloat64 num = DotProduct (size, r0, p0);
+	dFloat64 error2 = num;
+	for (int j = 0; (j < size) && (error2 > tolerance) && continueExecution; j ++) {
+
+		MatrixTimeVector (matrixP0, p0);
+		dFloat64 den = DotProduct (size, p0, matrixP0);
+
+		dAssert (fabs(den) > dFloat64 (0.0f));
+		dFloat64 alpha = num / den;
+
+		ScaleAdd (size, x, x, alpha, p0);
+        if ((j % 50) != 49) {
+		    ScaleAdd (size, r0, r0, -alpha, matrixP0);
+        } else {
+            MatrixTimeVector (matrixP0, x);
+            Sub(size, r0, b, matrixP0);
+        }
+
+		continueExecution = InversePrecoditionerTimeVector (MinvR0, r0);
+
+		dFloat64 num1 = DotProduct (size, r0, MinvR0);
+		dFloat64 beta = num1 / num;
+		ScaleAdd (size, p0, MinvR0, beta, p0);
+		num = DotProduct (size, r0, MinvR0);
+		iter ++;
+		error2 = num;
+		if (j > 10) {
+			error2 = dFloat64 (0.0f);
+			for (int i = 0; i < size; i ++) {
+				error2 = dMax (error2, r0[i] * r0[i]);
+			}
+		}
+	}
+
+	delete[] matrixP0;
+	delete[] MinvR0;
+	delete[] p0;
+	delete[] r0;
+
+	dAssert (iter <= size);
+	return num;
+}
+
+
+dComplementaritySolver::dBodyState::dBodyState()
+	:m_matrix(dGetIdentityMatrix())
+	,m_localFrame(dGetIdentityMatrix())
+	,m_inertia(dGetZeroMatrix())
+	,m_invInertia(dGetZeroMatrix())
+	,m_localInertia (0.0f)
+	,m_localInvInertia(0.0f)
+	,m_veloc(0.0f)
+	,m_omega(0.0f)
+	,m_externalForce(0.0f)
+	,m_externalTorque(0.0f)
+	,m_globalCentreOfMass(0.0f)
+	,m_mass(0.0f)
+	,m_invMass(0.0f)
+	,m_myIndex(0)
+{
+}
+
+const dVector& dComplementaritySolver::dBodyState::GetOmega() const
+{
+	return m_omega;
+}
+
+const dVector& dComplementaritySolver::dBodyState::GetVelocity() const
+{
+	return m_veloc;
+}
+
+dVector dComplementaritySolver::dBodyState::CalculatePointVelocity (const dVector& point) const
+{
+	return m_veloc + m_omega.CrossProduct(point - m_globalCentreOfMass);
+}
+
+dFloat dComplementaritySolver::dBodyState::GetMass () const
+{
+	return m_mass;
+}
+
+dFloat dComplementaritySolver::dBodyState::GetInvMass () const
+{
+	return m_invMass;
+}
+
+void dComplementaritySolver::dBodyState::SetMass (dFloat mass)
+{
+	m_mass = mass;
+	m_invMass = mass > (1.0e-3f) ?  1.0f / mass : 0.0f;
+}
+
+void dComplementaritySolver::dBodyState::SetInertia (dFloat Ixx, dFloat Iyy, dFloat Izz)
+{
+	m_localInertia[0] = Ixx;
+	m_localInertia[1] =	Iyy;
+	m_localInertia[2] =	Izz;
+	m_localInvInertia[0] = Ixx ? 1.0f / Ixx : 0.0f;
+	m_localInvInertia[1] = Iyy ? 1.0f / Iyy : 0.0f;
+	m_localInvInertia[2] = Izz ? 1.0f / Izz : 0.0f;
+}
+
+void dComplementaritySolver::dBodyState::GetInertia (dFloat& Ixx, dFloat& Iyy, dFloat& Izz) const
+{
+	Ixx = m_localInertia[0];
+	Iyy = m_localInertia[1];
+	Izz = m_localInertia[2];
+}
+
+const dMatrix& dComplementaritySolver::dBodyState::GetInertia() const
+{
+	return m_inertia;
+}
+
+const dMatrix& dComplementaritySolver::dBodyState::GetInvInertia() const
+{
+	return m_invInertia;
+}
+
+void dComplementaritySolver::dBodyState::SetMatrix (const dMatrix& matrix)
+{
+	m_matrix = matrix;
+	m_globalCentreOfMass = m_matrix.TransformVector(m_localFrame.m_posit);
+}
+
+const dMatrix& dComplementaritySolver::dBodyState::GetMatrix () const
+{
+	return m_matrix;
+}
+
+void dComplementaritySolver::dBodyState::SetLocalMatrix (const dMatrix& matrix)
+{
+	m_localFrame = matrix;
+	m_globalCentreOfMass = m_matrix.TransformVector(m_localFrame.m_posit);
+}
+
+const dMatrix& dComplementaritySolver::dBodyState::GetLocalMatrix () const
+{
+	return m_localFrame;
+}
+
+const dVector& dComplementaritySolver::dBodyState::GetCOM () const
+{
+	return m_localFrame.m_posit;
+}
+
+void dComplementaritySolver::dBodyState::SetVeloc (const dVector& veloc)
+{
+	m_veloc = veloc;
+}
+
+void dComplementaritySolver::dBodyState::SetOmega (const dVector& omega)
+{
+	m_omega = omega;
+}
+
+void dComplementaritySolver::dBodyState::SetForce (const dVector& force)
+{
+	m_externalForce = force;
+}
+
+void dComplementaritySolver::dBodyState::SetTorque (const dVector& torque)
+{
+	m_externalTorque = torque;
+}
+
+const dVector& dComplementaritySolver::dBodyState::GetForce () const
+{
+	return m_externalForce;
+}
+
+const dVector& dComplementaritySolver::dBodyState::GetTorque () const
+{
+	return m_externalTorque;
+}
+
+void dComplementaritySolver::dBodyState::UpdateInertia()
+{
+	dMatrix tmpMatrix (dGetZeroMatrix());
+
+	tmpMatrix[0] = m_localInertia * dVector (m_matrix[0][0], m_matrix[1][0], m_matrix[2][0], dFloat(0.0f));
+	tmpMatrix[1] = m_localInertia * dVector (m_matrix[0][1], m_matrix[1][1], m_matrix[2][1], dFloat(0.0f));
+	tmpMatrix[2] = m_localInertia * dVector (m_matrix[0][2], m_matrix[1][2], m_matrix[2][2], dFloat(0.0f));
+	m_inertia = tmpMatrix * m_matrix;
+
+	tmpMatrix[0] = m_localInvInertia * dVector (m_matrix[0][0], m_matrix[1][0], m_matrix[2][0], dFloat(0.0f));
+	tmpMatrix[1] = m_localInvInertia * dVector (m_matrix[0][1], m_matrix[1][1], m_matrix[2][1], dFloat(0.0f));
+	tmpMatrix[2] = m_localInvInertia * dVector (m_matrix[0][2], m_matrix[1][2], m_matrix[2][2], dFloat(0.0f));
+	m_invInertia = tmpMatrix * m_matrix;
+}
+
+void dComplementaritySolver::dBodyState::IntegrateForce (dFloat timestep, const dVector& force, const dVector& torque)
+{
+	dVector accel (force.Scale (m_invMass));
+	dVector alpha (m_invInertia.RotateVector(torque));
+	m_veloc += accel.Scale (timestep);
+	m_omega += alpha.Scale (timestep);
+}
+
+void dComplementaritySolver::dBodyState::IntegrateVelocity (dFloat timestep)
+{
+	// this is the fucking bug cause the vehicle malfunction
+	const dFloat D_MAX_ANGLE_STEP = dFloat (90.0f * dDegreeToRad);
+	while ((m_omega.DotProduct3(m_omega) * timestep * timestep) > (D_MAX_ANGLE_STEP * D_MAX_ANGLE_STEP)) {
+		dAssert (0);
+		m_omega = m_omega.Scale (dFloat (0.8f));
+	}
+
+	// this is correct
+	const dFloat D_ANGULAR_TOL = dFloat (0.0125f * dDegreeToRad);
+	m_globalCentreOfMass += m_veloc.Scale (timestep); 
+	dFloat omegaMag2 = m_omega.DotProduct3(m_omega);
+	if (omegaMag2 > (D_ANGULAR_TOL * D_ANGULAR_TOL)) {
+		dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
+		dVector omegaAxis (m_omega.Scale (invOmegaMag));
+		dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
+		dQuaternion rotation (omegaAxis, omegaAngle);
+		dQuaternion rotMatrix (m_matrix);
+		rotMatrix = rotMatrix * rotation;
+		rotMatrix.Scale( 1.0f / dSqrt (rotMatrix.DotProduct (rotMatrix)));
+		m_matrix = dMatrix (rotMatrix, m_matrix.m_posit);
+	}
+
+	m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localFrame.m_posit);
+
+#ifdef _DEBUG
+	int j0 = 1;
+	int j1 = 2;
+	for (int i = 0; i < 3; i ++) {
+		dAssert (m_matrix[i][3] == 0.0f);
+		dFloat val = m_matrix[i].DotProduct3(m_matrix[i]);
+		dAssert (dAbs (val - 1.0f) < 1.0e-5f);
+		dVector tmp (m_matrix[j0].CrossProduct(m_matrix[j1]));
+		val = tmp.DotProduct3(m_matrix[i]);
+		dAssert (dAbs (val - 1.0f) < 1.0e-5f);
+		j0 = j1;
+		j1 = i;
+	}
+#endif
+}
+
+void dComplementaritySolver::dBodyState::ApplyNetForceAndTorque (dFloat invTimestep, const dVector& veloc, const dVector& omega)
+{
+	dVector accel = (m_veloc - veloc).Scale(invTimestep);
+	dVector alpha = (m_omega - omega).Scale(invTimestep);
+
+	m_externalForce = accel.Scale(m_mass);
+	alpha = m_matrix.UnrotateVector(alpha);
+	m_externalTorque = m_matrix.RotateVector(alpha * m_localInertia);
+}
+
+void dComplementaritySolver::dBilateralJoint::Init(dBodyState* const state0, dBodyState* const state1)
+{
+	m_start = 0;
+	m_count = 0;
+	memset (m_rowIsMotor, 0, sizeof (m_rowIsMotor));
+	memset (m_motorAcceleration, 0, sizeof (m_motorAcceleration));
+	//memset (m_jointFeebackForce, 0, sizeof (m_jointFeebackForce));
+
+	m_state0 = state0;
+	m_state1 = state1;
+}
+
+void dComplementaritySolver::dBilateralJoint::AddContactRowJacobian (dParamInfo* const constraintParams, const dVector& pivot, const dVector& dir, dFloat restitution)
+{
+	dVector r0 (pivot - m_state0->m_globalCentreOfMass);
+	dVector r1 (pivot - m_state1->m_globalCentreOfMass);
+
+	int index = constraintParams->m_count;
+
+	dAssert(dir.m_w == 0.0f);
+	dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_J01;
+	dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_J10;
+
+	jacobian0.m_linear = dir;
+	jacobian0.m_angular = r0.CrossProduct(jacobian0.m_linear);
+
+	jacobian1.m_linear = dir.Scale(-1.0f);
+	jacobian1.m_angular = r1.CrossProduct(jacobian1.m_linear);
+
+	const dVector& omega0 = m_state0->m_omega;
+	const dVector& omega1 = m_state1->m_omega;
+	const dVector& veloc0 = m_state0->m_veloc;
+	const dVector& veloc1 = m_state1->m_veloc;
+
+	const dVector veloc(jacobian0.m_linear * veloc0 + jacobian0.m_angular * omega0 + jacobian1.m_linear * veloc1 + jacobian1.m_angular * omega1);
+	const dVector relAccel(veloc.Scale(constraintParams->m_timestepInv * (1.0f + restitution)));
+
+	dAssert(relAccel.m_w == 0.0f);
+	constraintParams->m_frictionCallback[index] = NULL;
+	constraintParams->m_diagonalRegularizer[index] = 0.0f;
+	constraintParams->m_jointAccel[index] = -(relAccel.m_x + relAccel.m_y + relAccel.m_z);
+	constraintParams->m_jointLowFrictionCoef[index] = D_COMPLEMENTARITY_MIN_FRICTION_BOUND;
+	constraintParams->m_jointHighFrictionCoef[index] = D_COMPLEMENTARITY_MAX_FRICTION_BOUND;
+	constraintParams->m_count = index + 1;
+}
+
+
+void dComplementaritySolver::dBilateralJoint::AddLinearRowJacobian(dParamInfo* const constraintParams, const dVector& pivot, const dVector& dir)
+{
+	dVector r0 (pivot - m_state0->m_globalCentreOfMass);
+	dVector r1 (pivot - m_state1->m_globalCentreOfMass);
+
+	int index = constraintParams->m_count;
+
+	dAssert(dir.m_w == 0.0f);
+	dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_J01;
+	dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_J10;
+
+	jacobian0.m_linear = dir;
+	jacobian0.m_angular = r0.CrossProduct(jacobian0.m_linear);
+
+	jacobian1.m_linear = dir.Scale(-1.0f);
+	jacobian1.m_angular = r1.CrossProduct(jacobian1.m_linear);
+
+	const dVector& omega0 = m_state0->m_omega;
+	const dVector& omega1 = m_state1->m_omega;
+	const dVector& veloc0 = m_state0->m_veloc;
+	const dVector& veloc1 = m_state1->m_veloc;
+
+	dVector centripetal0(omega0.CrossProduct(omega0.CrossProduct(r0)));
+	dVector centripetal1(omega1.CrossProduct(omega1.CrossProduct(r1)));
+	const dVector accel(jacobian0.m_linear * centripetal0 + jacobian1.m_linear * centripetal1);
+	const dVector veloc(jacobian0.m_linear * veloc0 + jacobian0.m_angular * omega0 + jacobian1.m_linear * veloc1 + jacobian1.m_angular * omega1);
+	const dVector relAccel(accel + veloc.Scale(constraintParams->m_timestepInv));
+
+	dAssert(relAccel.m_w == 0.0f);
+	constraintParams->m_frictionCallback[index] = NULL;
+	constraintParams->m_diagonalRegularizer[index] = 0.0f;
+	constraintParams->m_jointAccel[index] = -(relAccel.m_x + relAccel.m_y + relAccel.m_z);
+	constraintParams->m_jointLowFrictionCoef[index] = D_COMPLEMENTARITY_MIN_FRICTION_BOUND;
+	constraintParams->m_jointHighFrictionCoef[index] = D_COMPLEMENTARITY_MAX_FRICTION_BOUND;
+	constraintParams->m_count = index + 1;
+}
+
+void dComplementaritySolver::dBilateralJoint::AddAngularRowJacobian (dParamInfo* const constraintParams, const dVector& dir, dFloat jointAngle)
+{
+	int index = constraintParams->m_count;
+	dAssert(dir.m_w == 0.0f);
+
+	dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_J01; 
+	dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_J10;
+
+	jacobian0.m_linear = dVector(0.0f);
+	jacobian1.m_linear = dVector(0.0f);
+
+	jacobian0.m_angular = dir;
+	jacobian1.m_angular = dir.Scale (-1.0f);
+
+	const dVector& omega0 = m_state0->m_omega;
+	const dVector& omega1 = m_state1->m_omega;
+	const dVector omega (omega0 * jacobian0.m_angular + omega1 * jacobian1.m_angular);
+	const dVector alpha (omega.Scale (constraintParams->m_timestepInv));
+
+	constraintParams->m_frictionCallback[index] = NULL;
+	constraintParams->m_diagonalRegularizer[index] = 0.0f;
+	constraintParams->m_jointAccel[index] = -(alpha.m_x + alpha.m_y + alpha.m_z);
+	constraintParams->m_jointLowFrictionCoef[index] = D_COMPLEMENTARITY_MIN_FRICTION_BOUND;
+	constraintParams->m_jointHighFrictionCoef[index] = D_COMPLEMENTARITY_MAX_FRICTION_BOUND;
+	constraintParams->m_count = index + 1;
+}
+
+dFloat dComplementaritySolver::dBilateralJoint::GetRowAccelaration(dParamInfo* const constraintParams) const
+{ 
+	dAssert(constraintParams->m_count > 0);
+	return constraintParams->m_jointAccel[constraintParams->m_count - 1]; 
+}
+
+void dComplementaritySolver::dBilateralJoint::SetRowAccelaration(dParamInfo* const constraintParams, dFloat accel)
+{
+	dAssert(constraintParams->m_count > 0);
+	constraintParams->m_jointAccel[constraintParams->m_count - 1] = accel;
+}
+
+dFloat dComplementaritySolver::dBilateralJoint::CalculateMassMatrixDiagonal(dParamInfo* const constraintParams) const
+{
+/*
+	dAssert(constraintParams->m_count > 0);
+	const int index = constraintParams->m_count - 1;
+	const dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_J01;
+	const dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_J10;
+
+	const dMatrix& invInertia0 = m_state0->m_invInertia;
+	const dMatrix& invInertia1 = m_state1->m_invInertia;
+
+	const dFloat invMass0 = m_state0->m_invMass;
+	const dFloat invMass1 = m_state1->m_invMass;
+
+	const dVector JMinvIM0linear(jacobian0.m_linear.Scale(invMass0));
+	const dVector JMinvIM1linear(jacobian1.m_linear.Scale(invMass1));
+	const dVector JMinvIM0angular = invInertia0.UnrotateVector(jacobian0.m_angular);
+	const dVector JMinvIM1angular = invInertia1.UnrotateVector(jacobian1.m_angular);
+	const dVector tmpDiag(JMinvIM0linear * jacobian0.m_linear + JMinvIM0angular * jacobian0.m_angular + 
+						  JMinvIM1linear * jacobian1.m_linear + JMinvIM1angular * jacobian1.m_angular);
+
+	dFloat invEffectiveMass = (tmpDiag[0] + tmpDiag[1] + tmpDiag[2]);
+	springConst *= invEffectiveMass;
+	damperConst *= invEffectiveMass;
+
+springConst *= 2.0f;
+damperConst *= 0.5f;
+
+	const dFloat timestep = constraintParams->m_timestep;
+
+	//at =  [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] 
+	dFloat accel = springConst * posit + damperConst * speed + timestep * springConst * speed;
+	// yes I know this is the correct implicit term, but made the behavior too soft, so I am fudging it 
+	//dFloat den = dFloat(1.0f) + timestep * kd + timestep * ksd;
+	//dFloat den = timestep * damperConst + timestep * ksd;
+	dFloat den = 0.25f * timestep * (timestep * springConst + damperConst);
+
+	constraintParams->m_jointAccel[index] = - accel;
+	constraintParams->m_diagonalRegularizer[index] = den;
+*/
+
+	dAssert(constraintParams->m_count > 0);
+	const int index = constraintParams->m_count - 1;
+	const dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_J01;
+	const dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_J10;
+
+	const dMatrix& invInertia0 = m_state0->m_invInertia;
+	const dMatrix& invInertia1 = m_state1->m_invInertia;
+
+	const dFloat invMass0 = m_state0->m_invMass;
+	const dFloat invMass1 = m_state1->m_invMass;
+
+	const dVector JMinvIM0linear(jacobian0.m_linear.Scale(invMass0));
+	const dVector JMinvIM1linear(jacobian1.m_linear.Scale(invMass1));
+	const dVector JMinvIM0angular = invInertia0.UnrotateVector(jacobian0.m_angular);
+	const dVector JMinvIM1angular = invInertia1.UnrotateVector(jacobian1.m_angular);
+	const dVector tmpDiag(JMinvIM0linear * jacobian0.m_linear + JMinvIM0angular * jacobian0.m_angular +
+						  JMinvIM1linear * jacobian1.m_linear + JMinvIM1angular * jacobian1.m_angular);
+
+	return tmpDiag[0] + tmpDiag[1] + tmpDiag[2];
+}
+
+/*
+dFloat dComplementaritySolver::dBilateralJoint::CalculateRowZeroAccelaration (dParamInfo* const constraintParams) const
+{
+	const int i = constraintParams->m_count - 1;
+	dAssert (i >= 0);
+
+	const dVector& omega0 = m_state0->GetOmega();
+	const dVector& omega1 = m_state1->GetOmega();
+	const dVector& veloc0 = m_state0->GetVelocity();
+	const dVector& veloc1 = m_state1->GetVelocity();
+	dVector accel(constraintParams->m_jacobians[i].m_jacobian_J01.m_linear * veloc0 +
+				  constraintParams->m_jacobians[i].m_jacobian_J01.m_angular * omega0 +
+				  constraintParams->m_jacobians[i].m_jacobian_J10.m_linear * veloc1 +
+				  constraintParams->m_jacobians[i].m_jacobian_J10.m_angular * omega1);
+	return -(accel.m_x + accel.m_y + accel.m_z) * constraintParams->m_timestepInv;
+}
+*/
+
+dFloat dComplementaritySolver::dBilateralJoint::CalculateAngle (const dVector& planeDir, const dVector& cosDir, const dVector& sinDir) const
+{
+	dFloat cosAngle = planeDir.DotProduct3(cosDir);
+	dFloat sinAngle = sinDir.DotProduct3(planeDir.CrossProduct(cosDir));
+	return dAtan2(sinAngle, cosAngle);
+}
+
+/*
+void dComplementaritySolver::dBilateralJoint::AddAngularRowJacobian (dParamInfo* const constraintParams, const dVector& dir0, const dVector& dir1, dFloat accelerationRatio)
+{
+	int index = constraintParams->m_count;
+	dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_IM0; 
+
+	jacobian0.m_linear[0] = 0.0f;
+	jacobian0.m_linear[1] = 0.0f;
+	jacobian0.m_linear[2] = 0.0f;
+	jacobian0.m_linear[3] = 0.0f;
+	jacobian0.m_angular[0] = dir0.m_x;
+	jacobian0.m_angular[1] = dir0.m_y;
+	jacobian0.m_angular[2] = dir0.m_z;
+	jacobian0.m_angular[3] = 0.0f;
+
+	dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_IM1; 
+	jacobian1.m_linear[0] = 0.0f;
+	jacobian1.m_linear[1] = 0.0f;
+	jacobian1.m_linear[2] = 0.0f;
+	jacobian1.m_linear[3] = 0.0f;
+	jacobian1.m_angular[0] = dir1.m_x;
+	jacobian1.m_angular[1] = dir1.m_y;
+	jacobian1.m_angular[2] = dir1.m_z;
+	jacobian1.m_angular[3] = 0.0f;
+
+	m_rowIsMotor[index] = true;
+	m_motorAcceleration[index] = accelerationRatio;
+	constraintParams->m_jointAccel[index] = 0.0f;
+	constraintParams->m_jointLowFriction[index] = D_COMPLEMENTARITY_MIN_FRICTION_BOUND;
+	constraintParams->m_jointHighFriction[index] = D_COMPLEMENTARITY_MAX_FRICTION_BOUND;
+	constraintParams->m_count = index + 1;
+}
+*/
+
+void dComplementaritySolver::dBilateralJoint::JointAccelerations (dJointAccelerationDecriptor* const params)
+{
+	dJacobianColum* const jacobianColElements = params->m_colMatrix;
+	dJacobianPair* const jacobianRowElements = params->m_rowMatrix;
+
+	const dVector& bodyVeloc0 = m_state0->m_veloc;
+	const dVector& bodyOmega0 = m_state0->m_omega;
+	const dVector& bodyVeloc1 = m_state1->m_veloc;
+	const dVector& bodyOmega1 = m_state1->m_omega;
+
+	dFloat timestep = params->m_timeStep;
+	dFloat kd = COMPLEMENTARITY_VEL_DAMP * dFloat (4.0f);
+	dFloat ks = COMPLEMENTARITY_POS_DAMP * dFloat (0.25f);
+	for (int k = 0; k < params->m_rowsCount; k ++) {
+		if (m_rowIsMotor[k]) {
+			jacobianColElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianColElements[k].m_deltaAccel;
+		} else {
+			const dJacobianPair& Jt = jacobianRowElements[k];
+			dVector relVeloc (Jt.m_jacobian_J01.m_linear * bodyVeloc0 +
+							  Jt.m_jacobian_J01.m_angular * bodyOmega0 + 
+							  Jt.m_jacobian_J10.m_linear * bodyVeloc1 +
+							  Jt.m_jacobian_J10.m_angular * bodyOmega1);
+
+			dFloat vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
+			dFloat aRel = jacobianColElements[k].m_deltaAccel;
+			dFloat ksd = timestep * ks;
+			dFloat relPosit = 0.0f - vRel * timestep * params->m_firstPassCoefFlag;
+
+			dFloat num = ks * relPosit - kd * vRel - ksd * vRel;
+			dFloat den = dFloat (1.0f) + timestep * kd + timestep * ksd;
+			dFloat aRelErr = num / den;
+			jacobianColElements[k].m_coordenateAccel = aRelErr + aRel;
+		}
+	}
+}
+
+int dComplementaritySolver::dFrictionLessContactJoint::CompareContact (const dContact* const contactA, const dContact* const contactB, void* dommy)
+{
+	if (contactA->m_point[0] < contactB->m_point[0]) {
+		return -1;
+	} else if (contactA->m_point[0] > contactB->m_point[0]) {
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+int dComplementaritySolver::dFrictionLessContactJoint::ReduceContacts (int count, dContact* const contacts, dFloat tol)
+{
+	int mask[D_MAX_PLACEMENT_CONTACTS];
+	int index = 0;
+	int packContacts = 0;
+	dFloat window = tol;
+	dFloat window2 = window * window;
+	memset (mask, 0, size_t (count));
+	dSort (contacts, count, CompareContact, NULL);
+	dAssert (count <= D_MAX_PLACEMENT_CONTACTS);
+	for (int i = 0; i < count; i ++) {
+		if (!mask[i]) {
+			dFloat val = contacts[i].m_point[index] + window;
+			for (int j = i + 1; (j < count) && (contacts[j].m_point[index] < val) ; j ++) {
+				if (!mask[j]) {
+					dVector dp (contacts[j].m_point - contacts[i].m_point);
+					dFloat dist2 = dp.DotProduct3(dp);
+					if (dist2 < window2) {
+						mask[j] = 1;
+						packContacts = 1;
+					}
+				}
+			}
+		}
+	}
+
+	if (packContacts) {
+		int j = 0;
+		for (int i = 0; i < count; i ++) {
+			dAssert (i < D_MAX_PLACEMENT_CONTACTS);
+			if (!mask[i]) {
+				contacts[j] = contacts[i];
+				j ++;
+			}
+		}
+		count = j;
+	}
+	return count;
+}
+
+void dComplementaritySolver::dFrictionLessContactJoint::SetContacts (int count, dContact* const contacts, dFloat restitution)
+{
+	dFloat tol = 5.0e-3f;
+	count = ReduceContacts(count, contacts, tol);
+	while (count > D_MAX_PRAM_INFO_SIZE) {
+		tol *= 2.0f; 
+		count = ReduceContacts(count, contacts, tol);
+	}
+
+	m_count = count;
+	m_restitution = restitution;
+	memcpy (m_contacts, contacts, count * sizeof (dContact));
+}
+
+void dComplementaritySolver::dFrictionLessContactJoint::JacobianDerivative (dParamInfo* const constraintParams)
+{
+	for (int i = 0; i < m_count; i ++) {
+		AddLinearRowJacobian(constraintParams, m_contacts[i].m_point, m_contacts[i].m_point);
+		dAssert(0);
+/*
+		dVector velocError (pointData.m_veloc1 - pointData.m_veloc0);
+		//dFloat restitution = 0.05f;
+		dFloat relVelocErr = velocError.DotProduct3(m_contacts[i].m_normal);
+		dFloat penetration = 0.0f;
+		dFloat penetrationStiffness = 0.0f;
+		dFloat penetrationVeloc = penetration * penetrationStiffness;
+
+		if (relVelocErr > dFloat(1.0e-3f)) {
+			relVelocErr *= (m_restitution + dFloat (1.0f));
+		}
+
+		constraintParams->m_normalIndex[i] = 0;
+		constraintParams->m_frictionCallback[index] = NULL;
+		constraintParams->m_jointLowFrictionCoef[i] = dFloat (0.0f);
+		constraintParams->m_jointAccel[i] = dMax (dFloat (-4.0f), relVelocErr + penetrationVeloc) * constraintParams->m_timestepInv;
+*/
+	}
+}
+
+void dComplementaritySolver::dFrictionLessContactJoint::JointAccelerations (dJointAccelerationDecriptor* const params)
+{
+	dJacobianPair* const rowMatrix = params->m_rowMatrix;
+	dJacobianColum* const jacobianColElements = params->m_colMatrix;
+
+	const dVector& bodyVeloc0 = m_state0->GetVelocity();
+	const dVector& bodyOmega0 = m_state0->GetOmega();
+	const dVector& bodyVeloc1 = m_state1->GetVelocity();
+	const dVector& bodyOmega1 = m_state1->GetOmega();
+
+	int count = params->m_rowsCount;
+
+	dAssert (params->m_timeStep > dFloat (0.0f));
+	for (int k = 0; k < count; k ++) {
+		const dJacobianPair& Jt = rowMatrix[k];
+		dJacobianColum& element = jacobianColElements[k];
+
+		dVector relVeloc (Jt.m_jacobian_J01.m_linear * bodyVeloc0 + Jt.m_jacobian_J01.m_angular * bodyOmega0 + Jt.m_jacobian_J10.m_linear * bodyVeloc1 + Jt.m_jacobian_J10.m_angular * bodyOmega1);
+
+		dFloat vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
+		dFloat aRel = element.m_deltaAccel;
+		//dFloat restitution = (vRel <= 0.0f) ? 1.05f : 1.0f;
+		dFloat restitution = (vRel <= 0.0f) ? (dFloat (1.0f) + m_restitution) : dFloat(1.0f);
+		
+		vRel *= restitution;
+		vRel = dMin (dFloat (4.0f), vRel);
+		element.m_coordenateAccel = (aRel - vRel * params->m_invTimeStep);
+	}
+}
+
+int dComplementaritySolver::BuildJacobianMatrix (int jointCount, dBilateralJoint** const jointArray, dFloat timestep, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray, int maxRowCount)
+{
+	int rowCount = 0;
+
+	dParamInfo constraintParams;
+	constraintParams.m_timestep = timestep;
+	constraintParams.m_timestepInv = 1.0f / timestep;
+
+	// calculate Jacobian derivative for each active joint	
+	for (int j = 0; j < jointCount; j ++) {
+		dBilateralJoint* const joint = jointArray[j];
+		constraintParams.m_count = 0;
+		joint->JacobianDerivative (&constraintParams); 
+
+		int dofCount = constraintParams.m_count;
+		joint->m_count = dofCount;
+		joint->m_start = rowCount;
+
+		// complete the derivative matrix for this joint
+		int index = joint->m_start;
+		dBodyState* const state0 = joint->m_state0;
+		dBodyState* const state1 = joint->m_state1;
+
+		const dMatrix& invInertia0 = state0->m_invInertia;
+		const dMatrix& invInertia1 = state1->m_invInertia;
+
+		dFloat invMass0 = state0->m_invMass;
+		dFloat invMass1 = state1->m_invMass;
+		dFloat weight = 0.9f;
+
+		for (int i = 0; i < dofCount; i ++) {
+			dJacobianPair* const row = &jacobianArray[index];
+			dJacobianColum* const col = &jacobianColumnArray[index];
+			jacobianArray[rowCount] = constraintParams.m_jacobians[i]; 
+
+			dVector JMinvIM0linear (row->m_jacobian_J01.m_linear.Scale (invMass0));
+			dVector JMinvIM1linear (row->m_jacobian_J10.m_linear.Scale (invMass1));
+			dVector JMinvIM0angular = invInertia0.UnrotateVector(row->m_jacobian_J01.m_angular);
+			dVector JMinvIM1angular = invInertia1.UnrotateVector(row->m_jacobian_J10.m_angular);
+
+			dVector tmpDiag (JMinvIM0linear * row->m_jacobian_J01.m_linear + JMinvIM0angular * row->m_jacobian_J01.m_angular + JMinvIM1linear * row->m_jacobian_J10.m_linear + JMinvIM1angular * row->m_jacobian_J10.m_angular);
+			dVector tmpAccel (JMinvIM0linear * state0->m_externalForce + JMinvIM0angular * state0->m_externalTorque + JMinvIM1linear * state1->m_externalForce + JMinvIM1angular * state1->m_externalTorque);
+			dFloat extenalAcceleration = -(tmpAccel[0] + tmpAccel[1] + tmpAccel[2]);
+
+			col->m_diagDamp = 1.0f;
+			col->m_coordenateAccel = constraintParams.m_jointAccel[i];
+			col->m_normalIndex = constraintParams.m_normalIndex[i];
+			col->m_frictionCallback = constraintParams.m_frictionCallback[i];
+			col->m_jointLowFriction = constraintParams.m_jointLowFrictionCoef[i];
+			col->m_jointHighFriction = constraintParams.m_jointHighFrictionCoef[i];
+
+			col->m_deltaAccel = extenalAcceleration;
+			col->m_coordenateAccel += extenalAcceleration;
+
+			col->m_force = joint->m_jointFeebackForce[i] * weight;
+
+			dFloat stiffness = COMPLEMENTARITY_PSD_DAMP_TOL * col->m_diagDamp;
+			dFloat diag = (tmpDiag[0] + tmpDiag[1] + tmpDiag[2]);
+			dAssert (diag > dFloat (0.0f));
+			col->m_diagDamp = diag * stiffness;
+
+			diag *= (dFloat(1.0f) + stiffness);
+			col->m_invDJMinvJt = dFloat(1.0f) / diag;
+			index ++;
+			rowCount ++;
+			dAssert (rowCount < maxRowCount);
+		}
+	}
+	return rowCount;
+}
+
+void dComplementaritySolver::CalculateReactionsForces (int bodyCount, dBodyState** const bodyArray, int jointCount, dBilateralJoint** const jointArray, dFloat timestepSrc, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray)
+{
+	dJacobian stateVeloc[COMPLEMENTARITY_STACK_ENTRIES];
+	dJacobian internalForces [COMPLEMENTARITY_STACK_ENTRIES];
+
+	int stateIndex = 0;
+	dVector zero(dFloat (0.0f));
+	for (int i = 0; i < bodyCount; i ++) {
+		dBodyState* const state = bodyArray[i];
+		stateVeloc[stateIndex].m_linear = state->m_veloc;
+		stateVeloc[stateIndex].m_angular = state->m_omega;
+
+		internalForces[stateIndex].m_linear = zero;
+		internalForces[stateIndex].m_angular = zero;
+
+		state->m_myIndex = stateIndex;
+		stateIndex ++;
+		dAssert (stateIndex < int (sizeof (stateVeloc)/sizeof (stateVeloc[0])));
+	}
+
+	for (int i = 0; i < jointCount; i ++) {
+		dJacobian y0;
+		dJacobian y1;
+		y0.m_linear = zero;
+		y0.m_angular = zero;
+		y1.m_linear = zero;
+		y1.m_angular = zero;
+		dBilateralJoint* const constraint = jointArray[i];
+		int first = constraint->m_start;
+		int count = constraint->m_count;
+		for (int j = 0; j < count; j ++) { 
+			dJacobianPair* const row = &jacobianArray[j + first];
+			const dJacobianColum* const col = &jacobianColumnArray[j + first];
+			dFloat val = col->m_force; 
+			y0.m_linear += row->m_jacobian_J01.m_linear.Scale(val);
+			y0.m_angular += row->m_jacobian_J01.m_angular.Scale(val);
+			y1.m_linear += row->m_jacobian_J10.m_linear.Scale(val);
+			y1.m_angular += row->m_jacobian_J10.m_angular.Scale(val);
+		}
+		int m0 = constraint->m_state0->m_myIndex;
+		int m1 = constraint->m_state1->m_myIndex;
+		internalForces[m0].m_linear += y0.m_linear;
+		internalForces[m0].m_angular += y0.m_angular;
+		internalForces[m1].m_linear += y1.m_linear;
+		internalForces[m1].m_angular += y1.m_angular;
+	}
+
+
+	dFloat invTimestepSrc = dFloat (1.0f) / timestepSrc;
+	dFloat invStep = dFloat (0.25f);
+	dFloat timestep = timestepSrc * invStep;
+	dFloat invTimestep = invTimestepSrc * dFloat (4.0f);
+
+	int maxPasses = 5;
+	dFloat firstPassCoef = dFloat (0.0f);
+	dFloat maxAccNorm = dFloat (1.0e-2f);
+
+	for (int step = 0; step < 4; step ++) {
+		dJointAccelerationDecriptor joindDesc;
+		joindDesc.m_timeStep = timestep;
+		joindDesc.m_invTimeStep = invTimestep;
+		joindDesc.m_firstPassCoefFlag = firstPassCoef;
+
+		for (int i = 0; i < jointCount; i ++) {
+			dBilateralJoint* const constraint = jointArray[i];
+			joindDesc.m_rowsCount = constraint->m_count;
+			joindDesc.m_rowMatrix = &jacobianArray[constraint->m_start];
+			joindDesc.m_colMatrix = &jacobianColumnArray[constraint->m_start];
+			constraint->JointAccelerations (&joindDesc);
+		}
+		firstPassCoef = dFloat (1.0f);
+
+		dFloat accNorm = dFloat (1.0e10f);
+		for (int passes = 0; (passes < maxPasses) && (accNorm > maxAccNorm); passes ++) {
+			accNorm = dFloat (0.0f);
+			for (int i = 0; i < jointCount; i ++) {
+
+				dBilateralJoint* const constraint = jointArray[i];
+				int index = constraint->m_start;
+				int rowsCount = constraint->m_count;
+				int m0 = constraint->m_state0->m_myIndex;
+				int m1 = constraint->m_state1->m_myIndex;
+
+				dVector linearM0 (internalForces[m0].m_linear);
+				dVector angularM0 (internalForces[m0].m_angular);
+				dVector linearM1 (internalForces[m1].m_linear);
+				dVector angularM1 (internalForces[m1].m_angular);
+
+				dBodyState* const state0 = constraint->m_state0;
+				dBodyState* const state1 = constraint->m_state1;
+				const dMatrix& invInertia0 = state0->m_invInertia;
+				const dMatrix& invInertia1 = state1->m_invInertia;
+				dFloat invMass0 = state0->m_invMass;
+				dFloat invMass1 = state1->m_invMass;
+
+				for (int k = 0; k < rowsCount; k ++) {
+					dJacobianPair* const row = &jacobianArray[index];
+					dJacobianColum* const col = &jacobianColumnArray[index];
+
+					dVector JMinvIM0linear (row->m_jacobian_J01.m_linear.Scale (invMass0));
+					dVector JMinvIM1linear (row->m_jacobian_J10.m_linear.Scale (invMass1));
+					dVector JMinvIM0angular = invInertia0.UnrotateVector(row->m_jacobian_J01.m_angular);
+					dVector JMinvIM1angular = invInertia1.UnrotateVector(row->m_jacobian_J10.m_angular);
+					dVector acc (JMinvIM0linear * linearM0 + JMinvIM0angular * angularM0 + JMinvIM1linear * linearM1 + JMinvIM1angular * angularM1);
+
+					dFloat a = col->m_coordenateAccel - acc.m_x - acc.m_y - acc.m_z - col->m_force * col->m_diagDamp;
+					dFloat f = col->m_force + col->m_invDJMinvJt * a;
+
+					dFloat lowerFrictionForce = col->m_jointLowFriction;
+					dFloat upperFrictionForce = col->m_jointHighFriction;
+
+					if (f > upperFrictionForce) {
+						a = dFloat (0.0f);
+						f = upperFrictionForce;
+					} else if (f < lowerFrictionForce) {
+						a = dFloat (0.0f);
+						f = lowerFrictionForce;
+					}
+
+					accNorm = dMax (accNorm, dAbs (a));
+					dFloat prevValue = f - col->m_force;
+					col->m_force = f;
+
+					linearM0 += row->m_jacobian_J01.m_linear.Scale (prevValue);
+					angularM0 += row->m_jacobian_J01.m_angular.Scale (prevValue);
+					linearM1 += row->m_jacobian_J10.m_linear.Scale (prevValue);
+					angularM1 += row->m_jacobian_J10.m_angular.Scale (prevValue);
+					index ++;
+				}
+				internalForces[m0].m_linear = linearM0;
+				internalForces[m0].m_angular = angularM0;
+				internalForces[m1].m_linear = linearM1;
+				internalForces[m1].m_angular = angularM1;
+			}
+		}
+
+		for (int i = 0; i < bodyCount; i ++) {
+			dBodyState* const state = bodyArray[i];
+			//int index = state->m_myIndex;
+			dAssert (state->m_myIndex == i);
+			dVector force (state->m_externalForce + internalForces[i].m_linear);
+			dVector torque (state->m_externalTorque + internalForces[i].m_angular);
+			state->IntegrateForce(timestep, force, torque);
+		}
+	}
+
+	for (int i = 0; i < jointCount; i ++) {
+		dBilateralJoint* const constraint = jointArray[i];
+		int first = constraint->m_start;
+		int count = constraint->m_count;
+		for (int j = 0; j < count; j ++) { 
+			const dJacobianColum* const col = &jacobianColumnArray[j + first];
+			dFloat val = col->m_force; 
+			constraint->m_jointFeebackForce[j] = val;
+		}
+	}
+
+	for (int i = 0; i < jointCount; i ++) {
+		dBilateralJoint* const constraint = jointArray[i];
+		constraint->UpdateSolverForces (jacobianArray);
+	}
+
+	for (int i = 0; i < bodyCount; i ++) {
+		dBodyState* const state = bodyArray[i];
+		dAssert (state->m_myIndex == i);
+		state->ApplyNetForceAndTorque (invTimestepSrc, stateVeloc[i].m_linear, stateVeloc[i].m_angular);
+	}
+}
+

thirdparty/src/newton/dMath/dLinearAlgebra.h

@@ -0,0 +1,294 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dStdAfxMath.h"
+#include "dMathDefines.h"
+#include "dVector.h"
+#include "dMatrix.h"
+#include "dQuaternion.h"
+
+
+#ifndef __D_LINEAR_ALGEBRA_H__
+#define __D_LINEAR_ALGEBRA_H__
+
+#ifdef _MSC_VER
+	#pragma warning (disable: 4100) //unreferenced formal parameter
+#endif
+
+
+#define D_MAX_PRAM_INFO_SIZE		16 
+#define D_MAX_PLACEMENT_CONTACTS	128
+#define D_COMPLEMENTARITY_MAX_FRICTION_BOUND	dFloat(1.0e15f)
+#define D_COMPLEMENTARITY_MIN_FRICTION_BOUND	(-D_COMPLEMENTARITY_MAX_FRICTION_BOUND)
+
+
+class dSymmetricBiconjugateGradientSolve
+{
+	public:
+	dSymmetricBiconjugateGradientSolve () {}
+	virtual ~dSymmetricBiconjugateGradientSolve () {}	
+
+	virtual dFloat64 Solve (int size, dFloat64 tolerance, dFloat64* const x, const dFloat64* const b) const;
+
+	protected:
+	virtual void MatrixTimeVector (dFloat64* const out, const dFloat64* const v) const = 0;
+	virtual bool InversePrecoditionerTimeVector (dFloat64* const out, const dFloat64* const v) const = 0;
+
+	private:
+	dFloat64 DotProduct (int size, const dFloat64* const b, const dFloat64* const c) const;
+	void ScaleAdd (int size, dFloat64* const a, const dFloat64* const b, dFloat64 scale, const dFloat64* const c) const;
+	void Sub (int size, dFloat64* const a, const dFloat64* const b, const dFloat64* const c) const;
+};
+
+class dComplementaritySolver 
+{
+	public:
+	class dBodyState;
+	class dBilateralJoint;
+
+	class dContact
+	{
+		public:
+		dContact()
+			:m_point(0.0f)
+			,m_normal(0.0f)
+		{
+		}
+
+		dVector m_point;
+		dVector m_normal;
+	};
+
+	class dJacobian
+	{
+		public:
+		dJacobian ()
+			:m_linear(0.0f)
+			,m_angular(0.0f)
+		{
+		}
+
+		dJacobian (const dVector& linear, const dVector& angular)
+			:m_linear(linear)
+			,m_angular(angular)
+		{
+		}
+
+		dVector m_linear;
+		dVector m_angular;
+	};
+
+	class dJacobianPair
+	{
+		public:
+		dJacobian m_jacobian_J01;
+		dJacobian m_jacobian_J10;
+	};
+
+	class dJacobianColum
+	{
+		public:
+		dFloat m_force;
+		dFloat m_diagDamp;
+		dFloat m_deltaAccel;
+		dFloat m_invDJMinvJt;
+		dFloat m_coordenateAccel;
+		dFloat m_jointLowFriction;
+		dFloat m_jointHighFriction;
+		dBilateralJoint* m_frictionCallback;
+		int m_normalIndex;
+	};
+
+	class dParamInfo
+	{
+		public:
+		dJacobianPair m_jacobians[D_MAX_PRAM_INFO_SIZE];
+		dFloat m_jointAccel[D_MAX_PRAM_INFO_SIZE];
+		dFloat m_jointLowFrictionCoef[D_MAX_PRAM_INFO_SIZE];
+		dFloat m_jointHighFrictionCoef[D_MAX_PRAM_INFO_SIZE];
+		dFloat m_diagonalRegularizer[D_MAX_PRAM_INFO_SIZE];
+		int m_normalIndex[D_MAX_PRAM_INFO_SIZE];
+		dBilateralJoint* m_frictionCallback[D_MAX_PRAM_INFO_SIZE];
+		dFloat m_timestep;
+		dFloat m_timestepInv;
+		int m_count;
+	};
+
+	class dJointAccelerationDecriptor
+	{
+		public:
+		int m_rowsCount;
+		dFloat m_timeStep;
+		dFloat m_invTimeStep;
+		dFloat m_firstPassCoefFlag;
+		dJacobianPair* m_rowMatrix;
+		dJacobianColum* m_colMatrix;
+	};
+
+	class dBilateralJoint
+	{
+		public:
+		dBilateralJoint()
+			:m_state0(NULL)
+			,m_state1(NULL)
+			,m_ordinals(0x050403020100ll)
+			,m_start(0)
+			,m_count(0)
+			,m_dof(0)
+		{
+			memset(m_rowIsMotor, 0, sizeof (m_rowIsMotor));
+			memset(m_motorAcceleration, 0, sizeof (m_motorAcceleration));
+			memset(m_jointFeebackForce, 0, sizeof (m_jointFeebackForce));
+		}
+
+		virtual ~dBilateralJoint(){}
+
+		virtual void Init (dBodyState* const state0, dBodyState* const state1);
+		virtual void JacobianDerivative (dParamInfo* const constraintParams) = 0; 
+		virtual void UpdateSolverForces (const dJacobianPair* const jacobians) const = 0; 
+		virtual void JointAccelerations (dJointAccelerationDecriptor* const accelParam);
+		virtual void SpecialSolverFrictionCallback(const dFloat* const force, dFloat* const lowFriction, dFloat* const highFriction) const {}
+
+		void AddAngularRowJacobian (dParamInfo* const constraintParams, const dVector& dir, dFloat jointAngle);
+		void AddLinearRowJacobian (dParamInfo* const constraintParams, const dVector& pivot, const dVector& dir);
+		void AddContactRowJacobian (dParamInfo* const constraintParams, const dVector& pivot, const dVector& dir, dFloat restitution);
+
+		dFloat GetRowAccelaration(dParamInfo* const constraintParams) const;
+		void SetRowAccelaration(dParamInfo* const constraintParams, dFloat accel);
+
+		dFloat CalculateMassMatrixDiagonal(dParamInfo* const constraintParams) const;
+		dFloat CalculateAngle (const dVector& planeDir, const dVector& cosDir, const dVector& sinDir) const;
+
+		dFloat m_motorAcceleration[D_MAX_PRAM_INFO_SIZE];
+		dFloat m_jointFeebackForce[D_MAX_PRAM_INFO_SIZE];
+		int m_rowIsMotor[D_MAX_PRAM_INFO_SIZE];
+		dBodyState* m_state0;
+		dBodyState* m_state1;
+		union
+		{
+			long long m_ordinals;
+			char m_sourceJacobianIndex[8];
+		};
+		int m_start;
+		int m_count;
+		int m_dof;
+
+		friend class dBodyState;
+		friend class dComplementaritySolver;
+	};
+
+	class dFrictionLessContactJoint: public dBilateralJoint
+	{
+		public: 
+		dFrictionLessContactJoint()
+			:dBilateralJoint()
+			,m_restitution(0.0f)
+			,m_count (0)
+		{}
+		virtual ~dFrictionLessContactJoint(){}
+
+		void SetContacts (int count, dContact* const contacts, dFloat restitution);
+
+		protected:
+		void UpdateSolverForces (const dJacobianPair* const jacobians) const {}
+
+		static inline int CompareContact (const dContact* const contactA, const dContact* const contactB, void* dommy);
+		int ReduceContacts (int count, dContact* const contacts, dFloat tol);
+		void JacobianDerivative (dParamInfo* const constraintParams);
+		void JointAccelerations (dJointAccelerationDecriptor* const params);
+
+		dContact m_contacts[D_MAX_PRAM_INFO_SIZE];
+		dFloat m_restitution;
+		int m_count;
+	};
+
+	class dBodyState
+	{
+		public:
+		dBodyState();
+		virtual ~dBodyState() {}
+
+		dFloat GetMass () const;
+		void SetMass (dFloat mass);
+
+		dFloat GetInvMass () const;
+
+		void SetInertia (dFloat Ixx, dFloat Iyy, dFloat Izz);
+		void GetInertia (dFloat& Ixx, dFloat& Iyy, dFloat& Izz) const;
+
+		const dMatrix& GetInertia() const; 
+		const dMatrix& GetInvInertia() const; 
+
+		void SetVeloc (const dVector& veloc);
+		void SetOmega (const dVector& omega);
+		const dVector& GetOmega() const; 
+		const dVector& GetVelocity() const; 
+		dVector CalculatePointVelocity (const dVector& point) const;
+
+		void UpdateInertia();
+
+		void SetMatrix (const dMatrix& matrix);
+		const dMatrix& GetMatrix () const;
+
+		void SetLocalMatrix (const dMatrix& matrix);
+		const dMatrix& GetLocalMatrix () const;
+
+		void SetForce (const dVector& force);
+		void SetTorque (const dVector& torque);
+		const dVector& GetForce () const;
+		const dVector& GetTorque () const;
+
+		const dVector& GetCOM () const;
+
+		void IntegrateVelocity (dFloat timestep);
+		void IntegrateForce (dFloat timestep, const dVector& force, const dVector& torque);
+
+		protected:
+		virtual void ApplyNetForceAndTorque (dFloat invTimestep, const dVector& veloc, const dVector& omega);
+
+		dMatrix m_matrix;
+		dMatrix m_localFrame;
+		dMatrix m_inertia;
+		dMatrix m_invInertia;
+
+		dVector m_localInertia;
+		dVector m_localInvInertia;
+
+		dVector m_veloc;
+		dVector m_omega;
+		dVector m_externalForce;
+		dVector m_externalTorque;
+		dVector m_globalCentreOfMass;
+
+		dFloat m_mass;
+		dFloat m_invMass;
+		int m_myIndex;
+
+		friend class dBilateralJoint;
+		friend class dComplementaritySolver;
+	};
+
+	public:
+	dComplementaritySolver() {};
+	virtual ~dComplementaritySolver() {};
+
+	virtual int GetActiveJoints (dBilateralJoint** const jointArray, int bufferSize)
+	{
+		return 0;
+	}
+
+	virtual int BuildJacobianMatrix (int jointCount, dBilateralJoint** const jointArray, dFloat timestep, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray, int maxRowCount);
+	virtual void CalculateReactionsForces (int bodyCount, dBodyState** const bodyArray, int jointCount, dBilateralJoint** const jointArray, dFloat timestep, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray);
+};
+
+
+#endif
+

thirdparty/src/newton/dMath/dMathDefines.cpp

@@ -0,0 +1,15 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dStdAfxMath.h"
+#include "dMathDefines.h"
+
+

thirdparty/src/newton/dMath/dMathDefines.h

@@ -0,0 +1,925 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+
+#ifndef __dMathDefined__
+#define __dMathDefined__
+
+#include <math.h>
+#include <float.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+
+#ifdef _MSC_VER
+	#include <malloc.h>
+	#include <windows.h>
+	#ifdef _DEBUG
+		#include <stdarg.h>
+		inline void dExpandTraceMessage (const char* const fmt, ...)
+		{
+			va_list v_args;
+			char text[4096];
+
+			text[0] = 0;
+			va_start (v_args, fmt);     
+			vsprintf_s(text, fmt, v_args);
+			va_end (v_args);            
+
+			OutputDebugStringA (text);
+		}
+
+		#define dTrace(x)										\
+		{														\
+			dExpandTraceMessage x;								\
+		}																	
+	#else
+		#define dTrace(x)
+	#endif
+#else
+	#define dTrace(x)
+#endif
+
+#if ( defined (_MSC_VER) || defined (__MINGW32__) || defined (__MINGW64__) )
+	#include <crtdbg.h>
+	#define dAssert(x) _ASSERTE(x)
+#else 
+	#define dAssert(x) assert(x)
+#endif
+
+
+
+#ifndef dFloat
+	#ifdef _NEWTON_USE_DOUBLE
+		typedef double dFloat;
+	#else 
+		typedef float dFloat;
+	#endif
+#endif 
+
+#ifndef dFloat64
+	typedef double dFloat64;
+#endif 
+
+// some constants
+#define	dPi			  dFloat (3.141592f)
+#define	dRadToDegree (dFloat (180.0f) / dPi)
+#define	dDegreeToRad (dFloat (1.0f) / dRadToDegree)
+
+// transcendental functions
+#define	dSqrt(x)	dFloat (sqrt (dFloat(x))) 
+#define	dCiel(x)	dFloat (ceil (dFloat(x))) 
+#define	dFloor(x)	dFloat (floor (dFloat(x))) 
+#define	dLog(x)		dFloat (log (dFloat(x))) 
+#define	dPow(x,y)	dFloat (pow (dFloat(x), dFloat(y))) 
+
+#define dSin(x)		dFloat (sin (dFloat(x)))
+#define dCos(x)		dFloat (cos (dFloat(x)))
+#define dTan(x)		dFloat (tan (dFloat(x)))
+#define dAsin(x)	dFloat (asin (dFloat(x)))
+#define dAcos(x)	dFloat (acos (dFloat(x)))
+#define	dAtan(x)	dFloat (atan (dFloat(x)))
+#define	dAtan2(x,y) dFloat (atan2 (dFloat(x), dFloat(y)))
+
+
+
+#ifdef D_PROFILER
+	#include <dProfiler.h>
+
+	#define D_TRACKTIME() dProfilerZoneScoped(__FUNCTION__)
+	#define D_SET_TRACK_NAME(trackName) dProfilerSetTrackName(trackName)
+#else
+	#define D_TRACKTIME() 
+	#define D_SET_TRACK_NAME(trackName)
+#endif
+
+
+#define	D_MSC_VECTOR_ALIGNMENT
+
+enum dEulerAngleOrder
+{
+	m_pitchYawRoll,
+	m_pitchRollYaw,
+	PYR = (0 << 8) + (1 << 4) + (2 << 0),
+	PRY = (0 << 8) + (2 << 4) + (1 << 0),
+	YPR = (1 << 8) + (0 << 4) + (2 << 0),
+	YRP = (1 << 8) + (2 << 4) + (0 << 0),
+	RYP = (2 << 8) + (1 << 4) + (0 << 0),
+	RPY = (2 << 8) + (0 << 4) + (1 << 0),
+};
+
+#define dAlloca(type,size) (type*) alloca ((size) * sizeof (type))
+
+template <class T>
+T dAbs(T A)
+{
+	// this is far faster than the standard function (does not mess with cpu rounding mode)
+	return (A >= T(0.0f)) ? A : -A;
+}
+
+template <class T>
+T dSign (T A)
+{
+	return (A >= T(0)) ? T(1) : T(-1);
+}
+
+template <class T> 
+void dSwap(T& A, T& B)
+{
+	T tmp (A);
+	A = B;
+	B = tmp;
+}	
+
+template <class T>
+T dMax(T A, T B)
+{
+	return (A > B) ? A : B; 
+}
+
+template <class T>
+T dMin(T A, T B)
+{
+	return (A < B) ? A : B; 
+}
+
+template <class T>
+T dClamp(T val, T min, T max)
+{
+	return dMax (min, dMin (max, val));
+}
+
+template <class T>
+T dMod(T val, T mod)
+{
+	return T (fmod (T(val), T(mod)));
+}
+
+template <class T> 
+void dSort (T* const array, int elements, int (*compare) (const T* const  A, const T* const B, void* const context), void* const context = NULL)
+{
+	int stride = 8;
+	int stack[1024][2];
+
+	stack[0][0] = 0;
+	stack[0][1] = elements - 1;
+	int stackIndex = 1;
+	while (stackIndex) {
+		stackIndex --;
+		int lo = stack[stackIndex][0];
+		int hi = stack[stackIndex][1];
+		if ((hi - lo) > stride) {
+			int i = lo;
+			int j = hi;
+			T val (array[(lo + hi) >> 1]);
+			do {    
+				while (compare (&array[i], &val, context) < 0) i ++;
+				while (compare (&array[j], &val, context) > 0) j --;
+
+				if (i <= j)	{
+					dSwap(array[i], array[j]);
+					i++; 
+					j--;
+				}
+			} while (i <= j);
+
+			if (i < hi) {
+				stack[stackIndex][0] = i;
+				stack[stackIndex][1] = hi;
+				stackIndex ++;
+			}
+			if (lo < j) {
+				stack[stackIndex][0] = lo;
+				stack[stackIndex][1] = j;
+				stackIndex ++;
+			}
+			dAssert (stackIndex < int (sizeof (stack) / (2 * sizeof (stack[0][0]))));
+		}
+	}
+
+	stride = stride * 2;
+	if (elements < stride) {
+		stride = elements;
+	}
+	for (int i = 1; i < stride; i ++) {
+		if (compare (&array[0], &array[i], context) > 0) {
+			dSwap(array[0], array[i]);
+		}
+	}
+
+	for (int i = 1; i < elements; i ++) {
+		int j = i;
+		T tmp (array[i]);
+		for (; compare (&array[j - 1], &tmp, context) > 0; j --) {
+			dAssert (j > 0);
+			array[j] = array[j - 1];
+		}
+		array[j] = tmp;
+	}
+
+#ifdef _DEBUG
+	for (int i = 0; i < (elements - 1); i ++) {
+		dAssert (compare (&array[i], &array[i + 1], context) <= 0);
+	}
+#endif
+}
+
+// return dot product
+template<class T>
+T dDotProduct(int size, const T* const A, const T* const B)
+{
+	T val(0.0f);
+	for (int i = 0; i < size; i++) {
+		val = val + A[i] * B[i];
+	}
+	return val;
+}
+
+template<class T>
+void dMatrixTimeVector(int size, const T* const matrix, const T* const v, T* const out)
+{
+	int stride = 0;
+	for (int i = 0; i < size; i++) {
+		out[i] = dDotProduct(size, &matrix[stride], v);
+		stride += size;
+	}
+}
+
+template<class T>
+bool dSolveGaussian(int size, T* const matrix, T* const b)
+{
+	for (int i = 0; i < size - 1; i++) {
+		const T* const rowI = &matrix[i * size];
+		int m = i;
+		T maxVal(dAbs(rowI[i]));
+		for (int j = i + 1; j < size - 1; j++) {
+			T val(dAbs(matrix[size * j + i]));
+			if (val > maxVal) {
+				m = j;
+				maxVal = val;
+			}
+		}
+
+		if (maxVal < T(1.0e-12f)) {
+			return false;
+		}
+
+		if (m != i) {
+			T* const rowK = &matrix[m * size];
+			T* const rowJ = &matrix[i * size];
+			for (int j = 0; j < size; j++) {
+				dSwap(rowK[j], rowJ[j]);
+			}
+			dSwap(b[i], b[m]);
+		}
+
+		T den = T(1.0f) / rowI[i];
+		for (int k = i + 1; k < size; k++) {
+			T* const rowK = &matrix[size * k];
+			T factor(-rowK[i] * den);
+			for (int j = i + 1; j < size; j++) {
+				rowK[j] += rowI[j] * factor;
+			}
+			rowK[i] = T(0.0f);
+			b[k] += b[i] * factor;
+		}
+	}
+
+	for (int i = size - 1; i >= 0; i--) {
+		T acc(0);
+		T* const rowI = &matrix[i * size];
+		for (int j = i + 1; j < size; j++) {
+			acc = acc + rowI[j] * b[j];
+		}
+		b[i] = (b[i] - acc) / rowI[i];
+	}
+	return true;
+}
+
+
+template<class T>
+void dCholeskySolve(int size, int stride, const T* const choleskyMatrix, T* const x)
+{
+	int rowStart = 0;
+	for (int i = 0; i < size; i++) {
+		T acc(0.0f);
+		const T* const row = &choleskyMatrix[rowStart];
+		for (int j = 0; j < i; j++) {
+			acc = acc + row[j] * x[j];
+		}
+		x[i] = (x[i] - acc) / row[i];
+		rowStart += stride;
+	}
+
+	for (int i = size - 1; i >= 0; i--) {
+		T acc = 0.0f;
+		for (int j = i + 1; j < size; j++) {
+			acc = acc + choleskyMatrix[stride * j + i] * x[j];
+		}
+		x[i] = (x[i] - acc) / choleskyMatrix[stride * i + i];
+	}
+}
+
+template<class T>
+bool dCholeskyFactorization(int size, int stride, T* const matrix)
+{
+	T* const invDiagonal = dAlloca(T, size);
+	for (int i = 0; i < size; i++) {
+		T* const rowN = &matrix[stride * i];
+
+		int rowStart = 0;
+		for (int j = 0; j <= i; j++) {
+			T s(0.0f);
+			T* const rowJ = &matrix[rowStart];
+			for (int k = 0; k < j; k++) {
+				s += rowN[k] * rowJ[k];
+			}
+
+			if (i == j) {
+				T diag = rowN[i] - s;
+				if (diag < T(1.0e-8f)) {
+					return false;
+				}
+				rowN[i] = T(sqrt(diag));
+				invDiagonal[i] = T (1.0f) / rowN[i];
+			} else {
+				//rowN[j] = ((T(1.0f) / rowJ[j]) * (rowN[j] - s));
+				rowN[j] = invDiagonal[j] * (rowN[j] - s);
+			}
+			rowStart += stride;
+		}
+	}
+	return true;
+}
+
+template<class T>
+bool dTestPSDmatrix(int size, int stride, const T* const matrix)
+{
+	T* const copy = dAlloca(T, size * size);
+	int row = 0;
+	for (int i = 0; i < size; i ++)
+	{
+		memcpy (&copy[i * size], &matrix[row], size * sizeof (T));
+		row += stride;
+	}
+	return dCholeskyFactorization(size, size, copy);
+}
+
+template<class T>
+void dPermuteRows(int size, int i, int j, T* const matrix, T* const choleskyMatrix, T* const x, T* const r, T* const low, T* const high, int* const permute)
+{
+	if (i != j) {
+		T* const matrixRowA = &matrix[size * i];
+		T* const matrixRowB = &matrix[size * j];
+		T* const choleskyRowA = &choleskyMatrix[size * i];
+		T* const choleskyRowB = &choleskyMatrix[size * j];
+		for (int k = 0; k < size; k++) {
+			dSwap(matrixRowA[k], matrixRowB[k]);
+			dSwap(choleskyRowA[k], choleskyRowB[k]);
+		}
+
+		int stride = 0;
+		for (int k = 0; k < size; k++) {
+			dSwap(matrix[stride + i], matrix[stride + j]);
+			stride += size;
+		}
+
+		dSwap(x[i], x[j]);
+		dSwap(r[i], r[j]);
+		dSwap(low[i], low[j]);
+		dSwap(high[i], high[j]);
+		dSwap(permute[i], permute[j]);
+	}
+}
+
+template<class T>
+void dCholeskyUpdate(int size, int row, int colum, T* const choleskyMatrix, T* const tmp, T* const reflexion)
+{
+	if (row != colum) {
+		dAssert(row < colum);
+
+		for (int i = row; i < size; i++) {
+			T* const rowI = &choleskyMatrix[size * i];
+			T mag(T(dFloat(0.0f)));
+			for (int j = i + 1; j < size; j++) {
+				mag += rowI[j] * rowI[j];
+				reflexion[j] = rowI[j];
+			}
+			if (mag > T(dFloat(1.0e-9f))) {
+				reflexion[i] = rowI[i] - T(sqrt(mag + rowI[i] * rowI[i]));
+
+				T vMag2(mag + reflexion[i] * reflexion[i]);
+				T den(dFloat(1.0f) / T(sqrt(vMag2)));
+				for (int j = i; j < size; j++) {
+					reflexion[j] *= den;
+				}
+
+				for (int j = i; j < size; j++) {
+					T acc(0.0f);
+					T* const rowJ = &choleskyMatrix[size * j];
+					for (int k = i; k < size; k++) {
+						acc += rowJ[k] * reflexion[k];
+					}
+					tmp[j] = acc * T(dFloat(2.0f));
+				}
+
+				for (int j = i + 1; j < size; j++) {
+					T* const rowJ = &choleskyMatrix[size * j];
+					for (int k = i; k < size; k++) {
+						rowJ[k] -= tmp[j] * reflexion[k];
+					}
+				}
+				rowI[i] -= tmp[i] * reflexion[i];
+			}
+
+			for (int k = i + 1; k < size; k++) {
+				rowI[k] = T(dFloat(0.0f));
+			}
+
+			if (rowI[i] < T(dFloat(0.0f))) {
+				for (int k = i; k < size; k++) {
+					choleskyMatrix[size * k + i] = -choleskyMatrix[size * k + i];
+				}
+			}
+		}
+		for (int i = row; i < size; i++) {
+			choleskyMatrix[size * i + i] = dMax(choleskyMatrix[size * i + i], T(dFloat(1.0e-6f)));
+		}
+	}
+}
+
+template<class T>
+void dCalculateDelta_x(int size, T dir, int n, const T* const matrix, const T* const choleskyMatrix, T* const delta_x)
+{
+	const T* const row = &matrix[size * n];
+	for (int i = 0; i < n; i++) {
+		delta_x[i] = -row[i] * dir;
+	}
+	dCholeskySolve(size, n, choleskyMatrix, delta_x);
+	delta_x[n] = dir;
+}
+
+// calculate delta_r = A * delta_x
+template<class T>
+void dCalculateDelta_r(int size, int n, const T* const matrix, const T* const delta_x, T* const delta_r)
+{
+	int stride = n * size;
+	for (int i = n; i < size; i++) {
+		delta_r[i] = dDotProduct(size, &matrix[stride], delta_x);
+		stride += size;
+	}
+}
+
+
+// solve a general Linear complementary program (LCP)
+// A * x = b + r
+// subjected to constraints
+// x(i) = low(i),  if r(i) >= 0  
+// x(i) = high(i), if r(i) <= 0  
+// low(i) <= x(i) <= high(i),  if r(i) == 0  
+//
+// return true is the system has a solution.
+// in return 
+// x is the solution,
+// r is return in vector b
+// note: although the system is called LCP, the solve is far more general than a strict LCP
+// to solve a strict LCP set the following
+// low(i) = 0
+// high(i) = infinity.
+// this the same as enforcing the constrain: x(i) * r(i) = 0
+template <class T>
+bool dSolveDantzigLCP(int size, T* const matrix, T* const x, T* const b, T* const low, T* const high, T regularizer = T(1.e-4f))
+{
+	T* const choleskyMatrix = dAlloca(T, size * size);
+	T* const x0 = dAlloca(T, size);
+	T* const r0 = dAlloca(T, size);
+	T* const delta_r = dAlloca(T, size);
+	T* const delta_x = dAlloca(T, size);
+	T* const tmp0 = dAlloca(T, size);
+	T* const tmp1 = dAlloca(T, size);
+	int* const permute = dAlloca(int, size);
+
+	memcpy(choleskyMatrix, matrix, sizeof(T) * size * size);
+	bool pass = dCholeskyFactorization(size, choleskyMatrix);
+	while (!pass) {
+		int stride = 0;
+		for (int i = 0; i < size; i ++) {
+			matrix[stride] += matrix[stride] * regularizer;
+			stride += size + 1;
+		}
+		memcpy(choleskyMatrix, matrix, sizeof(T) * size * size);
+		pass = dCholeskyFactorization(size, choleskyMatrix);
+	}
+
+	for (int i = 0; i < size; i++) {
+		T* const row = &choleskyMatrix[i * size];
+		for (int j = i + 1; j < size; j++) {
+			row[j] = T(0.0f);
+		}
+	}
+
+	int index = 0;
+	int count = size;
+	int clampedIndex = size;
+	for (int i = 0; i < size; i++) {
+		permute[i] = short(i);
+		r0[i] = -b[i];
+		x0[i] = dFloat(0.0f);
+		delta_x[i] = T(dFloat(0.0f));
+		delta_r[i] = T(dFloat(0.0f));
+	}
+
+	bool findInitialGuess = size >= 6;
+	if (findInitialGuess) {
+		int initialGuessCount = size;
+		for (int j = 0; (j < 5) && findInitialGuess; j++) {
+
+			findInitialGuess = false;
+			for (int i = 0; i < initialGuessCount; i++) {
+				x0[i] = -r0[i];
+			}
+			dCholeskySolve(size, initialGuessCount, choleskyMatrix, x0);
+			int permuteStart = initialGuessCount;
+			for (int i = 0; i < initialGuessCount; i++) {
+				T f = x0[i];
+				if ((f < low[i]) || (f > high[i])) {
+					findInitialGuess = true;
+					x0[i] = T(dFloat(0.0f));
+					dPermuteRows(size, i, initialGuessCount - 1, matrix, choleskyMatrix, x0, r0, low, high, permute);
+					permuteStart = dMin(permuteStart, i);
+					i--;
+					initialGuessCount--;
+				}
+			}
+			if (findInitialGuess) {
+				dCholeskyUpdate(size, permuteStart, size - 1, choleskyMatrix, tmp0, tmp1);
+			}
+		}
+
+		if (initialGuessCount == size) {
+			for (int i = 0; i < size; i++) {
+				x[i] = x0[i];
+				b[i] = T(dFloat(0.0f));
+			}
+			return true;
+		} else {
+			if (!findInitialGuess) {
+				for (int i = 0; i < initialGuessCount; i++) {
+					r0[i] = T(dFloat(0.0f));
+				}
+				for (int i = initialGuessCount; i < size; i++) {
+					r0[i] += dDotProduct(size, &matrix[i * size], x0);
+				}
+				index = initialGuessCount;
+				count = size - initialGuessCount;
+			} else {
+				//dAssert(0);
+				for (int i = 0; i < size; i++) {
+					x0[i] = dFloat(0.0f);
+				}
+			}
+		}
+	}
+
+	while (count) {
+		bool loop = true;
+		bool calculateDelta_x = true;
+
+		while (loop) {
+			loop = false;
+			T clamp_x(0.0f);
+			int swapIndex = -1;
+
+			if (dAbs(r0[index]) > T(1.0e-12f)) {
+				if (calculateDelta_x) {
+					T dir(dFloat(1.0f));
+					dCalculateDelta_x(size, dir, index, matrix, choleskyMatrix, delta_x);
+				}
+
+				calculateDelta_x = true;
+				dCalculateDelta_r(size, index, matrix, delta_x, delta_r);
+				dAssert(delta_r[index] != T(dFloat(0.0f)));
+				dAssert(dAbs(delta_x[index]) == T(1.0f));
+				delta_r[index] = (delta_r[index] == T(dFloat(0.0f))) ? T(dFloat(1.0e-12f)) : delta_r[index];
+
+				T s = -r0[index] / delta_r[index];
+				dAssert(dAbs(s) >= T(dFloat(0.0f)));
+
+				for (int i = 0; i <= index; i++) {
+					T x1 = x0[i] + s * delta_x[i];
+					if (x1 > high[i]) {
+						swapIndex = i;
+						clamp_x = high[i];
+						s = (high[i] - x0[i]) / delta_x[i];
+					} else if (x1 < low[i]) {
+						swapIndex = i;
+						clamp_x = low[i];
+						s = (low[i] - x0[i]) / delta_x[i];
+					}
+				}
+				dAssert(dAbs(s) >= T(dFloat(0.0f)));
+
+				for (int i = clampedIndex; (i < size) && (s > T(1.0e-12f)); i++) {
+					T r1 = r0[i] + s * delta_r[i];
+					if ((r1 * r0[i]) < T(dFloat(0.0f))) {
+						dAssert(dAbs(delta_r[i]) > T(dFloat(0.0f)));
+						T s1 = -r0[i] / delta_r[i];
+						dAssert(dAbs(s1) >= T(dFloat(0.0f)));
+						dAssert(dAbs(s1) <= dAbs(s));
+						if (dAbs(s1) < dAbs(s)) {
+							s = s1;
+							swapIndex = i;
+						}
+					}
+				}
+
+				for (int i = 0; i < size; i++) {
+					dAssert((x0[i] + dAbs(x0[i]) * T(dFloat(1.0e-4f))) >= low[i]);
+					dAssert((x0[i] - dAbs(x0[i]) * T(dFloat(1.0e-4f))) <= high[i]);
+
+					x0[i] += s * delta_x[i];
+					r0[i] += s * delta_r[i];
+
+					dAssert((x0[i] + dFloat(1.0f)) >= low[i]);
+					dAssert((x0[i] - dFloat(1.0f)) <= high[i]);
+				}
+			}
+
+			if (swapIndex == -1) {
+				r0[index] = T(dFloat(0.0f));
+				delta_r[index] = T(dFloat(0.0f));
+				index++;
+				count--;
+				loop = false;
+			} else if (swapIndex == index) {
+				count--;
+				clampedIndex--;
+				x0[index] = clamp_x;
+				dPermuteRows(size, index, clampedIndex, matrix, choleskyMatrix, x0, r0, low, high, permute);
+				dCholeskyUpdate(size, index, clampedIndex, choleskyMatrix, tmp0, tmp1);
+
+				loop = count ? true : false;
+			} else if (swapIndex > index) {
+				loop = true;
+				r0[swapIndex] = T(dFloat(0.0f));
+				dAssert(swapIndex < size);
+				dAssert(clampedIndex <= size);
+				if (swapIndex < clampedIndex) {
+					count--;
+					clampedIndex--;
+					dPermuteRows(size, clampedIndex, swapIndex, matrix, choleskyMatrix, x0, r0, low, high, permute);
+					dCholeskyUpdate(size, swapIndex, clampedIndex, choleskyMatrix, tmp0, tmp1);
+					dAssert(clampedIndex >= index);
+				} else {
+					count++;
+					dAssert(clampedIndex < size);
+					dPermuteRows(size, clampedIndex, swapIndex, matrix, choleskyMatrix, x0, r0, low, high, permute);
+					dCholeskyUpdate(size, clampedIndex, swapIndex, choleskyMatrix, tmp0, tmp1);
+					clampedIndex++;
+					dAssert(clampedIndex <= size);
+					dAssert(clampedIndex >= index);
+				}
+				calculateDelta_x = false;
+			} else {
+				dAssert(index > 0);
+				x0[swapIndex] = clamp_x;
+				delta_x[index] = T(dFloat(0.0f));
+
+				dAssert(swapIndex < index);
+				dPermuteRows(size, swapIndex, index - 1, matrix, choleskyMatrix, x0, r0, low, high, permute);
+				dPermuteRows(size, index - 1, index, matrix, choleskyMatrix, x0, r0, low, high, permute);
+				dPermuteRows(size, clampedIndex - 1, index, matrix, choleskyMatrix, x0, r0, low, high, permute);
+				dCholeskyUpdate(size, swapIndex, clampedIndex - 1, choleskyMatrix, tmp0, tmp1);
+
+				clampedIndex--;
+				index--;
+				loop = true;
+			}
+		}
+	}
+
+	for (int i = 0; i < size; i++) {
+		int j = permute[i];
+		x[j] = x0[i];
+		b[j] = r0[i];
+	}
+	return true;
+}
+
+
+template <class T>
+bool dCholeskyWithRegularizer(int size, int block, T* const matrix, T regularizer)
+{
+	T* const copy = dAlloca(T, size * size);
+	memcpy(copy, matrix, size * size * sizeof (T));
+	bool pass = dCholeskyFactorization(size, block, matrix);
+
+	int count = 0;
+	while (!pass && (count < 10)) {
+		int stride = 0;
+		for (int i = 0; i < block; i++) {
+			copy[stride] += copy[stride] * regularizer;
+			stride += size + 1;
+		}
+		memcpy(matrix, copy, sizeof(T)* size * size);
+		pass = dCholeskyFactorization(size, block, matrix);
+	}
+	return pass;
+}
+
+
+// solve a general Linear complementary program (LCP)
+// A * x = b + r
+// subjected to constraints
+// x(i) = low(i),  if r(i) >= 0  
+// x(i) = high(i), if r(i) <= 0  
+// low(i) <= x(i) <= high(i),  if r(i) == 0  
+//
+// return true is the system has a solution.
+// in return 
+// x is the solution,
+// b is zero
+// note: although the system is called LCP, the solver is far more general than a strict LCP
+// to solve a strict LCP, set the following
+// low(i) = 0
+// high(i) = infinity.
+// this is the same as enforcing the constraint: x(i) * r(i) = 0
+template <class T>
+bool dSolvePartitionDantzigLCP(int size, T* const symmetricMatrixPSD, T* const x, T* const b, T* const low, T* const high, int unboundedSize, T regularizer = T(1.e-4f))
+{
+	bool ret = false;
+	if (unboundedSize > 0) {
+
+		ret = dCholeskyWithRegularizer(size, unboundedSize, symmetricMatrixPSD, regularizer);
+		if (ret) {
+			memcpy (x, b, unboundedSize * sizeof (T));
+			dCholeskySolve(size, unboundedSize, symmetricMatrixPSD, x);
+			int base = unboundedSize * size;
+			for (int i = unboundedSize; i < size; i++) {
+				b[i] -= dDotProduct(unboundedSize, &symmetricMatrixPSD[base], x);
+				base += size;
+			}
+
+			const int boundedSize = size - unboundedSize;
+			T* const l = dAlloca(T, boundedSize);
+			T* const h = dAlloca(T, boundedSize);
+			T* const c = dAlloca(T, boundedSize);
+			T* const u = dAlloca(T, boundedSize);
+			T* const a11 = dAlloca(T, boundedSize * boundedSize);
+			T* const a10 = dAlloca(T, boundedSize * unboundedSize);
+
+			for (int i = 0; i < boundedSize; i++) {
+				T* const g = &a10[i * unboundedSize];
+				const T* const row = &symmetricMatrixPSD[(unboundedSize + i) * size];
+				for (int j = 0; j < unboundedSize; j++) {
+					g[j] = -row[j];
+				}
+				dCholeskySolve(size, unboundedSize, symmetricMatrixPSD, g);
+
+				T* const arow = &a11[i * boundedSize];
+				const T* const row2 = &symmetricMatrixPSD[(unboundedSize + i) * size];
+				arow[i] = row2[unboundedSize + i] + dDotProduct(unboundedSize, g, row2);
+				for (int j = i + 1; j < boundedSize; j++) {
+					const T* const row1 = &symmetricMatrixPSD[(unboundedSize + j) * size];
+					T elem = row1[unboundedSize + i] + dDotProduct(unboundedSize, g, row1);
+					arow[j] = elem;
+					a11[j * boundedSize + i] = elem;
+				}
+				u[i] = T(0.0f);
+				c[i] = b[i + unboundedSize];
+				l[i] = low[i + unboundedSize];
+				h[i] = high[i + unboundedSize];
+			}
+
+			if (dSolveDantzigLCP(boundedSize, a11, u, c, l, h, regularizer)) {
+				for (int i = 0; i < boundedSize; i++) {
+					const T s = u[i];
+					x[unboundedSize + i] = s;
+					const T* const g = &a10[i * unboundedSize];
+					for (int j = 0; j < unboundedSize; j++) {
+						x[j] += g[j] * s;
+					}
+				}
+				ret = true;
+			}
+		}
+	} else {
+		ret = dSolveDantzigLCP(size, symmetricMatrixPSD, x, b, low, high, regularizer);
+	}
+
+	return ret;
+}
+
+
+// solve a general Linear complementary program (LCP)
+// A * x = b + r
+// subjected to constraints
+// x(i) = low(i),  if r(i) >= 0  
+// x(i) = high(i), if r(i) <= 0  
+// low(i) <= x(i) <= high(i),  if r(i) == 0  
+//
+// return true is the system has a solution.
+// in return 
+// x is the solution,
+// r is return in vector b
+// note: although the system is called LCP, the solver is far more general than a strict LCP
+// to solve a strict LCP, set the following
+// low(i) = 0
+// high(i) = infinity.
+// this the same as enforcing the constraint: x(i) * r(i) = 0
+template <class T>
+void dGaussSeidelLcpSor(const int size, const int stride, const T* const matrix, T* const x, const T* const b, const int* const normalIndex, const T* const low, const T* const high, T tol2, int maxIterCount, T sor)
+{
+	const T* const me = matrix;
+	T* const invDiag1 = dAlloca(T, size);
+	T* const u = dAlloca(T, size + 1);
+	int* const index = dAlloca(int, size);
+
+	u[size] = T(1.0f);
+	int rowStart = 0;
+	for (int j = 0; j < size; j++) {
+		u[j] = x[j];
+		index[j] = normalIndex[j] ? j + normalIndex[j] : size;
+	}
+
+	for (int j = 0; j < size; j++) {
+		const T val = u[index[j]];
+		const T l = low[j] * val;
+		const T h = high[j] * val;
+		u[j] = dClamp(u[j], l, h);
+		invDiag1[j] = T(1.0f) / me[rowStart + j];
+		rowStart += stride;
+	}
+
+	T tolerance(tol2 * 2.0f);
+	const T* const invDiag = invDiag1;
+	const int maxCount = dMax (8, size);
+	for (int i = 0; (i < maxCount) && (tolerance > tol2); i++) {
+		int base = 0;
+		tolerance = T(0.0f);
+		for (int j = 0; j < size; j++) {
+			const T* const row = &me[base];
+			T r(b[j] - dDotProduct(size, row, u));
+			T f((r + row[j] * u[j]) * invDiag[j]);
+
+			const T val = u[index[j]];
+			const T l = low[j] * val;
+			const T h = high[j] * val;
+			if (f > h) {
+				u[j] = h;
+			} else if (f < l) {
+				u[j] = l;
+			} else {
+				tolerance += r * r;
+				u[j] = f;
+			}
+			base += stride;
+		}
+	}
+
+#ifdef _DEBUG 
+	int passes = 0;
+#endif
+	for (int i = 0; (i < maxIterCount) && (tolerance > tol2); i++) {
+		int base = 0;
+		tolerance = T(0.0f);
+#ifdef _DEBUG 
+		passes++;
+#endif
+		for (int j = 0; j < size; j++) {
+			const T* const row = &me[base];
+			T r(b[j] - dDotProduct(size, row, u));
+			T f((r + row[j] * u[j]) * invDiag[j]);
+			f = u[j] + (f - u[j]) * sor;
+
+			const T val = u[index[j]];
+			const T l = low[j] * val;
+			const T h = high[j] * val;
+			if (f > h) {
+				u[j] = h;
+			} else if (f < l) {
+				u[j] = l;
+			} else {
+				tolerance += r * r;
+				u[j] = f;
+			}
+			base += stride;
+		}
+	}
+
+	for (int j = 0; j < size; j++) {
+		x[j] = u[j];
+	}
+}
+
+#endif
+

thirdparty/src/newton/dMath/dMatrix.h

@@ -0,0 +1,181 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+
+#ifndef __dMatrix__
+#define __dMatrix__
+
+#include "dVector.h"
+
+class dMatrix;
+class dQuaternion;
+class dQuaternion;
+
+dMatrix dGetZeroMatrix ();
+dMatrix dGetIdentityMatrix();
+
+class dMatrix
+{
+	public:
+	dMatrix ();
+	dMatrix (const dFloat* const array);
+#ifndef _NEWTON_USE_DOUBLE
+	dMatrix (const dFloat64* const array);
+#endif
+	dMatrix (const dVector &front, const dVector &up, const dVector &right, const dVector &posit);
+	dMatrix (const dQuaternion &rotation, const dVector &position);
+	dMatrix (dFloat pitch, dFloat yaw, dFloat roll, const dVector& location);
+
+	dVector& operator[] (int i);
+	const dVector& operator[] (int i) const;
+
+	dMatrix Inverse () const;
+	dMatrix Transpose () const;
+	dMatrix Transpose4X4 () const;
+	
+	dVector RotateVector (const dVector &v) const;
+	dVector UnrotateVector (const dVector &v) const;
+	dVector TransformVector (const dVector &v) const;
+	dVector UntransformVector (const dVector &v) const;
+	dVector TransformPlane (const dVector &localPlane) const;
+	dVector UntransformPlane (const dVector &globalPlane) const;
+    void GetEulerAngles(dVector& euler1, dVector& euler2, dEulerAngleOrder order = m_pitchYawRoll) const;
+
+	bool TestIdentity() const; 
+	bool TestOrthogonal() const; 
+	dMatrix Inverse4x4() const;
+	dVector RotateVector4x4 (const dVector &v) const;
+	dMatrix JacobiDiagonalization (dVector& eigenValues, const dMatrix& initialMatrix = dGetIdentityMatrix()) const;
+
+	// decompose this matrix into [this = transpose(stretchAxis) * matrix(scale) * stretchAxis * transformMatrix];
+	void PolarDecomposition (dMatrix& transformMatrix, dVector& scale, dMatrix& stretchAxis, const dMatrix& initialStretchAxis = dGetIdentityMatrix()) const;
+	
+	// constructor for polar composition
+	dMatrix (const dMatrix& transformMatrix, const dVector& scale, const dMatrix& stretchAxis);
+
+	void TransformTriplex (dFloat* const dst, int dstStrideInBytes, const dFloat* const src, int srcStrideInBytes, int count) const;
+#ifndef _NEWTON_USE_DOUBLE
+	void TransformTriplex (dFloat64* const dst, int dstStrideInBytes, const dFloat64* const src, int srcStrideInBytes, int count) const;
+#endif
+
+	dMatrix operator* (const dMatrix & B) const;
+
+	void Trace () const
+	{
+		dTrace (("(%ff, %ff, %ff, %ff)\n", m_front.m_x, m_front.m_y, m_front.m_z, m_front.m_w));
+		dTrace (("(%ff, %ff, %ff, %ff)\n", m_up.m_x, m_up.m_y, m_up.m_z, m_up.m_w));
+		dTrace (("(%ff, %ff, %ff, %ff)\n", m_right.m_x, m_right.m_y, m_right.m_z, m_right.m_w));
+		dTrace (("(%ff, %ff, %ff, %ff)\n", m_posit.m_x, m_posit.m_y, m_posit.m_z, m_posit.m_w));
+	}
+
+
+	bool SanityCheck() const;
+
+	dVector m_front;
+	dVector m_up;
+	dVector m_right;
+	dVector m_posit;
+};
+
+inline dMatrix::dMatrix ()
+{
+}
+
+inline dMatrix::dMatrix (const dVector &front, const dVector &up, const dVector &right,	const dVector &posit)
+	:m_front (front), m_up(up), m_right(right), m_posit(posit)
+{
+}
+
+inline dMatrix::dMatrix (const dFloat* const array)
+{
+	memcpy (&(*this)[0][0], array, sizeof (dMatrix));
+}
+
+#ifndef _NEWTON_USE_DOUBLE
+inline dMatrix::dMatrix (const dFloat64* const array)
+{
+	dFloat* const ptr = &(*this)[0][0];
+	for (int i = 0; i < 16; i ++) {
+		ptr[i] = dFloat (array[i]);
+	}
+}
+#endif
+
+inline dVector& dMatrix::operator[] (int  i)
+{
+	return (&m_front)[i];
+}
+
+inline const dVector& dMatrix::operator[] (int  i) const
+{
+	return (&m_front)[i];
+}
+
+dMatrix dRollMatrix(dFloat ang);
+dMatrix dYawMatrix(dFloat ang);
+dMatrix dPitchMatrix(dFloat ang);
+dMatrix dGrammSchmidt(const dVector& dir);
+
+
+class dSpatialMatrix
+{
+	public:
+	inline dSpatialMatrix()
+	{
+	}
+
+	inline dSpatialMatrix(dFloat val)
+	{
+		for (int i = 0; i < 6; i++) {
+			m_rows[i] = dSpatialVector(val);
+		}
+	}
+
+	inline dSpatialVector& operator[] (int i)
+	{
+		dAssert(i < 6);
+		dAssert(i >= 0);
+		return m_rows[i];
+	}
+
+	inline const dSpatialVector& operator[] (int i) const
+	{
+		dAssert(i < 6);
+		dAssert(i >= 0);
+		return m_rows[i];
+	}
+
+	dSpatialMatrix Inverse(int rows) const;
+
+	inline dSpatialVector VectorTimeMatrix(const dSpatialVector& jacobian) const
+	{
+		dSpatialVector tmp(m_rows[0].Scale(jacobian[0]));
+		for (int i = 1; i < 6; i++) {
+			tmp = tmp + m_rows[i].Scale(jacobian[i]);
+		}
+		return tmp;
+	}
+
+	inline dSpatialVector VectorTimeMatrix(const dSpatialVector& jacobian, int dof) const
+	{
+		dSpatialVector tmp(0.0f);
+		for (int i = 0; i < dof; i++) {
+			tmp = tmp + m_rows[i].Scale(jacobian[i]);
+		}
+		return tmp;
+	}
+
+	dSpatialVector m_rows[6];
+};
+
+
+#endif
+

thirdparty/src/newton/dMath/dQuaternion.cpp

@@ -0,0 +1,206 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dStdAfxMath.h"
+#include "dMathDefines.h"
+#include "dVector.h"
+#include "dMatrix.h"
+#include "dQuaternion.h"
+
+enum QUAT_INDEX
+{
+	X_INDEX = 0,
+	Y_INDEX = 1,
+	Z_INDEX = 2
+};
+static QUAT_INDEX QIndex[] = { Y_INDEX, Z_INDEX, X_INDEX };
+
+dQuaternion::dQuaternion (const dMatrix &matrix)
+{
+	dFloat trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
+	dAssert (matrix[2].DotProduct3(matrix[0].CrossProduct(matrix[1])) > 0.0f);
+
+	if (trace > dFloat(0.0f)) {
+		trace = dSqrt (trace + dFloat(1.0f));
+		m_w = dFloat (0.5f) * trace;
+		trace = dFloat (0.5f) / trace;
+		m_x = (matrix[1][2] - matrix[2][1]) * trace;
+		m_y = (matrix[2][0] - matrix[0][2]) * trace;
+		m_z = (matrix[0][1] - matrix[1][0]) * trace;
+
+	} else {
+
+		QUAT_INDEX i = X_INDEX;
+		if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
+			i = Y_INDEX;
+		}
+		if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
+			i = Z_INDEX;
+		}
+		QUAT_INDEX j = QIndex [i];
+		QUAT_INDEX k = QIndex [j];
+
+		trace = dFloat(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
+		trace = dSqrt (trace);
+
+		dFloat* const ptr = &m_x;
+		ptr[i] = dFloat (0.5f) * trace;
+		trace = dFloat (0.5f) / trace;
+		m_w = (matrix[j][k] - matrix[k][j]) * trace;
+		ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
+		ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
+	}
+
+#if _DEBUG
+
+	dMatrix tmp (*this, matrix.m_posit);
+	dMatrix unitMatrix (tmp * matrix.Inverse());
+	for (int i = 0; i < 4; i ++) {
+		dFloat err = dAbs (unitMatrix[i][i] - dFloat(1.0f));
+		dAssert (err < dFloat (1.0e-3f));
+	}
+
+	dFloat err = dAbs (DotProduct(*this) - dFloat(1.0f));
+	dAssert (err < dFloat(1.0e-3f));
+#endif
+
+}
+
+dQuaternion::dQuaternion (const dVector &unitAxis, dFloat angle)
+{
+	angle *= dFloat (0.5f);
+	m_w = dCos (angle);
+	dFloat sinAng = dSin (angle);
+
+#ifdef _DEBUG
+	if (dAbs (angle) > dFloat(1.0e-6f)) {
+		dAssert (dAbs (dFloat(1.0f) - unitAxis.DotProduct3(unitAxis)) < dFloat(1.0e-3f));
+	} 
+#endif
+	m_x = unitAxis.m_x * sinAng;
+	m_y = unitAxis.m_y * sinAng;
+	m_z = unitAxis.m_z * sinAng;
+}
+
+dVector dQuaternion::CalcAverageOmega (const dQuaternion &q1, dFloat invdt) const
+{
+	dQuaternion q0 (*this);
+	if (q0.DotProduct (q1) < 0.0f) {
+		q0.Scale(-1.0f);
+	}
+	dQuaternion dq (q0.Inverse() * q1);
+	dVector omegaDir (dq.m_x, dq.m_y, dq.m_z);
+
+	dFloat dirMag2 = omegaDir.DotProduct3(omegaDir);
+	if (dirMag2	< dFloat(dFloat (1.0e-5f) * dFloat (1.0e-5f))) {
+		return dVector (dFloat(0.0f));
+	}
+
+	dFloat dirMagInv = dFloat (1.0f) / dSqrt (dirMag2);
+	dFloat dirMag = dirMag2 * dirMagInv;
+
+	dFloat omegaMag = dFloat(2.0f) * dAtan2 (dirMag, dq.m_w) * invdt;
+	return omegaDir.Scale (dirMagInv * omegaMag);
+}
+
+dQuaternion dQuaternion::Slerp (const dQuaternion &q1, dFloat t) const 
+{
+	dQuaternion q;
+
+	dFloat dot = DotProduct (q1);
+	dAssert (dot >= 0.0f);
+
+	if ((dot + dFloat(1.0f)) > dFloat(1.0e-5f)) {
+		dFloat Sclp;
+		dFloat Sclq;
+		if (dot < dFloat(0.995f)) {
+
+			dFloat ang = dAcos (dot);
+			dFloat sinAng = dSin (ang);
+
+			dFloat den = dFloat(1.0f) / sinAng;
+
+			Sclp = dSin ((dFloat(1.0f) - t ) * ang) * den;
+			Sclq = dSin (t * ang) * den;
+
+		} else  {
+			Sclp = dFloat(1.0f) - t;
+			Sclq = t;
+		}
+
+		q.m_w = m_w * Sclp + q1.m_w * Sclq;
+		q.m_x = m_x * Sclp + q1.m_x * Sclq;
+		q.m_y = m_y * Sclp + q1.m_y * Sclq;
+		q.m_z = m_z * Sclp + q1.m_z * Sclq;
+
+	} else {
+		q.m_w =  m_z;
+		q.m_x = -m_y;
+		q.m_y =  m_x;
+		q.m_z =  m_w;
+
+		dFloat Sclp = dSin ((dFloat(1.0f) - t) * dFloat (dPi *0.5f));
+		dFloat Sclq = dSin (t * dFloat (dPi * 0.5f));
+
+		q.m_w = m_w * Sclp + q.m_w * Sclq;
+		q.m_x = m_x * Sclp + q.m_x * Sclq;
+		q.m_y = m_y * Sclp + q.m_y * Sclq;
+		q.m_z = m_z * Sclp + q.m_z * Sclq;
+	}
+
+	dot = q.DotProduct (q);
+	if ((dot) < (1.0f - 1.0e-4f) ) {
+		dot = dFloat(1.0f) / dSqrt (dot);
+		//dot = dgRsqrt (dot);
+		q.m_w *= dot;
+		q.m_x *= dot;
+		q.m_y *= dot;
+		q.m_z *= dot;
+	}
+	return q;
+}
+
+dVector dQuaternion::RotateVector (const dVector& point) const
+{
+	dMatrix matrix (*this, dVector (0.0f, 0.0f, 0.0f, 1.0f));
+	return matrix.RotateVector(point);
+}
+
+dVector dQuaternion::UnrotateVector (const dVector& point) const
+{
+	dMatrix matrix (*this, dVector (0.0f, 0.0f, 0.0f, 1.0f));
+	return matrix.UnrotateVector(point);
+}
+
+void dQuaternion::GetEulerAngles(dVector& euler1, dVector& euler2, dEulerAngleOrder order) const 
+{
+	dMatrix matrix (*this, dVector (0.0f,0.0f,0.0f,1.0f));
+	//return matrix.GetEulerAngles (order);
+    matrix.GetEulerAngles (euler1, euler2, order);
+}
+
+dQuaternion dQuaternion::IntegrateOmega (const dVector& omega, dFloat timestep) const
+{
+	// this is correct
+	dQuaternion rotation (*this);
+	dFloat omegaMag2 = omega.DotProduct3(omega);
+	const dFloat errAngle = 0.0125f * dDegreeToRad;
+	const dFloat errAngle2 = errAngle * errAngle;
+	if (omegaMag2 > errAngle2) {
+		dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
+		dVector omegaAxis (omega.Scale (invOmegaMag));
+		dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
+		dQuaternion deltaRotation (omegaAxis, omegaAngle);
+		rotation = rotation * deltaRotation;
+		rotation.Scale(1.0f / dSqrt (rotation.DotProduct (rotation)));
+	}
+	return rotation;
+}

thirdparty/src/newton/dMath/dQuaternion.h

@@ -0,0 +1,109 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#ifndef __dQuaternion__
+#define __dQuaternion__
+
+#include "dVector.h"
+class dMatrix;
+
+D_MSC_VECTOR_ALIGNMENT
+class dQuaternion
+{
+	public:
+	dQuaternion (); 
+	dQuaternion (const dMatrix& matrix);
+	//dQuaternion (dFloat w, dFloat x, dFloat y, dFloat z); 
+	dQuaternion (dFloat x, dFloat y, dFloat z, dFloat w); 
+	dQuaternion (const dVector& unit_Axis, dFloat Angle = 0.0f);
+	
+	void Scale (dFloat scale); 
+	void Normalize (); 
+	inline dFloat DotProduct (const dQuaternion& q) const;
+	dQuaternion Inverse () const; 
+
+	dVector RotateVector (const dVector& point) const;
+	dVector UnrotateVector (const dVector& point) const;
+
+    void GetEulerAngles(dVector& euler1, dVector& euler2, dEulerAngleOrder order = m_pitchYawRoll) const;
+	dVector CalcAverageOmega (const dQuaternion &q1, dFloat invdt) const;
+	dQuaternion Slerp (const dQuaternion &q1, dFloat t) const;
+	dQuaternion IntegrateOmega (const dVector& omega, dFloat timestep) const;
+
+	dQuaternion operator* (const dQuaternion &q) const;
+	dQuaternion operator+ (const dQuaternion &q) const; 
+	dQuaternion operator- (const dQuaternion &q) const; 
+
+	dFloat m_x;
+	dFloat m_y;
+	dFloat m_z;
+	dFloat m_w;
+};
+
+inline dQuaternion::dQuaternion () 
+	:m_x(0.0f)
+	,m_y(0.0f)
+	,m_z(0.0f)
+	,m_w(1.0f)
+{
+}
+
+inline dQuaternion::dQuaternion (dFloat x, dFloat y, dFloat z, dFloat w) 
+	:m_x(x)
+	,m_y(y)
+	,m_z(z)
+	,m_w(w)
+{
+}
+
+inline void dQuaternion::Scale (dFloat scale) 
+{
+	m_w *= scale;
+	m_x *= scale;
+	m_y *= scale;
+	m_z *= scale;
+}
+
+inline void dQuaternion::Normalize () 
+{
+	Scale (1.0f / dSqrt (DotProduct (*this)));
+}
+
+inline dFloat dQuaternion::DotProduct (const dQuaternion &q1) const
+{
+	return m_w * q1.m_w + m_x * q1.m_x + m_y * q1.m_y + m_z * q1.m_z;
+}
+
+inline dQuaternion dQuaternion::Inverse () const 
+{
+	return dQuaternion (-m_x, -m_y, -m_z, m_w);
+}
+
+inline dQuaternion dQuaternion::operator+ (const dQuaternion &q) const
+{
+	return dQuaternion (m_x + q.m_x, m_y + q.m_y, m_z + q.m_z, m_w + q.m_w);
+}
+
+inline dQuaternion dQuaternion::operator- (const dQuaternion &B) const
+{
+	return dQuaternion (m_x - B.m_x, m_y - B.m_y, m_z - B.m_z, m_w - B.m_w);
+}
+
+inline dQuaternion dQuaternion::operator* (const dQuaternion &q) const
+{
+	return dQuaternion (q.m_x * m_w + q.m_w * m_x - q.m_z * m_y + q.m_y * m_z, 
+						q.m_y * m_w + q.m_z * m_x + q.m_w * m_y - q.m_x * m_z, 
+						q.m_z * m_w - q.m_y * m_x + q.m_x * m_y + q.m_w * m_z, 
+						q.m_w * m_w - q.m_x * m_x - q.m_y * m_y - q.m_z * m_z); 
+}
+
+#endif 
+

thirdparty/src/newton/dMath/dStdAfxMath.cpp

@@ -0,0 +1,20 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+// stdafx.cpp : source file that includes just the standard includes
+// MinimalMath.pch will be the pre-compiled header
+// stdafx.obj will contain the pre-compiled type information
+
+#include "dStdAfxMath.h"
+
+// TODO: reference any additional headers you need in STDAFX.H
+// and not in this file
+

thirdparty/src/newton/dMath/dStdAfxMath.h

@@ -0,0 +1,36 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+// stdafx.h : include file for standard system include files,
+// or project specific include files that are used frequently, but
+// are changed infrequently
+//
+
+#ifndef __STDAFXMATH_H__
+#define __STDAFXMATH_H__
+
+
+#ifdef _MSC_VER
+	#ifndef WIN32_LEAN_AND_MEAN
+		#define WIN32_LEAN_AND_MEAN		// Exclude rarely-used stuff from Windows headers
+	#endif
+	#include <windows.h>
+	#include <crtdbg.h>
+#endif
+
+#if ( defined (__MINGW32__) || defined (__MINGW64__) )
+	#include <crtdbg.h>
+#endif
+
+#include <stdio.h>
+#include <assert.h>
+#endif
+

thirdparty/src/newton/dMath/dVector.cpp

@@ -0,0 +1,13 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dStdAfxMath.h"
+

thirdparty/src/newton/dMath/dVector.h

@@ -0,0 +1,328 @@
+/* Copyright (c) <2003-2019> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#ifndef __dVector__
+#define __dVector__
+
+
+#include "dMathDefines.h"
+
+
+// small but very effective 4 dimensional template vector class 
+
+template<class T>
+class TemplateVector
+{
+	public:
+	TemplateVector ();
+	TemplateVector (const T val);
+	TemplateVector (const T* const ptr);
+	TemplateVector (T m_x, T m_y, T m_z, T m_w = T(1.0f)); 
+	TemplateVector Scale (T s) const;
+
+	T& operator[] (int i);
+	const T& operator[] (int i) const;
+
+	TemplateVector operator+ (const TemplateVector &A) const; 
+	TemplateVector operator- (const TemplateVector &A) const; 
+	TemplateVector operator* (const TemplateVector &A) const; 
+	TemplateVector& operator+= (const TemplateVector &A);
+	TemplateVector& operator-= (const TemplateVector &A); 
+	TemplateVector& operator*= (const TemplateVector &A); 
+
+	TemplateVector Abs() const;
+	TemplateVector Min(const TemplateVector &A) const;
+	TemplateVector Max(const TemplateVector &A) const;
+
+	T DotProduct3 (const TemplateVector &A) const; 
+	TemplateVector Normalize () const; 
+	TemplateVector CrossProduct (const TemplateVector &A) const; 
+
+	T m_x;
+	T m_y;
+	T m_z;
+	T m_w;
+};
+
+class dVector: public TemplateVector<dFloat>
+{
+	public:
+	dVector()
+		:TemplateVector<dFloat>()
+	{
+	}
+
+	dVector(dFloat val)
+		:TemplateVector<dFloat>(val)
+	{
+	}
+
+	dVector (const TemplateVector<dFloat>& v)
+		:TemplateVector<dFloat>(v)
+	{
+	}
+
+	dVector (const dFloat* const ptr)
+		:TemplateVector<dFloat>(ptr)
+	{
+	}
+
+	dVector (dFloat x, dFloat y, dFloat z, dFloat w = 1.0f) 
+		:TemplateVector<dFloat>(x, y, z, w)
+	{
+	}
+
+#ifndef _NEWTON_USE_DOUBLE
+	dVector (const TemplateVector<dFloat64>& v)
+		:TemplateVector<dFloat>(dFloat(v.m_x), dFloat(v.m_y), dFloat(v.m_z), dFloat(v.m_w))
+	{
+	}
+#endif
+};
+
+
+class dBigVector: public TemplateVector<dFloat64>
+{
+	public: 
+	dBigVector(){};
+	dBigVector(dFloat64 val)
+		:TemplateVector<dFloat64> (val)
+	{
+	}
+
+	dBigVector (const dFloat64* const ptr)
+		:TemplateVector<dFloat64> (ptr)
+	{
+	}
+
+	dBigVector (const TemplateVector<dFloat64>& v)
+		:TemplateVector<dFloat64> (v.m_x, v.m_y, v.m_z, v.m_w)
+	{
+	}
+
+	dBigVector(const dVector& v)
+		:TemplateVector<dFloat64>(v.m_x, v.m_y, v.m_z, v.m_w)
+	{
+	}
+
+	dBigVector (dFloat64 x, dFloat64 y, dFloat64 z, dFloat64 w = dFloat(1.0f))
+		:TemplateVector<dFloat64> (x, y, z, w)
+	{
+	}
+};
+
+
+template<class T>
+TemplateVector<T>::TemplateVector() 
+{
+}
+
+template<class T>
+TemplateVector<T>::TemplateVector(const T val)
+	:m_x (val), m_y(val), m_z(val), m_w(val)
+{
+}
+
+template<class T>
+TemplateVector<T>::TemplateVector(const T *ptr)
+	:m_x (ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(0.0f)
+{
+}
+
+template<class T>
+TemplateVector<T>::TemplateVector(T x, T y, T z, T w) 
+	:m_x (x), m_y(y), m_z(z), m_w(w)
+{
+}
+
+template<class T>
+T& TemplateVector<T>::operator[] (int i)
+{
+	return (&m_x)[i];
+}	
+
+template<class T>
+const T& TemplateVector<T>::operator[] (int i) const
+{
+	return (&m_x)[i];
+}
+
+template<class T>
+TemplateVector<T> TemplateVector<T>::Scale (T scale) const
+{
+	return TemplateVector<T> (m_x * scale, m_y * scale, m_z * scale, m_w);
+}
+
+template<class T>
+TemplateVector<T> TemplateVector<T>::operator+ (const TemplateVector<T>& B) const
+{
+	return TemplateVector<T> (m_x + B.m_x, m_y + B.m_y, m_z + B.m_z, m_w);
+}
+
+template<class T>
+TemplateVector<T> TemplateVector<T>::operator* (const TemplateVector<T>& B) const
+{
+	return TemplateVector<T>(m_x * B.m_x, m_y * B.m_y, m_z * B.m_z, m_w);
+}
+
+
+template<class T>
+TemplateVector<T>& TemplateVector<T>::operator+= (const TemplateVector<T>& A) 
+{
+	m_x += A.m_x;
+	m_y += A.m_y;
+	m_z += A.m_z;
+	return *this;
+}
+
+template<class T>
+TemplateVector<T> TemplateVector<T>::operator- (const TemplateVector<T>& A) const
+{
+	return TemplateVector<T> (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w);
+}
+
+template<class T>
+TemplateVector<T>& TemplateVector<T>::operator-= (const TemplateVector<T>& A) 
+{
+	m_x -= A.m_x;
+	m_y -= A.m_y;
+	m_z -= A.m_z;
+	return *this;
+}
+
+template<class T>
+TemplateVector<T>& TemplateVector<T>::operator*= (const TemplateVector<T>& A)
+{
+	m_x *= A.m_x;
+	m_y *= A.m_y;
+	m_z *= A.m_z;
+	//m_w *= A.m_w;
+	return *this;
+}
+
+template<class T>
+TemplateVector<T> TemplateVector<T>::Abs() const
+{
+	return dVector(dAbs(m_x), dAbs(m_y), dAbs(m_z), dAbs(m_w));
+}
+
+template<class T>
+TemplateVector<T> TemplateVector<T>::Min(const TemplateVector &A) const
+{
+	return dVector (dMin (m_x, A.m_x), dMin (m_y, A.m_y), dMin (m_z, A.m_z), dMin (m_w, A.m_w));
+}
+
+template<class T>
+TemplateVector<T> TemplateVector<T>::Max(const TemplateVector &A) const
+{
+	return dVector (dMax (m_x, A.m_x), dMax (m_y, A.m_y), dMax (m_z, A.m_z), dMax (m_w, A.m_w));
+}
+
+
+template<class T>
+T TemplateVector<T>::DotProduct3 (const TemplateVector<T>& A) const
+{
+	return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z;
+}
+
+template<class T>
+TemplateVector<T> TemplateVector<T>::Normalize () const
+{
+	T mag (DotProduct3(*this));
+	return Scale (1.0f / T(sqrt (mag)));
+}
+
+template<class T>
+TemplateVector<T> TemplateVector<T>::CrossProduct (const TemplateVector<T>& A) const
+{
+	return TemplateVector<T> (m_y * A.m_z - m_z * A.m_y, m_z * A.m_x - m_x * A.m_z, m_x * A.m_y - m_y * A.m_x, m_w);
+}
+
+
+class dSpatialVector
+{
+	public:
+	inline dSpatialVector()
+	{
+	}
+
+	inline dSpatialVector(const dFloat a)
+	{
+		for (int i = 0; i < 6; i++) {
+			m_d[i] = a;
+		}
+	}
+
+	inline dSpatialVector(const dVector& low, const dVector& high)
+	{
+		m_d[0] = low[0];
+		m_d[1] = low[1];
+		m_d[2] = low[2];
+		m_d[3] = high[0];
+		m_d[4] = high[1];
+		m_d[5] = high[2];
+	}
+
+	inline dFloat& operator[] (int i)
+	{
+		dAssert(i < 6);
+		dAssert(i >= 0);
+		return m_d[i];
+	}
+
+	inline const dFloat& operator[] (int i) const
+	{
+		dAssert(i < 6);
+		dAssert(i >= 0);
+		return m_d[i];
+	}
+
+	inline dSpatialVector operator+ (const dSpatialVector& A) const
+	{
+		dSpatialVector tmp;
+		for (int i = 0; i < 6; i++) {
+			tmp[i] = m_d[i] + A.m_d[i];
+		}
+		return tmp;
+	}
+
+	inline dSpatialVector operator* (const dSpatialVector& A) const
+	{
+		dSpatialVector tmp;
+		for (int i = 0; i < 6; i++) {
+			tmp[i] = m_d[i] * A.m_d[i];
+		}
+		return tmp;
+	}
+
+	inline dFloat DotProduct(const dSpatialVector& v) const
+	{
+		dFloat acc = dFloat (0.0f);
+		for (int i = 0; i < 6; i++) {
+			acc += m_d[i] * v.m_d[i];
+		}
+		return acc;
+	}
+
+	inline dSpatialVector Scale(dFloat s) const
+	{
+		dSpatialVector tmp;
+		for (int i = 0; i < 6; i++) {
+			tmp[i] = m_d[i] * s;
+		}
+		return tmp;
+	}
+
+	dFloat m_d[6];
+};
+
+#endif
+

thirdparty/src/newton/dgCore/dg.cpp

@@ -0,0 +1,27 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+
+
+
+
+

thirdparty/src/newton/dgCore/dg.h

@@ -0,0 +1,67 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#if !defined(AFX_DG_H__FGRTY_5GR39H_98TQ_H)
+#define AFX_DG_H__FGRTY_5GR39H_98TQ_H
+
+#include "dgTypes.h"
+
+#include "dgRef.h"
+#include "dgCRC.h"
+#include "dgObb.h"
+#include "dgRtti.h"
+#include "dgList.h"
+#include "dgTree.h"
+#include "dgNode.h"
+#include "dgHeap.h"
+#include "dgSort.h"
+#include "dgArray.h"
+#include "dgStack.h"
+#include "dgDebug.h"
+#include "dgGraph.h"
+#include "dgPlane.h"
+#include "dgGoogol.h"
+#include "dgVector.h"
+#include "dgMatrix.h"
+#include "dgMemory.h"
+#include "dgRandom.h"
+#include "dgThread.h"
+#include "dgProfiler.h"
+#include "dgFastQueue.h"
+#include "dgPolyhedra.h"
+#include "dgThreadHive.h"
+#include "dgPathFinder.h"
+#include "dgRefCounter.h"
+#include "dgQuaternion.h"
+#include "dgMutexThread.h"
+#include "dgConvexHull3d.h"
+#include "dgConvexHull4d.h"
+#include "dgIntersections.h"
+#include "dgGeneralVector.h"
+#include "dgGeneralMatrix.h"
+#include "dgAABBPolygonSoup.h"
+#include "dgSmallDeterminant.h"
+#include "dgPolygonSoupBuilder.h"
+#include "dgPolygonSoupDatabase.h"
+#include "dgPolyhedraMassProperties.h"
+#include "dgDelaunayTetrahedralization.h"
+
+#endif

thirdparty/src/newton/dgCore/dgAABBPolygonSoup.h

@@ -0,0 +1,251 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DG_AABB_POLYGON_SOUP_H_
+#define __DG_AABB_POLYGON_SOUP_H_
+
+#include "dgStdafx.h"
+#include "dgIntersections.h"
+#include "dgPolygonSoupDatabase.h"
+
+
+class dgPolygonSoupDatabaseBuilder;
+
+
+class dgAABBPolygonSoup: public dgPolygonSoupDatabase
+{
+	public:
+	class dgNode
+	{
+		public:
+		enum dgNodeType
+		{
+			m_binary = 0,
+			m_leaf,
+		};
+
+		class dgLeafNodePtr
+		{
+			#define DG_INDEX_COUNT_BITS 6
+
+			public:
+			DG_INLINE dgLeafNodePtr ()
+			{
+				dgAssert (0);
+			}
+
+			DG_INLINE dgLeafNodePtr (dgUnsigned32 node)
+			{
+				m_node = node;
+				dgAssert (!IsLeaf());
+			}
+
+			DG_INLINE dgUnsigned32 IsLeaf () const 
+			{
+				return m_node & 0x80000000;
+			}
+
+			DG_INLINE dgUnsigned32 GetCount() const 
+			{
+				dgAssert (IsLeaf());
+				return (m_node & (~0x80000000)) >> (32 - DG_INDEX_COUNT_BITS - 1);
+			}
+
+			DG_INLINE dgUnsigned32 GetIndex() const 
+			{
+				dgAssert (IsLeaf());
+				return m_node & (~(-(1 << (32 - DG_INDEX_COUNT_BITS - 1))));
+			}
+
+
+			DG_INLINE dgLeafNodePtr (dgUnsigned32 faceIndexCount, dgUnsigned32 faceIndexStart)
+			{
+				dgAssert (faceIndexCount < (1<<DG_INDEX_COUNT_BITS));
+				m_node = 0x80000000 | (faceIndexCount << (32 - DG_INDEX_COUNT_BITS - 1)) | faceIndexStart;
+			}
+
+			DG_INLINE dgNode* GetNode (const void* const root) const
+			{
+				return ((dgNode*) root) + m_node;
+			}
+
+			union {
+				dgUnsigned32 m_node;
+			};
+		};
+
+
+		dgNode ()
+			:m_indexBox0(0)
+			,m_indexBox1(0)
+			,m_left(0)
+			,m_right(0)
+		{
+		}
+
+		DG_INLINE dgFloat32 RayDistance (const dgFastRayTest& ray, const dgTriplex* const vertexArray) const
+		{
+			dgVector minBox (&vertexArray[m_indexBox0].m_x);
+			dgVector maxBox (&vertexArray[m_indexBox1].m_x);
+			minBox = minBox & dgVector::m_triplexMask;
+			maxBox = maxBox & dgVector::m_triplexMask;
+			return ray.BoxIntersect(minBox, maxBox);
+		}
+
+		DG_INLINE dgFloat32 BoxPenetration (const dgFastAABBInfo& obb, const dgTriplex* const vertexArray) const
+		{
+			dgVector p0 (&vertexArray[m_indexBox0].m_x);
+			dgVector p1 (&vertexArray[m_indexBox1].m_x);
+			p0 = p0 & dgVector::m_triplexMask;
+			p1 = p1 & dgVector::m_triplexMask;
+			dgVector minBox (p0 - obb.m_p1);
+			dgVector maxBox (p1 - obb.m_p0);
+			dgAssert(maxBox.m_x >= minBox.m_x);
+			dgAssert(maxBox.m_y >= minBox.m_y);
+			dgAssert(maxBox.m_z >= minBox.m_z);
+
+			dgVector mask ((minBox * maxBox) < dgVector::m_zero);
+			dgVector dist (maxBox.GetMin (minBox.Abs()) & mask);
+			dist = dist.GetMin(dist.ShiftTripleRight());
+			dist = dist.GetMin(dist.ShiftTripleRight());
+
+			if (dist.GetScalar() > dgFloat32 (0.0f)) {
+				dgVector origin (dgVector::m_half * (p1 + p0));
+				dgVector size (dgVector::m_half * (p1 - p0));
+
+				origin = obb.UntransformVector(origin);
+				size = obb.m_absDir.RotateVector(size);
+				dgVector q0 (origin - size);
+				dgVector q1 (origin + size);
+				dgVector minBox1 (q0 - obb.m_size);
+				dgVector maxBox1 (q1 + obb.m_size);
+				dgAssert(maxBox1.m_x >= minBox1.m_x);
+				dgAssert(maxBox1.m_y >= minBox1.m_y);
+				dgAssert(maxBox1.m_z >= minBox1.m_z);
+				dgVector mask1 ((minBox1 * maxBox1) < dgVector::m_zero);
+				dgVector dist1 (maxBox1.GetMin (minBox1.Abs()) & mask1);
+				dist1 = dist1.GetMin(dist1.ShiftTripleRight());
+				dist1 = dist1.GetMin(dist1.ShiftTripleRight());
+				dist = dist.GetMin(dist1);
+			} else {
+				dgVector p1p0((minBox.Abs()).GetMin(maxBox.Abs()).AndNot(mask));
+				dist = p1p0.DotProduct(p1p0);
+				dist = dist.Sqrt() * dgVector::m_negOne;
+			}
+			return	dist.GetScalar();
+		}
+
+		DG_INLINE dgFloat32 BoxIntersect (const dgFastRayTest& ray, const dgFastRayTest& obbRay, const dgFastAABBInfo& obb, const dgTriplex* const vertexArray) const
+		{
+			dgVector p0 (&vertexArray[m_indexBox0].m_x);
+			dgVector p1 (&vertexArray[m_indexBox1].m_x);
+			p0 = p0 & dgVector::m_triplexMask;
+			p1 = p1 & dgVector::m_triplexMask;
+
+			dgVector minBox (p0 - obb.m_p1);
+			dgVector maxBox (p1 - obb.m_p0);
+			dgFloat32 dist = ray.BoxIntersect(minBox, maxBox);
+			if (dist < dgFloat32 (1.0f)) {
+				dgVector origin (dgVector::m_half * (p1 + p0));
+				dgVector size (dgVector::m_half * (p1 - p0));
+
+				origin = obb.UntransformVector(origin);
+				size = obb.m_absDir.RotateVector(size);
+				dgVector q0 (origin - size);
+				dgVector q1 (origin + size);
+
+				dgVector minBox1 (q0 - obb.m_size);
+				dgVector maxBox1 (q1 + obb.m_size);
+				dgFloat32 dist1 = obbRay.BoxIntersect(minBox1, maxBox1);
+				dist = (dist1  > dgFloat32 (1.0f)) ? dist1 : dgMax (dist1, dist);
+			}
+			return dist;
+		}
+
+		dgInt32 m_indexBox0;
+		dgInt32 m_indexBox1;
+		dgLeafNodePtr m_left;
+		dgLeafNodePtr m_right;
+	};
+
+	class dgSpliteInfo;
+	class dgNodeBuilder;
+
+	virtual void GetAABB (dgVector& p0, dgVector& p1) const;
+	virtual void Serialize (dgSerialize callback, void* const userData) const;
+	virtual void Deserialize (dgDeserialize callback, void* const userData, dgInt32 revisionNumber);
+
+	protected:
+	dgAABBPolygonSoup ();
+	virtual ~dgAABBPolygonSoup ();
+
+	void Create (const dgPolygonSoupDatabaseBuilder& builder, bool optimizedBuild);
+	void CalculateAdjacendy ();
+	virtual void ForAllSectorsRayHit (const dgFastRayTest& ray, dgFloat32 maxT, dgRayIntersectCallback callback, void* const context) const;
+	virtual void ForAllSectors (const dgFastAABBInfo& obbAabb, const dgVector& boxDistanceTravel, dgFloat32 m_maxT, dgAABBIntersectCallback callback, void* const context) const;
+	
+
+	DG_INLINE void* GetRootNode() const 
+	{
+		return m_aabb;
+	}
+
+	DG_INLINE void* GetBackNode(const void* const root) const 
+	{
+		dgNode* const node = (dgNode*) root;
+		return node->m_left.IsLeaf() ? NULL : node->m_left.GetNode(m_aabb);
+	}
+
+	DG_INLINE void* GetFrontNode(const void* const root) const 
+	{
+		dgNode* const node = (dgNode*) root;
+		return node->m_right.IsLeaf() ? NULL : node->m_right.GetNode(m_aabb);
+	}
+
+	DG_INLINE void GetNodeAABB(const void* const root, dgVector& p0, dgVector& p1) const 
+	{
+		const dgNode* const node = (dgNode*)root;
+		p0 = dgVector (&((dgTriplex*)m_localVertex)[node->m_indexBox0].m_x);
+		p1 = dgVector (&((dgTriplex*)m_localVertex)[node->m_indexBox1].m_x);
+		p0 = p0 & dgVector::m_triplexMask;
+		p1 = p1 & dgVector::m_triplexMask;
+	}
+
+	virtual dgVector ForAllSectorsSupportVectex (const dgVector& dir) const;
+
+	private:
+	dgNodeBuilder* BuildTopDown (dgNodeBuilder* const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgNodeBuilder** const allocator) const;
+	dgFloat32 CalculateFaceMaxSize (const dgVector* const vertex, dgInt32 indexCount, const dgInt32* const indexArray) const;
+//	static dgIntersectStatus CalculateManifoldFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount);
+	static dgIntersectStatus CalculateDisjointedFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance);
+	static dgIntersectStatus CalculateAllFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance);
+	void ImproveNodeFitness (dgNodeBuilder* const node) const;
+
+	dgInt32 m_nodesCount;
+	dgInt32 m_indexCount;
+	dgNode* m_aabb;
+	dgInt32* m_indices;
+};
+
+
+#endif
+
+

thirdparty/src/newton/dgCore/dgArray.h

@@ -0,0 +1,227 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+/****************************************************************************
+*
+*  Visual C++ 6.0 created by: Julio Jerez
+*
+****************************************************************************/
+#ifndef __dgArray__
+#define __dgArray__
+
+#include "dgStdafx.h"
+
+template<class T>
+class dgArray
+{
+	public:
+	dgArray ();
+	dgArray (dgMemoryAllocator* const allocator, dgInt32 aligmentInBytes = DG_MEMORY_GRANULARITY);
+	dgArray (const dgArray& source, dgInt32 itemsToCopy);
+	dgArray(const dgArray& source);
+	~dgArray ();
+	DG_CLASS_ALLOCATOR(allocator)
+	
+	DG_INLINE T& operator[] (dgInt32 i);
+	DG_INLINE const T& operator[] (dgInt32 i) const;
+	
+	void Clear () const;
+	void Resize (dgInt32 size) const;
+	DG_INLINE void ResizeIfNecessary  (dgInt32 index) const;
+
+	dgInt32 GetElementSize() const;
+	dgInt32 GetBytesCapacity () const;
+	dgInt32 GetElementsCapacity () const; 
+	dgMemoryAllocator* GetAllocator() const;
+	void SetAllocator(dgMemoryAllocator* const allocator);
+
+	protected:
+	mutable T *m_array;
+	private:
+	mutable dgInt32 m_maxSize;
+	dgInt32 m_aligmentInBytes;
+	dgMemoryAllocator* m_allocator;
+};
+
+template<class T>
+dgArray<T>::dgArray()
+	:m_array(NULL)
+	,m_maxSize(0)
+	,m_aligmentInBytes(DG_MEMORY_GRANULARITY)
+	,m_allocator(NULL)
+{
+	m_aligmentInBytes = 1 << dgExp2(m_aligmentInBytes);
+}
+
+template<class T>
+dgArray<T>::dgArray (dgMemoryAllocator* const allocator, dgInt32 aligmentInBytes)
+	:m_array(NULL)
+	,m_maxSize(0)
+	,m_aligmentInBytes(aligmentInBytes)
+	,m_allocator(allocator)
+{
+	if (m_aligmentInBytes <= 0) {
+		m_aligmentInBytes = DG_MEMORY_GRANULARITY;
+	}
+	m_aligmentInBytes = 1 << dgExp2(m_aligmentInBytes);
+}
+
+template<class T>
+dgArray<T>::dgArray (const dgArray& source, dgInt32 itemsToCopy)
+	:m_array(NULL)
+	,m_maxSize(itemsToCopy)
+	,m_aligmentInBytes(source.m_aligmentInBytes)
+	,m_allocator(source.m_allocator)
+{
+	if (source.m_array) {
+		m_array = (T*) m_allocator->MallocLow (sizeof (T) * itemsToCopy, m_aligmentInBytes);
+		for (dgInt32 i = 0; i < itemsToCopy; i++) {
+			m_array[i] = source.m_array[i];
+		}
+	}
+}
+template<class T>
+dgArray<T>::dgArray(const dgArray& source)
+	:m_array(NULL)
+	,m_maxSize(source.m_maxSize)
+	,m_aligmentInBytes(source.m_aligmentInBytes)
+	,m_allocator(source.m_allocator)
+{
+	dgAssert(0);
+	// use dgArray<T>::dgArray(const dgArray& source, dgInt32 itemsToCopy)
+	if (source.m_array) {
+		m_array = (T*)m_allocator->MallocLow(sizeof(T) * m_maxSize, m_aligmentInBytes);
+		for (dgInt32 i = 0; i < m_maxSize; i++) {
+			m_array[i] = source.m_array[i];
+		}
+	}
+}
+
+template<class T>
+dgArray<T>::~dgArray ()
+{
+	if (m_array) {
+		m_allocator->FreeLow (m_array);
+	}
+}
+
+template<class T>
+DG_INLINE const T& dgArray<T>::operator[] (dgInt32 i) const
+{ 
+	dgAssert (i >= 0);
+	while (i >= m_maxSize) {
+		Resize (i * 2);
+	}
+	return m_array[i];
+}
+
+template<class T>
+DG_INLINE T& dgArray<T>::operator[] (dgInt32 i)
+{
+	dgAssert (i >= 0);
+	while (i >= m_maxSize) {
+		Resize (i * 2);
+	}
+	return m_array[i];
+}
+
+template<class T>
+dgInt32 dgArray<T>::GetElementSize() const
+{
+	return sizeof (T);
+}
+
+template<class T>
+dgInt32 dgArray<T>::GetElementsCapacity () const
+{
+	return m_maxSize;
+}
+
+template<class T>
+dgInt32 dgArray<T>::GetBytesCapacity () const
+{
+	return  m_maxSize * GetElementSize();
+}
+
+template<class T>
+void dgArray<T>::Clear () const
+{
+	if (m_array) {
+		m_allocator->FreeLow (m_array);
+		m_array = NULL;
+	}
+	m_maxSize = 0;
+}
+
+template<class T>
+dgMemoryAllocator* dgArray<T>::GetAllocator() const
+{
+	return m_allocator;
+}
+
+template<class T>
+void dgArray<T>::SetAllocator(dgMemoryAllocator* const allocator)
+{
+	dgAssert (!m_allocator);
+	m_allocator = allocator;
+}
+
+template<class T>
+void dgArray<T>::Resize (dgInt32 size) const
+{
+	if (size >= m_maxSize) {
+		size = dgMax (size, 16);
+		T* const newArray = (T*) m_allocator->MallocLow (dgInt32 (sizeof (T) * size), m_aligmentInBytes);
+		if (m_array) {
+			for (dgInt32 i = 0; i < m_maxSize; i ++) {
+				newArray[i]	= m_array[i];
+			}
+			m_allocator->FreeLow (m_array);
+		}
+		m_array = newArray;
+		m_maxSize = size;
+	} else if (size < m_maxSize) {
+		size = dgMax (size, 16);
+		T* const newArray = (T*) m_allocator->MallocLow (dgInt32 (sizeof (T) * size), m_aligmentInBytes);
+		if (m_array) {
+			for (dgInt32 i = 0; i < size; i ++) {
+				newArray[i]	= m_array[i];
+			}
+			m_allocator->FreeLow (m_array);
+		}
+		m_array = newArray;
+		m_maxSize = size;
+	}
+}
+
+template<class T>
+DG_INLINE void dgArray<T>::ResizeIfNecessary  (dgInt32 size) const
+{
+	while (size >= m_maxSize) {
+		Resize (m_maxSize * 2);
+	}
+}
+
+#endif
+
+
+
+

thirdparty/src/newton/dgCore/dgCRC.cpp

@@ -0,0 +1,390 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgCRC.h"
+#include "dgList.h"
+#include "dgTree.h"
+#include "dgDebug.h"
+#include "dgRandom.h"
+
+namespace InternalCRC
+{
+	const dgInt32 STRING_POOL_SIZE =	1024 * 8 - 256;
+
+	static dgUnsigned32 randBits[] = {
+		0x00000001, 0x2C11F801, 0xDFD8F60E, 0x6C8FA2B7, 
+		0xB573754C, 0x1522DCDD, 0x21615D3A, 0xE1B307F3, 
+		0x12AFA158, 0x53D18179, 0x70950126, 0x941702EF, 
+		0x756FE824, 0x694801D5, 0x36DF4DD2, 0x63D80FAB, 
+		0xB8AE95B0, 0x902439F1, 0x090C6F3E, 0x2B7C6A27, 
+		0x8344B5FC, 0x67D3C5CD, 0x22F5516A, 0x2FB00E63, 
+		0xFC761508, 0x87B00169, 0x27EBA056, 0x8CC0B85F, 
+		0xE33D3ED4, 0x95DA08C5, 0x13E5C802, 0x9DD9E41B, 
+		0xD4577F60, 0x3DD6B7E1, 0x096AF46E, 0x1A00CD97, 
+		0x4B10E2AC, 0x22EAAABD, 0x683F119A, 0x62D070D3, 
+		0xA8D034B8, 0xAA363D59, 0x58CECB86, 0x40F589CF, 
+		0x4F630184, 0x38918BB5, 0xB85B8E32, 0x0A6A948B, 
+		0x9A099510, 0x402871D1, 0x11E7859E, 0xEE73CD07, 
+		0x4142FB5C, 0x39D68BAD, 0x0FE19DCA, 0xE35B2F43, 
+		0x75590068, 0x66433549, 0x929182B6, 0x71EC773F, 
+		0xBBAC3034, 0xF2BD8AA5, 0x5743A062, 0x5AB120FB, 
+		0x5ABFD6C0, 0xDDD867C1, 0xDC3522CE, 0xD0EC6877, 
+		0xE106000C, 0xB7C6689D, 0xED3FF5FA, 0xC75749B3, 
+		0x126B7818, 0x1A75E939, 0x0546C5E6, 0x8A9C80AF, 
+		0x48A3CAE4, 0x756D0595, 0x7060FE92, 0xA594896B, 
+		0x12354470, 0x896599B1, 0xDAC6CBFE, 0xCB419FE7, 
+		0x9C44F0BC, 0xAFA9418D, 0xB87D1A2A, 0x428BC023, 
+		0x33229BC8, 0xC92D5929, 0xB1C19516, 0x0FBCA61F, 
+		0xE594D194, 0x716EFC85, 0x0036A8C2, 0xD7BBCDDB, 
+		0x16E4DE20, 0xD10F07A1, 0x68CF812E, 0x390A7357, 
+		0x8BAACD6C, 0x2C2E167D, 0x3E7C0A5A, 0x167F9293, 
+		0x3D596B78, 0x08888519, 0x9994F046, 0x0FC3E78F, 
+		0x008A4444, 0x87526F75, 0xB0079EF2, 0x238DEE4B, 
+		0xCA09A3D0, 0x4ED3B191, 0xFA42425E, 0x379DE2C7, 
+		0x1EA2961C, 0x1FC3E76D, 0x90DFC68A, 0x0279C103, 
+		0xF9AAE728, 0xF2666D09, 0xEF13D776, 0x92E944FF, 
+		0x364F22F4, 0x37665E65, 0x05D6E122, 0x7131EABB, 
+		0x479E9580, 0x98729781, 0x4BD20F8E, 0x1612EE37, 
+		0xCB574ACC, 0x5499B45D, 0x360B4EBA, 0x33814B73, 
+		0x43720ED8, 0x146610F9, 0x45514AA6, 0x0B23BE6F, 
+		0x026E6DA4, 0xD1B9C955, 0x94676F52, 0xCE8EC32B, 
+		0x165EB330, 0x2F6AB971, 0x92F1E8BE, 0xC54095A7, 
+		0xBEB3EB7C, 0x5C9E7D4D, 0x5921A2EA, 0xB45D31E3, 
+		0xB8C9E288, 0x5FE670E9, 0xC02049D6, 0xC42A53DF, 
+		0x6F332454, 0x661BB045, 0x2B3C4982, 0xDF4B779B, 
+		0xD7C4FCE0, 0x70FB1761, 0xADD4CDEE, 0x47BDD917, 
+		0x8C63782C, 0x8181423D, 0xFA05C31A, 0xDD947453, 
+		0x6A8D6238, 0x1A068CD9, 0x4413D506, 0x5374054F, 
+		0xC5A84704, 0xB41B1335, 0x06986FB2, 0x4CCF080B, 
+		0xF80C7290, 0x8622B151, 0x536DBF1E, 0x21E1B887, 
+		0xDED0F0DC, 0xB4B1032D, 0x1D5AAF4A, 0xC56E12C3, 
+		0x8C578DE8, 0xCBA564C9, 0xA67EEC36, 0x0837D2BF, 
+		0x3D98D5B4, 0x1B06F225, 0xFF7EE1E2, 0x3640747B, 
+		0x5E301440, 0x53A08741, 0x436FBC4E, 0xC9C333F7, 
+		0x2727558C, 0x7F5CC01D, 0xFC83677A, 0xAFF10D33, 
+		0x24836598, 0x3161F8B9, 0xDD748F66, 0x5B6CBC2F, 
+		0xAD8FD064, 0x89EE4D15, 0xBBB2A012, 0xA086BCEB, 
+		0x1BEAE1F0, 0x69F39931, 0x764DC57E, 0x17394B67, 
+		0x4D51A63C, 0xF273790D, 0x35A2EBAA, 0x7EE463A3, 
+		0xBC2BE948, 0x2B9B48A9, 0x2FC7BE96, 0x5FC9C19F, 
+		0x3AD83714, 0x6FA02405, 0xDDB6AA42, 0xE648E15B, 
+		0x1DB7DBA0, 0xF55AE721, 0x4D3ADAAE, 0xB3DAFED7, 
+		0x5FFAE2EC, 0x96A42DFD, 0xFB9C3BDA, 0x21CF1613, 
+		0x0F2C18F8, 0xAE705499, 0x650B79C6, 0x31C5E30F, 
+		0x097D09C4, 0xAAAB76F5, 0x34CE0072, 0x27EDE1CB, 
+		0xDAD20150, 0xADD57111, 0xC229FBDE, 0x8AFF4E47, 
+		0x448E0B9C, 0x5C5DDEED, 0x4612580A, 0x05F82483, 
+		0xBC1EF4A8, 0xB1C01C89, 0xF592C0F6, 0x6798207F, 
+		0xEC494874, 0x795F45E5, 0xECFBA2A2, 0xBB9CBE3B, 
+		0xF567104f, 0x47289407, 0x25683fa6, 0x2fde5836, 
+	};
+
+	struct StringPool
+	{
+		char buff[STRING_POOL_SIZE];
+
+		StringPool ()
+		{
+		}
+
+		StringPool (const StringPool &arg)
+		{
+		}
+	};
+	
+/*	
+	class CRCStringLookup: public dgTree<char *, dgUnsigned32>, public dgList<StringPool>
+	{
+	
+		dgInt32 size;
+		char *ptr;
+	
+		void AddContainer ()
+		{
+			StringPool tmp;
+			dgListNode *node;
+	
+//			Addtop (tmp);
+//			node = GetFirst();
+			node = Addtop (tmp);
+
+			ptr = node->GetInfo().buff;
+			size = STRING_POOL_SIZE;
+		}
+	
+		public:
+
+		CRCStringLookup () 
+			:dgTree<char *, dgUnsigned32>(NULL), dgList<StringPool>()
+		{
+			AddContainer();
+		}
+	
+		~CRCStringLookup () 
+		{
+		}
+
+
+		void AddString (dgUnsigned32 crc, const char *string)
+		{
+			dgInt32 length;
+	
+			if (dgTree<char *, dgUnsigned32>::Find(crc)) {
+				return;
+			}
+	
+			length = dgInt32 (strlen (string)) + 1;
+			if (size < length) {
+				AddContainer();
+			}
+	
+			strcpy (ptr, string);
+			Insert (ptr, crc);
+	
+			ptr += length;
+			size -= length;
+		}
+
+		const char* FindString (dgUnsigned32 crc) const
+		{
+			dgTreeNode *node;
+
+			node = dgTree<char *, dgUnsigned32>::Find(crc);
+//			return node ? node->GetInfo() : NULL;
+			return node ? node->GetInfo() : NULL;
+		}
+	};
+
+	static CRCStringLookup& GetDatabase ()
+	{
+		static CRCStringLookup database;
+		return database;
+	}
+*/
+
+
+
+	/*
+	dgUnsigned32 RSHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 b    = 378551;
+	dgUnsigned32 a    = 63689;
+	dgUnsigned32 hash = 0;
+	dgUnsigned32 i    = 0;
+
+	for(i = 0; i < len; str++, i++)	{
+	hash = hash * a + (*str);
+	a    = a * b;
+	}
+
+	return (hash & 0x7FFFFFFF);
+	}
+	// End Of RS Hash Function 
+
+
+	dgUnsigned32 JSHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 hash = 1315423911;
+	dgUnsigned32 i    = 0;
+
+	for(i = 0; i < len; str++, i++)
+	{
+	hash ^= ((hash << 5) + (*str) + (hash >> 2));
+	}
+
+	return (hash & 0x7FFFFFFF);
+	}
+	// End Of JS Hash Function 
+
+
+	dgUnsigned32 PJWHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 BitsInUnsignedInt = (dgUnsigned32)(sizeof(dgUnsigned32) * 8);
+	dgUnsigned32 ThreeQuarters     = (dgUnsigned32)((BitsInUnsignedInt  * 3) / 4);
+	dgUnsigned32 OneEighth         = (dgUnsigned32)(BitsInUnsignedInt / 8);
+	dgUnsigned32 HighBits          = (dgUnsigned32)(0xFFFFFFFF) << (BitsInUnsignedInt - OneEighth);
+	dgUnsigned32 hash              = 0;
+	dgUnsigned32 test              = 0;
+	dgUnsigned32 i                 = 0;
+
+	for(i = 0; i < len; str++, i++)
+	{
+	hash = (hash << OneEighth) + (*str);
+
+	if((test = hash & HighBits)  != 0)
+	{
+	hash = (( hash ^ (test >> ThreeQuarters)) & (~HighBits));
+	}
+	}
+
+	return (hash & 0x7FFFFFFF);
+	}
+	// End Of  P. J. Weinberger Hash Function 
+
+
+	dgUnsigned32 ELFHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 hash = 0;
+	dgUnsigned32 x    = 0;
+	dgUnsigned32 i    = 0;
+
+	for(i = 0; i < len; str++, i++)
+	{
+	hash = (hash << 4) + (*str);
+	if((x = hash & 0xF0000000L) != 0)
+	{
+	hash ^= (x >> 24);
+	hash &= ~x;
+	}
+	}
+
+	return (hash & 0x7FFFFFFF);
+	}
+	// End Of ELF Hash Function 
+
+
+	dgUnsigned32 BKDRHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 seed = 131; // 31 131 1313 13131 131313 etc.. 
+	dgUnsigned32 hash = 0;
+	dgUnsigned32 i    = 0;
+
+	for(i = 0; i < len; str++, i++)
+	{
+	hash = (hash * seed) + (*str);
+	}
+
+	return (hash & 0x7FFFFFFF);
+	}
+	// End Of BKDR Hash Function 
+
+
+	dgUnsigned32 SDBMHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 hash = 0;
+	dgUnsigned32 i    = 0;
+
+	for(i = 0; i < len; str++, i++)
+	{
+	hash = (*str) + (hash << 6) + (hash << 16) - hash;
+	}
+
+	return (hash & 0x7FFFFFFF);
+	}
+	// End Of SDBM Hash Function 
+
+
+	dgUnsigned32 DEKHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 hash = len;
+	dgUnsigned32 i    = 0;
+
+	for(i = 0; i < len; str++, i++)
+	{
+	hash = ((hash << 5) ^ (hash >> 27)) ^ (*str);
+	}
+	return (hash & 0x7FFFFFFF);
+	}
+	// End Of DEK Hash Function 
+
+
+	dgUnsigned32 APHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 hash = 0;
+	dgUnsigned32 i    = 0;
+
+	for(i = 0; i < len; str++, i++)
+	{
+	hash ^= ((i & 1) == 0) ? (  (hash <<  7) ^ (*str) ^ (hash >> 3)) :
+	(~((hash << 11) ^ (*str) ^ (hash >> 5)));
+	}
+
+	return (hash & 0x7FFFFFFF);
+	}
+	*/
+
+	// End Of DJB Hash Function 
+	dgUnsigned32 DJBHash(const char* const str, dgInt32 len)
+	{
+		dgUnsigned32 hash = 5381;
+		for(dgInt32 i = 0; i < len; i++)
+		{
+			//hash = ((hash << 5) + hash) + (*str);
+			hash = ((hash << 5) + hash) + str[i];
+		}
+
+		return (hash & 0x7FFFFFFF);
+	}
+}	
+
+
+dgUnsigned32 dgApi dgCRC (const char* const name)
+{
+//	dgUnsigned32 c;
+//	dgUnsigned32 crc;
+//	unsigned char *ptr;
+//	dgUnsigned32 val;
+	if (!name) {
+		return 0;
+	}
+
+	dgUnsigned32 crc = 0;
+	//for (ptr = (unsigned char*)name; *ptr; ptr ++) {
+	for (dgInt32 i = 0; name[i]; i ++) {
+		char c = name[i];
+		dgUnsigned32 val = InternalCRC::randBits[((crc >> 24) ^ c) & 0xff];
+		crc = (crc << 8) ^ val;
+	}
+
+	dgAssert (0);
+//	InternalCRC::GetDatabase().AddString (crc, name);
+	return crc;
+}
+
+
+
+dgUnsigned32 dgApi dgCRC (const void* const buffer, dgInt32 size, dgUnsigned32 crcAcc)
+{
+	dgAssert (buffer);
+	unsigned char* const ptr = (unsigned char*)buffer;
+	for (dgInt32 i = 0; i < size; i ++) {
+		dgUnsigned32 c = ptr[i];
+		dgUnsigned32 val = InternalCRC::randBits[((crcAcc >> 24) ^ c) & 0xff];
+		crcAcc = (crcAcc << 8) ^ val;
+	}
+	return crcAcc;
+}
+
+
+const char* dgApi dgInverseCRC (dgUnsigned32 crc)
+{	
+	dgAssert (0);
+	return NULL;
+//	return InternalCRC::GetDatabase().FindString (crc);
+}
+	
+
+
+dgUnsigned32 dgApi dgHash (const void* const string, int size)
+{
+	return InternalCRC::DJBHash ((char*)string, size);
+}

thirdparty/src/newton/dgCore/dgCRC.h

@@ -0,0 +1,35 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgCRC__
+#define __dgCRC__
+
+#include "dgStdafx.h"
+
+
+dgUnsigned32 dgApi dgCRC (const char* const string);
+dgUnsigned32 dgApi dgCRC (const void* const buffer, dgInt32 size, dgUnsigned32 crcAcc = 0);
+const char* dgApi dgInverseCRC (dgUnsigned32 CRC);
+
+dgUnsigned32 dgApi dgHash (const void* const string, dgInt32 size);
+
+#endif
+

thirdparty/src/newton/dgCore/dgConvexHull3d.h

@@ -0,0 +1,136 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DG_CONVEXHULL_3D__
+#define __DG_CONVEXHULL_3D__
+
+#include "dgStdafx.h"
+#include "dgList.h"
+#include "dgArray.h"
+#include "dgPlane.h"
+#include "dgVector.h"
+#include "dgMatrix.h"
+#include "dgQuaternion.h"
+
+#define DG_OLD_CONVEXHULL_3D
+
+class dgMemoryAllocator;
+class dgConvexHull3DVertex;
+class dgConvexHull3dAABBTreeNode;
+
+class dgConvexHull3DFace
+{
+	public:
+	dgConvexHull3DFace();
+	
+	void SetMark(dgInt32 mark) {m_mark = mark;}
+	dgInt32 GetMark() const {return m_mark;}
+	dgList<dgConvexHull3DFace>::dgListNode* GetTwin(dgInt32 index) const { return m_twin[index];}
+
+	private:
+	dgFloat64 Evalue (const dgBigVector* const pointArray, const dgBigVector& point) const;
+	dgBigPlane GetPlaneEquation (const dgBigVector* const pointArray) const;
+
+	public:
+	dgInt32 m_index[3]; 
+	private:
+	dgInt32 m_mark;
+	dgList<dgConvexHull3DFace>::dgListNode* m_twin[3];
+	friend class dgConvexHull3d;
+};
+
+class dgConvexHull3d: public dgList<dgConvexHull3DFace>
+{
+#ifdef	DG_OLD_CONVEXHULL_3D
+	class dgNormalMap;
+#endif
+
+	public:
+	dgConvexHull3d(const dgConvexHull3d& source);
+	dgConvexHull3d(dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount = 0x7fffffff);
+	virtual ~dgConvexHull3d();
+
+	dgInt32 GetVertexCount() const;
+	const dgBigVector* GetVertexPool() const;
+	const dgBigVector& GetVertex(dgInt32 i) const;
+
+	dgFloat64 GetDiagonal() const;
+	void GetAABB (dgBigVector& boxP0, dgBigVector& boxP1) const;
+	dgFloat64 RayCast (const dgBigVector& localP0, const dgBigVector& localP1) const;
+	void CalculateVolumeAndSurfaceArea (dgFloat64& volume, dgFloat64& surcafeArea) const;
+
+	protected:
+	dgConvexHull3d(dgMemoryAllocator* const allocator);
+	void BuildHull (const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount);
+
+	virtual dgListNode* AddFace (dgInt32 i0, dgInt32 i1, dgInt32 i2);
+	virtual void DeleteFace (dgListNode* const node) ;
+	virtual dgInt32 InitVertexArray(dgConvexHull3DVertex* const points, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize);
+
+	bool CheckFlatSurface(dgConvexHull3dAABBTreeNode* vertexTree, dgConvexHull3DVertex* const points, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount);
+	void CalculateConvexHull2d (dgConvexHull3dAABBTreeNode* vertexTree, dgConvexHull3DVertex* const points, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount);
+	void CalculateConvexHull3d (dgConvexHull3dAABBTreeNode* vertexTree, dgConvexHull3DVertex* const points, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount);
+	
+	dgInt32 SupportVertex (dgConvexHull3dAABBTreeNode** const tree, const dgConvexHull3DVertex* const points, const dgBigVector& dir, const bool removeEntry = true) const;
+	dgFloat64 TetrahedrumVolume (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3) const;
+
+	dgInt32 GetUniquePoints(dgConvexHull3DVertex* const points, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize);
+	dgConvexHull3dAABBTreeNode* BuildTree (dgConvexHull3dAABBTreeNode* const parent, dgConvexHull3DVertex* const points, dgInt32 count, dgInt32 baseIndex, dgInt8** const memoryPool, dgInt32& maxMemSize) const;
+	static dgInt32 ConvexCompareVertex(const dgConvexHull3DVertex* const A, const dgConvexHull3DVertex* const B, void* const context);
+	bool Sanity() const;
+	void Save (const char* const filename) const;
+
+	dgInt32 m_count;
+	dgFloat64 m_diag;
+	dgBigVector m_aabbP0;
+	dgBigVector m_aabbP1;
+	dgArray<dgBigVector> m_points;
+};
+
+
+inline dgInt32 dgConvexHull3d::GetVertexCount() const
+{
+	return m_count;
+}
+
+inline const dgBigVector* dgConvexHull3d::GetVertexPool() const
+{
+	return &m_points[0];
+}
+
+inline const dgBigVector& dgConvexHull3d::GetVertex(dgInt32 index) const
+{
+	return m_points[index];
+}
+
+inline dgFloat64 dgConvexHull3d::GetDiagonal() const
+{
+	return m_diag;
+}
+
+
+inline void dgConvexHull3d::GetAABB (dgBigVector& boxP0, dgBigVector& boxP1) const
+{
+	boxP0 = m_aabbP0;
+	boxP1 = m_aabbP1;
+}
+
+#endif

thirdparty/src/newton/dgCore/dgConvexHull4d.h

@@ -0,0 +1,193 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DG_CONVEXHULL_4D__
+#define __DG_CONVEXHULL_4D__
+
+#include "dgStdafx.h"
+#include "dgList.h"
+#include "dgArray.h"
+#include "dgPlane.h"
+#include "dgVector.h"
+#include "dgMatrix.h"
+#include "dgQuaternion.h"
+
+class dgMemoryAllocator;
+class dgConvexHull4dAABBTreeNode;
+
+class dgConvexHull4dVector: public dgBigVector
+{
+	public:
+	void operator = (const dgBigVector& a) 
+	{
+		m_x = a.m_x;
+		m_y = a.m_y;
+		m_z = a.m_z;
+		m_w = a.m_w;
+		m_index = 0;
+		m_mark = 0;
+	}
+
+	dgInt32 m_index;
+	dgInt32 m_mark;
+};
+
+
+class dgConvexHull4dTetraherum
+{
+	public:
+	class dgTetrahedrumFace 
+	{
+		public:
+		dgInt32 m_index[4];
+		dgList<dgConvexHull4dTetraherum>::dgListNode* m_twin;
+	};
+
+
+	class dgTetrahedrumPlane: public dgBigVector
+	{
+		public: 
+		dgTetrahedrumPlane (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3);
+		dgFloat64 Evalue (const dgBigVector& point) const;
+		dgFloat64 m_dist;
+	};
+
+	dgConvexHull4dTetraherum();
+	dgTetrahedrumPlane GetPlaneEquation (const dgConvexHull4dVector* const points) const;
+	dgFloat64 GetTetraVolume (const dgConvexHull4dVector* const pointArray) const;	
+	dgBigVector CircumSphereCenter (const dgConvexHull4dVector* const pointArray) const;
+	dgFloat64 Evalue (const dgConvexHull4dVector* const pointArray, const dgBigVector& point) const;
+
+	dgInt32 GetMark() const { return m_mark; }
+	void SetMark(dgInt32 mark) { m_mark = mark; }
+
+	private:
+	void Init (const dgConvexHull4dVector* const points, dgInt32 v0, dgInt32 v1, dgInt32 v2, dgInt32 v3);
+
+	public:
+	dgTetrahedrumFace m_faces[4];
+	dgInt32 m_mark;
+	dgInt32 m_uniqueID;
+
+#ifdef _DEBUG
+	dgInt32 m_debugID;
+#endif
+	friend class dgConvexHull4d;
+	friend class dgDelaunayTetrahedralization;
+};
+
+
+class dgConvexHull4d: public dgList<dgConvexHull4dTetraherum>
+{
+	public:
+	dgConvexHull4d(dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol);
+	virtual ~dgConvexHull4d();
+
+	dgInt32 GetVertexCount() const;
+	dgInt32 GetVertexIndex(dgInt32 i) const;
+	const dgBigVector& GetVertex(dgInt32 i) const;
+
+	const dgConvexHull4dVector* GetHullVertexArray() const;
+	dgFloat64 GetTetraVolume (const dgConvexHull4dTetraherum* const tetra) const;
+
+	dgInt32 IncMark (); 
+	void Save (const char* const filename) const;
+
+	protected:
+	dgConvexHull4d(dgMemoryAllocator* const allocator);
+
+	void BuildHull (dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol);
+
+	virtual dgInt32 AddVertex (const dgBigVector& vertex);
+	virtual dgInt32 InitVertexArray(dgConvexHull4dVector* const points, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize);
+	
+	virtual dgListNode* AddFace (dgInt32 i0, dgInt32 i1, dgInt32 i2, dgInt32 i3);
+	virtual void DeleteFace (dgListNode* const node);
+
+	dgListNode* FindFacingNode(const dgBigVector& vertex);
+	
+	void InsertNewVertex(dgInt32 vertexIndex, dgListNode* const frontFace, dgList<dgListNode*>& deletedFaces, dgList<dgListNode*>& newFaces);
+	dgInt32 SupportVertex (dgConvexHull4dAABBTreeNode** const tree, const dgConvexHull4dVector* const points, const dgBigVector& dir, const bool removeEntry = true) const;
+	
+	void CalculateConvexHull (dgConvexHull4dAABBTreeNode* vertexTree, dgConvexHull4dVector* const points, dgInt32 count, dgFloat64 distTol);
+	void LinkSibling (dgListNode* node0, dgListNode* node1)	const;
+	bool Sanity() const;
+	dgConvexHull4dAABBTreeNode* BuildTree (dgConvexHull4dAABBTreeNode* const parent, dgConvexHull4dVector* const points, dgInt32 count, dgInt32 baseIndex, dgInt8** const memoryPool, dgInt32& maxMemSize) const;
+
+	static dgInt32 ConvexCompareVertex(const dgConvexHull4dVector* const  A, const dgConvexHull4dVector* const B, void* const context);
+	
+	class dgNormalMap
+	{
+		public:
+		dgNormalMap();
+		private:
+		void TessellateTriangle(dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgBigVector* const buffer, dgInt32& count);
+
+		dgBigVector m_normal[1024];
+		dgInt32 m_count;
+		friend class dgConvexHull4d; 
+	};
+	static const dgNormalMap& GetNormaMap();
+
+	dgInt32 m_mark;
+	dgInt32 m_count;
+	dgFloat64 m_diag;
+	dgArray<dgConvexHull4dVector> m_points;
+};
+
+
+inline dgInt32 dgConvexHull4d::IncMark ()
+{
+	m_mark ++;
+	return m_mark;
+}
+
+inline dgInt32 dgConvexHull4d::GetVertexCount() const
+{
+	return m_count;
+}
+
+inline dgInt32 dgConvexHull4d::GetVertexIndex(dgInt32 index) const
+{
+	dgAssert (index >= 0);
+	dgAssert (index < m_count);
+	return m_points[index].m_index;
+}
+
+
+inline const dgBigVector& dgConvexHull4d::GetVertex(dgInt32 index) const
+{
+	dgAssert (index >= 0);
+	dgAssert (index < m_count);
+	return m_points[index];
+}
+
+inline const dgConvexHull4dVector* dgConvexHull4d::GetHullVertexArray() const
+{
+	return &m_points[0];
+}
+
+inline dgFloat64 dgConvexHull4d::GetTetraVolume (const dgConvexHull4dTetraherum* const tetra) const
+{
+	return tetra->GetTetraVolume (&m_points[0]);
+}
+
+#endif

thirdparty/src/newton/dgCore/dgDebug.cpp

@@ -0,0 +1,26 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgDebug.h"
+
+
+

thirdparty/src/newton/dgCore/dgDebug.h

@@ -0,0 +1,75 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgDebug__
+#define __dgDebug__
+
+#include "dgStdafx.h"
+
+
+	inline void dgApi dgExpandTraceMessage(const char *fmt, ...)
+	{
+		va_list v_args;
+		char text[4096];
+
+		text[0] = 0;
+		va_start(v_args, fmt);
+		vsprintf(text, fmt, v_args);
+		va_end(v_args);
+
+	#if defined (_WIN_32_VER) || defined (_WIN_64_VER)
+		OutputDebugStringA(text);
+	#else 
+		printf("%s\n", text);
+	#endif
+	}
+
+#ifdef _MSC_VER 
+	#ifdef _DEBUG 
+		#define DG_TRACE
+	#endif
+#endif
+
+
+#ifdef DG_TRACE
+	#define dgTrace(x)	dgExpandTraceMessage x;
+#else
+	#define dgTrace(x);
+#endif
+
+
+#ifdef _DEBUG
+	inline void TraceFuntionName (const char *name)
+	{
+		//	static int trace;
+		//	dgTrace (("%d %s\n", trace, name));
+		dgTrace (("%s\n", name));
+	}
+
+	//#define TRACE_FUNCTION(name) TraceFuntionName (name)
+	#define TRACE_FUNCTION(name)
+#else
+	#define TRACE_FUNCTION(name)
+#endif
+
+	
+#endif
+

thirdparty/src/newton/dgCore/dgDelaunayTetrahedralization.cpp

@@ -0,0 +1,178 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgSort.h"
+#include "dgStack.h"
+#include "dgGoogol.h"
+#include "dgSmallDeterminant.h"
+#include "dgDelaunayTetrahedralization.h"
+
+dgDelaunayTetrahedralization::dgDelaunayTetrahedralization(dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 count, dgInt32 strideInByte, dgFloat64 distTol)
+	:dgConvexHull4d(allocator)
+{
+	dgSetPrecisionDouble precision;
+	dgStack<dgBigVector> pool(count + 2);
+
+	dgBigVector* const points = &pool[0];
+	dgInt32 stride = dgInt32 (strideInByte / sizeof (dgFloat64));
+	for (dgInt32 i = 0; i < count; i ++) {
+		dgFloat64 x = dgRoundToFloat (vertexCloud[i * stride + 0]);
+		dgFloat64 y = dgRoundToFloat (vertexCloud[i * stride + 1]);
+		dgFloat64 z = dgRoundToFloat (vertexCloud[i * stride + 2]);
+		points[i] = dgBigVector (x, y, z, x * x + y * y + z * z);
+	}
+
+	dgInt32 oldCount = count;
+	BuildHull (allocator, &pool[0].m_x, sizeof (dgBigVector), count, distTol);
+	if (oldCount > m_count) {
+		// the mesh is convex, need to add two steiners point to make tractable
+		dgBigVector origin (dgFloat64 (0.0f));
+		dgFloat64 maxW = dgFloat64 (-1.0e20f);
+		for (dgInt32 i = 0; i < count; i++) {
+			dgFloat64 x = dgRoundToFloat(vertexCloud[i * stride + 0]);
+			dgFloat64 y = dgRoundToFloat(vertexCloud[i * stride + 1]);
+			dgFloat64 z = dgRoundToFloat(vertexCloud[i * stride + 2]);
+			points[i] = dgBigVector(x, y, z, x * x + y * y + z * z);
+			origin += points[i];
+			maxW = dgMax (points[i].m_w, maxW);
+		}
+		origin = origin.Scale (dgFloat64 (1.0f) / count);
+		points[count + 0] = origin;
+		points[count + 1] = origin;
+		points[count + 0].m_w += dgFloat64 (1.0f);
+		points[count + 1].m_w -= dgFloat64 (1.0f);
+ 		BuildHull (allocator, &pool[0].m_x, sizeof (dgBigVector), count + 2, distTol);
+	}
+	if (oldCount > m_count) {
+		// this is probably a regular convex solid, which will have a zero volume hull
+		// add the rest of the points by incremental insertion with small perturbation
+		dgInt32 hullCount = m_count;
+		
+		for (dgInt32 i = 0; i < count; i ++) {
+			bool inHull = false;
+			const dgConvexHull4dVector* const hullPoints = &m_points[0];
+			for (dgInt32 j = 0; j < hullCount; j ++) {
+				if (hullPoints[j].m_index == i) {
+					inHull = true;
+					break;
+				}			
+			}
+			if (!inHull) {
+				dgBigVector q (points[i]);
+				dgInt32 index = AddVertex(q);
+				if (index == -1) {
+					q.m_x += dgFloat64 (1.0e-3f);
+					q.m_y += dgFloat64 (1.0e-3f);
+					q.m_z += dgFloat64 (1.0e-3f);
+					index = AddVertex(q);
+					dgAssert (index != -1);
+				}
+				dgAssert (index != -1);
+				m_points[index].m_index = i;
+			}
+		}
+	}
+
+	SortVertexArray ();
+}
+
+dgDelaunayTetrahedralization::~dgDelaunayTetrahedralization()
+{
+}
+
+dgInt32 dgDelaunayTetrahedralization::AddVertex (const dgBigVector& vertex)
+{
+	dgSetPrecisionDouble precision;
+
+	dgBigVector p (vertex);
+	dgAssert(p.m_w == dgFloat32(0.0f));
+	p.m_w = p.DotProduct(p).GetScalar();
+	dgInt32 index = dgConvexHull4d::AddVertex(p);
+
+	return index;
+}
+
+dgInt32 dgDelaunayTetrahedralization::CompareVertexByIndex(const dgConvexHull4dVector* const  A, const dgConvexHull4dVector* const B, void* const context)
+{
+	if (A->m_index < B ->m_index) {
+		return -1;
+	} else if (A->m_index > B->m_index) {
+		return 1;
+	}
+	return 0;
+}
+
+void dgDelaunayTetrahedralization::SortVertexArray ()
+{
+	dgConvexHull4dVector* const points = &m_points[0];
+	for (dgListNode* node = GetFirst(); node; node = node->GetNext()) {	
+		dgConvexHull4dTetraherum* const tetra = &node->GetInfo();
+		for (dgInt32 i = 0; i < 4; i ++) {
+			dgConvexHull4dTetraherum::dgTetrahedrumFace& face = tetra->m_faces[i];
+			for (dgInt32 j = 0; j < 4; j ++) {
+				dgInt32 index = face.m_index[j];
+				face.m_index[j] = points[index].m_index;
+			}
+		}
+	}
+
+	dgSort(points, m_count, CompareVertexByIndex);
+}
+
+void dgDelaunayTetrahedralization::RemoveUpperHull ()
+{
+	dgSetPrecisionDouble precision;
+
+	dgListNode* nextNode = NULL;
+	for (dgListNode* node = GetFirst(); node; node = nextNode) {
+		nextNode = node->GetNext();
+
+		dgConvexHull4dTetraherum* const tetra = &node->GetInfo();
+		tetra->SetMark(0);
+		dgFloat64 w = GetTetraVolume (tetra);
+		if (w >= dgFloat64 (0.0f)) {
+			DeleteFace(node);
+		}
+	}
+}
+
+void dgDelaunayTetrahedralization::DeleteFace (dgListNode* const node)
+{
+	dgConvexHull4dTetraherum* const tetra = &node->GetInfo();
+	for (dgInt32 i = 0; i < 4; i ++) {
+		dgListNode* const twinNode = tetra->m_faces[i].m_twin;
+		if (twinNode) {
+			dgConvexHull4dTetraherum* const twinTetra = &twinNode->GetInfo();
+			for (dgInt32 j = 0; j < 4; j ++) {
+				if (twinTetra->m_faces[j].m_twin == node) {
+					twinTetra->m_faces[j].m_twin = NULL;
+					break;
+				}
+			}
+		}
+	}
+	dgConvexHull4d::DeleteFace (node);
+}
+
+
+
+

thirdparty/src/newton/dgCore/dgDelaunayTetrahedralization.h

@@ -0,0 +1,45 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DG_DEALUNAY_TETRAHEDRALIZAION_4D__
+#define __DG_DEALUNAY_TETRAHEDRALIZAION_4D__
+
+#include "dgStdafx.h"
+#include "dgConvexHull4d.h"
+
+class dgDelaunayTetrahedralization: public dgConvexHull4d
+{
+	public:
+	dgDelaunayTetrahedralization(dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 count, dgInt32 strideInByte, dgFloat64 distTol);
+	virtual ~dgDelaunayTetrahedralization();
+	void RemoveUpperHull ();
+
+	dgInt32 AddVertex (const dgBigVector& vertex);
+
+	protected:
+	virtual void DeleteFace (dgListNode* const node) ;
+
+	void SortVertexArray ();
+	static dgInt32 CompareVertexByIndex(const dgConvexHull4dVector* const  A, const dgConvexHull4dVector* const B, void* const context);
+
+};
+
+#endif

thirdparty/src/newton/dgCore/dgFastQueue.h

@@ -0,0 +1,113 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+/****************************************************************************
+*
+*  Visual C++ 6.0 created by: Julio Jerez
+*
+****************************************************************************/
+#ifndef __dgFastQueue__
+#define __dgFastQueue__
+
+#include "dgStdafx.h"
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+class dgFastQueue
+{
+	public:
+    DG_CLASS_ALLOCATOR_NEW(allocator)
+
+	dgFastQueue (dgMemoryAllocator* const allocator);
+	~dgFastQueue ();
+
+	bool IsEmpty() const;
+	bool IsFull() const;
+	const T& GetHead() const;
+	void Pop();
+	void Push(T& object);
+
+	private:
+	T* m_pool;
+    dgMemoryAllocator* m_allocator;
+	dgInt32 m_head;
+	dgInt32 m_tail;
+};
+
+
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+dgFastQueue<T, sizeInPowerOfTwo>::dgFastQueue (dgMemoryAllocator* const allocator)
+    :m_allocator(allocator)
+	,m_head(0)
+	,m_tail(0)
+{
+	dgAssert (((sizeInPowerOfTwo -1) & (-sizeInPowerOfTwo)) == 0);
+    m_pool = (T*) m_allocator->MallocLow(sizeInPowerOfTwo * sizeof (T));
+}
+
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+dgFastQueue<T, sizeInPowerOfTwo>::~dgFastQueue ()
+{
+    m_allocator->FreeLow(m_pool); 
+}
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+bool dgFastQueue<T, sizeInPowerOfTwo>::IsEmpty() const
+{
+	return (m_head == m_tail);
+}
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+bool dgFastQueue<T, sizeInPowerOfTwo>::IsFull() const
+{
+	return (((m_tail + 1) & (sizeInPowerOfTwo - 1)) == m_head);
+}
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+const T& dgFastQueue<T, sizeInPowerOfTwo>::GetHead() const
+{
+	dgAssert (!IsEmpty());
+	return m_pool[m_head];
+}
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+void dgFastQueue<T, sizeInPowerOfTwo>::Pop()
+{
+	dgAssert (!IsEmpty());
+	m_head = (m_head + 1) & (sizeInPowerOfTwo - 1);
+}
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+void dgFastQueue<T, sizeInPowerOfTwo>::Push(T& object)
+{
+	dgAssert (!IsFull());
+	m_pool[m_tail] = object; 
+	m_tail = (m_tail + 1) & (sizeInPowerOfTwo - 1);
+}
+
+
+
+#endif
+
+
+
+

thirdparty/src/newton/dgCore/dgGeneralMatrix.cpp

@@ -0,0 +1,66 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgGeneralMatrix.h"
+#include "dgStack.h"
+#include "dgMemory.h"
+
+/*
+DG_INLINE bool dgCholeskyFactorizationAddRow(dgInt32 size, dgInt32 n, dgFloat32* const matrix, dgInt32 rowStride)
+{
+	dgFloat32* const rowN = &matrix[rowStride * n];
+
+	dgInt32 stride = 0;
+	for (dgInt32 j = 0; j <= n; j++) {
+		dgFloat32 s = dgFloat32(0.0f);
+		dgFloat32* const rowJ = &matrix[stride];
+		for (dgInt32 k = 0; k < j; k++) {
+			s += rowN[k] * rowJ[k];
+		}
+
+		if (n == j) {
+			dgFloat32 diag = rowN[n] - s;
+			if (diag < dgFloat32(dgFloat32(1.0e-6f))) {
+				return false;
+			}
+
+			rowN[n] = dgFloat32(sqrt(diag));
+		} else {
+			rowN[j] = (rowN[j] - s) / rowJ[j];
+		}
+
+		stride += rowStride;
+	}
+
+	return true;
+}
+
+
+bool dgCholeskyFactorization(dgInt32 size, dgFloat32* const psdMatrix, dgInt32 rowStride)
+{
+	bool state = true;
+	for (dgInt32 i = 0; (i < size) && state; i++) {
+		state = state && dgCholeskyFactorizationAddRow(size, i, psdMatrix, rowStride);
+	}
+	return state;
+}
+*/

thirdparty/src/newton/dgCore/dgGeneralVector.cpp

@@ -0,0 +1,25 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgGeneralVector.h"
+
+

thirdparty/src/newton/dgCore/dgGeneralVector.h

@@ -0,0 +1,89 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+*
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+*
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+*
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+*
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgGeneralVector__
+#define __dgGeneralVector__
+
+#include "dgStdafx.h"
+#include "dgDebug.h"
+#include "dgMemory.h"
+
+template <class T>
+DG_INLINE T dgSQRH(const T num, const T den)
+{
+	T r(num / den);
+	return T(sqrt(T(1.0f) + r * r));
+}
+
+template <class T>
+DG_INLINE T dgPythag(const T a, const T b)
+{
+	T absa(dgAbs(a));
+	T absb(dgAbs(b));
+	return (absa > absb) ? (absa * dgSQRH(absb, absa)) : ((absb == T(0.0f) ? T(0.0f) : (absb * dgSQRH(absa, absb))));
+}
+
+template <class T>
+DG_INLINE T dgSign(const T a, const T b)
+{
+	return (b >= T(0.0f)) ? (a >= T(0.0f) ? a : -a) : (a >= T(0.0f) ? -a : a);
+}
+
+
+// return dot product
+template<class T>
+DG_INLINE T dgDotProduct(dgInt32 size, const T* const A, const T* const B)
+{
+	T val(0.0f);
+	for (dgInt32 i = 0; i < size; i++) {
+		val = val + A[i] * B[i];
+	}
+	return val;
+}
+
+template<class T>
+DG_INLINE void dgAdd(dgInt32 size, T* const X, const T* const A, const T* const B)
+{
+	for (dgInt32 i = 0; i < size; i++) {
+		X[i] = A[i] + B[i];
+	}
+}
+
+template<class T>
+DG_INLINE void dgSub(dgInt32 size, T* const X, const T* const A, const T* const B)
+{
+	for (dgInt32 i = 0; i < size; i++) {
+		X[i] = A[i] - B[i];
+	}
+}
+
+template<class T>
+DG_INLINE void dgMulAdd(dgInt32 size, T* const X, const T* const A, const T* const B, T C)
+{
+	for (dgInt32 i = 0; i < size; i++) {
+		X[i] = A[i] + B[i] * C;
+	}
+}
+
+
+#endif
+
+

thirdparty/src/newton/dgCore/dgGoogol.cpp

@@ -0,0 +1,584 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "dgStdafx.h"
+#include "dgGoogol.h"
+
+dgGoogol dgGoogol::m_zero(0.0); 
+dgGoogol dgGoogol::m_one(1.0); 
+dgGoogol dgGoogol::m_two(2.0);  
+dgGoogol dgGoogol::m_three(3.0);   
+dgGoogol dgGoogol::m_half(0.5);   
+
+#ifdef _IMPLEMENT_USING_INTEGER_ARITHMETIC_
+
+	dgGoogol::dgGoogol(void)
+		:m_sign(0)
+		,m_exponent(0)
+	{
+		memset (m_mantissa, 0, sizeof (m_mantissa));
+	}
+
+	dgGoogol::dgGoogol(dgFloat64 value)
+		:m_sign(0)
+		,m_exponent(0)
+	{
+		dgInt32 exp;
+		dgFloat64 mantissa = fabs (frexp(value, &exp));
+
+		m_exponent = dgInt16 (exp);
+		m_sign = (value >= 0) ? 0 : 1;
+
+		memset (m_mantissa, 0, sizeof (m_mantissa));
+		m_mantissa[0] = dgUnsigned64 (dgFloat64 (dgUnsigned64(1)<<62) * mantissa);
+
+		// it looks like GCC have problems with this
+		//dgAssert (m_mantissa[0] >= 0);
+		dgAssert ((m_mantissa[0] & dgUnsigned64(1)<<63) == 0);
+	}
+
+	void dgGoogol::CopySignedMantissa (dgUnsigned64* const mantissa) const
+	{
+		memcpy (mantissa, m_mantissa, sizeof (m_mantissa));
+		if (m_sign) {
+			NegateMantissa (mantissa);
+		}
+	}
+
+	dgGoogol::operator double() const
+	{
+		dgFloat64 mantissa = (dgFloat64(1.0f) / dgFloat64 (dgUnsigned64(1)<<62)) * dgFloat64 (m_mantissa[0]);
+		mantissa = ldexp(mantissa, m_exponent) * (m_sign ?  dgFloat64 (-1.0f) : dgFloat64 (1.0f));
+		return mantissa;
+	}
+
+	dgGoogol dgGoogol::operator+ (const dgGoogol &A) const
+	{
+		dgGoogol tmp;
+		dgAssert (dgInt64 (m_mantissa[0]) >= 0);
+		dgAssert (dgInt64 (A.m_mantissa[0]) >= 0);
+
+		if (m_mantissa[0] && A.m_mantissa[0]) {
+			dgUnsigned64 mantissa0[DG_GOOGOL_SIZE];
+			dgUnsigned64 mantissa1[DG_GOOGOL_SIZE];
+			dgUnsigned64 mantissa[DG_GOOGOL_SIZE];
+
+			CopySignedMantissa (mantissa0);
+			A.CopySignedMantissa (mantissa1);
+
+			dgInt32 exponetDiff = m_exponent - A.m_exponent;
+			dgInt32 exponent = m_exponent;
+			if (exponetDiff > 0) {
+				ShiftRightMantissa (mantissa1, exponetDiff);
+			} else if (exponetDiff < 0) {
+				exponent = A.m_exponent;
+				ShiftRightMantissa (mantissa0, -exponetDiff);
+			} 
+
+			dgUnsigned64 carrier = 0;
+			for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) {
+				dgUnsigned64 m0 = mantissa0[i];
+				dgUnsigned64 m1 = mantissa1[i];
+				mantissa[i] = m0 + m1 + carrier;
+				carrier = CheckCarrier (m0, m1) | CheckCarrier (m0 + m1, carrier);
+			}
+
+			dgInt8 sign = 0;
+			if (dgInt64 (mantissa[0]) < 0) {
+				sign = 1;
+				NegateMantissa (mantissa);
+			}
+
+			dgInt32 bits = NormalizeMantissa (mantissa);
+			if (bits <= (-64 * DG_GOOGOL_SIZE)) {
+				tmp.m_sign = 0;
+				tmp.m_exponent = 0;
+			} else {
+				tmp.m_sign = sign;
+				tmp.m_exponent =  dgInt16 (exponent + bits);
+			}
+
+			memcpy (tmp.m_mantissa, mantissa, sizeof (m_mantissa));
+
+
+		} else if (A.m_mantissa[0]) {
+			tmp = A;
+		} else {
+			tmp = *this;
+		}
+
+		dgAssert (dgInt64 (tmp.m_mantissa[0]) >= 0);
+		return tmp;
+	}
+
+	dgGoogol dgGoogol::operator- (const dgGoogol &A) const
+	{
+		dgGoogol tmp (A);
+		tmp.m_sign = !tmp.m_sign;
+		return *this + tmp;
+	}
+
+	void dgGoogol::ScaleMantissa (dgUnsigned64* const dst, dgUnsigned64 scale) const
+	{
+		dgUnsigned64 carrier = 0;
+		for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) {
+			if (m_mantissa[i]) {
+				dgUnsigned64 low;
+				dgUnsigned64 high;
+				ExtendeMultiply (scale, m_mantissa[i], high, low);
+				dgUnsigned64 acc = low + carrier;
+				carrier = CheckCarrier (low, carrier);	
+				dgAssert (CheckCarrier (carrier, high) == 0);
+				carrier += high;
+				dst[i + 1] = acc;
+			} else {
+				dst[i + 1] = carrier;
+				carrier = 0;
+			}
+
+		}
+		dst[0] = carrier;
+	}
+
+	dgGoogol dgGoogol::operator* (const dgGoogol &A) const
+	{
+		dgAssert (dgInt64 (m_mantissa[0]) >= 0);
+		dgAssert (dgInt64 (A.m_mantissa[0]) >= 0);
+
+		if (m_mantissa[0] && A.m_mantissa[0]) {
+			dgUnsigned64 mantissaAcc[DG_GOOGOL_SIZE * 2];
+			memset (mantissaAcc, 0, sizeof (mantissaAcc));
+			for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) {
+				dgUnsigned64 a = m_mantissa[i];
+				if (a) {
+					dgUnsigned64 mantissaScale[2 * DG_GOOGOL_SIZE];
+					memset (mantissaScale, 0, sizeof (mantissaScale));
+					A.ScaleMantissa (&mantissaScale[i], a);
+
+					dgUnsigned64 carrier = 0;
+					for (dgInt32 j = 0; j < 2 * DG_GOOGOL_SIZE; j ++) {
+						const dgInt32 k = 2 * DG_GOOGOL_SIZE - 1 - j;
+						dgUnsigned64 m0 = mantissaAcc[k];
+						dgUnsigned64 m1 = mantissaScale[k];
+						mantissaAcc[k] = m0 + m1 + carrier;
+						carrier = CheckCarrier (m0, m1) | CheckCarrier (m0 + m1, carrier);
+					}
+				}
+			}
+
+			dgUnsigned64 carrier = 0;
+			//dgInt32 bits = dgUnsigned64(LeadingZeros (mantissaAcc[0]) - 2);
+			dgInt32 bits = LeadingZeros (mantissaAcc[0]) - 2;
+			for (dgInt32 i = 0; i < 2 * DG_GOOGOL_SIZE; i ++) {
+				const dgInt32 k = 2 * DG_GOOGOL_SIZE - 1 - i;
+				dgUnsigned64 a = mantissaAcc[k];
+				mantissaAcc[k] = (a << dgUnsigned64(bits)) | carrier;
+				carrier = a >> dgUnsigned64(64 - bits);
+			}
+
+			dgInt32 exp = m_exponent + A.m_exponent - (bits - 2);
+
+			dgGoogol tmp;
+			tmp.m_sign = m_sign ^ A.m_sign;
+			tmp.m_exponent = dgInt16 (exp);
+			memcpy (tmp.m_mantissa, mantissaAcc, sizeof (m_mantissa));
+
+			return tmp;
+		} 
+		return dgGoogol(0.0);
+	}
+
+	dgGoogol dgGoogol::operator/ (const dgGoogol &A) const
+	{
+		dgGoogol tmp (1.0 / A);
+		tmp = tmp * (m_two - A * tmp);
+		tmp = tmp * (m_two - A * tmp);
+		int test = 0;
+		dgInt32 passes = 0;
+		do {
+			passes ++;
+			dgGoogol tmp0 (tmp);
+			tmp = tmp * (m_two - A * tmp);
+			test = memcmp (&tmp0, &tmp, sizeof (dgGoogol));
+		} while (test && (passes < (2 * DG_GOOGOL_SIZE)));	
+		dgAssert (passes <= (2 * DG_GOOGOL_SIZE));
+		return (*this) * tmp;
+	}
+
+
+	dgGoogol dgGoogol::Abs () const
+	{
+		dgGoogol tmp (*this);
+		tmp.m_sign = 0;
+		return tmp;
+	}
+
+	dgGoogol dgGoogol::Floor () const
+	{
+		if (m_exponent < 1) {
+			return dgGoogol (0.0);
+		} 
+		dgInt32 bits = m_exponent + 2;
+		dgInt32 start = 0;
+		while (bits >= 64) {
+			bits -= 64;
+			start ++;
+		}
+
+		dgGoogol tmp (*this);
+		for (dgInt32 i = DG_GOOGOL_SIZE - 1; i > start; i --) {
+			tmp.m_mantissa[i] = 0;
+		}
+		// some compilers do no like this and I do not know why is that
+		//dgUnsigned64 mask = (-1LL) << (64 - bits);
+		dgUnsigned64 mask (~0ULL);
+		mask <<= (64 - bits);
+		tmp.m_mantissa[start] &= mask;
+		if (m_sign) {
+			dgAssert (0);
+		}
+
+		return tmp;
+	}
+
+	dgGoogol dgGoogol::InvSqrt () const
+	{
+		const dgGoogol& me = *this;
+		dgGoogol x (1.0f / sqrt (me));
+
+		dgInt32 test = 0;
+		dgInt32 passes = 0;
+		do {
+			passes ++;
+			dgGoogol tmp (x);
+			x = m_half * x * (m_three - me * x * x);
+			test = memcmp (&x, &tmp, sizeof (dgGoogol));
+		} while (test && (passes < (2 * DG_GOOGOL_SIZE)));	
+		dgAssert (passes <= (2 * DG_GOOGOL_SIZE));
+		return x;
+	}
+
+	dgGoogol dgGoogol::Sqrt () const
+	{
+		return *this * InvSqrt();
+	}
+
+	void dgGoogol::ToString (char* const string) const
+	{
+		dgGoogol tmp (*this);
+		dgGoogol base (10.0);
+		while (dgFloat64 (tmp) > 1.0) {
+			tmp = tmp/base;
+		}
+
+		dgInt32 index = 0;
+		while (tmp.m_mantissa[0]) {
+			tmp = tmp * base;
+			dgGoogol digit (tmp.Floor());
+			tmp -= digit;
+			dgFloat64 val = digit;
+			string[index] = char (val) + '0';
+			index ++;
+		}
+		string[index] = 0;
+	}
+
+
+#else 
+
+	dgGoogol::dgGoogol(void)
+		:m_value(0.0)
+	{
+	}
+
+	dgGoogol::dgGoogol(dgFloat64 value)
+		:m_value (value)
+	{
+	}
+
+	void dgGoogol::CopySignedMantissa (dgUnsigned64* const mantissa) const
+	{
+	}
+
+	dgGoogol::operator double() const
+	{
+		return m_value;
+	}
+
+	dgGoogol dgGoogol::operator+ (const dgGoogol &A) const
+	{
+		return m_value + A.m_value;
+	}
+
+
+	dgGoogol dgGoogol::operator- (const dgGoogol &A) const
+	{
+		return m_value - A.m_value;
+	}
+
+	dgGoogol dgGoogol::operator* (const dgGoogol &A) const
+	{
+		return m_value * A.m_value;
+	}
+
+	dgGoogol dgGoogol::operator/ (const dgGoogol &A) const
+	{
+		return m_value / A.m_value;
+	}
+
+
+	dgGoogol dgGoogol::Abs () const
+	{
+		return fabs (m_value);
+	}
+
+	dgGoogol dgGoogol::InvSqrt () const
+	{
+		return 1.0 / sqrt (m_value);
+	}
+
+	dgGoogol dgGoogol::Sqrt () const
+	{
+		return sqrt(m_value);
+	}
+
+
+	dgGoogol dgGoogol::Floor () const
+	{
+		return floor (m_value);
+	}
+
+	void dgGoogol::ToString (char* const string) const
+	{
+		sprintf (string, "%f", m_value);
+	}
+
+	void dgGoogol::ScaleMantissa (dgUnsigned64* const dst, dgUnsigned64 scale) const
+	{
+	}
+
+#endif
+
+
+
+void dgGoogol::NegateMantissa (dgUnsigned64* const mantissa) const
+{
+	dgUnsigned64 carrier = 1;
+	for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) {
+		dgUnsigned64 a = ~mantissa[i] + carrier;
+		if (a) {
+			carrier = 0;
+		}
+		mantissa[i] = a;
+	}
+}
+
+
+void dgGoogol::ShiftRightMantissa (dgUnsigned64* const mantissa, dgInt32 bits) const
+{
+	dgUnsigned64 carrier = 0;
+	if (dgInt64 (mantissa[0]) < dgInt64 (0)) {
+		carrier = dgUnsigned64 (-1);
+	}
+	
+	while (bits >= 64) {
+		for (dgInt32 i = DG_GOOGOL_SIZE - 2; i >= 0; i --) {
+			mantissa[i + 1] = mantissa[i];
+		}
+		mantissa[0] = carrier;
+		bits -= 64;
+	}
+
+	if (bits > 0) {
+		carrier <<= (64 - bits);
+		for (dgInt32 i = 0; i < DG_GOOGOL_SIZE; i ++) {
+			dgUnsigned64 a = mantissa[i];
+			mantissa[i] = (a >> bits) | carrier;
+			carrier = a << (64 - bits);
+		}
+	}
+}
+
+dgInt32 dgGoogol::LeadingZeros (dgUnsigned64 a) const
+{
+	#define dgCOUNTBIT(mask,add)		\
+	{									\
+		dgUnsigned64 test = a & mask;	\
+		n += test ? 0 : add;			\
+		a = test ? test : (a & ~mask);	\
+	}
+
+	dgInt32 n = 0;
+    dgAssert (a);
+	dgCOUNTBIT (0xffffffff00000000LL, 32);
+	dgCOUNTBIT (0xffff0000ffff0000LL, 16);
+	dgCOUNTBIT (0xff00ff00ff00ff00LL,  8);
+	dgCOUNTBIT (0xf0f0f0f0f0f0f0f0LL,  4);
+	dgCOUNTBIT (0xccccccccccccccccLL,  2);
+	dgCOUNTBIT (0xaaaaaaaaaaaaaaaaLL,  1);
+
+	return n;
+}
+
+dgInt32 dgGoogol::NormalizeMantissa (dgUnsigned64* const mantissa) const
+{
+	dgAssert (dgInt64 (mantissa[0]) >= 0);
+
+	dgInt32 bits = 0;
+	if(dgInt64 (mantissa[0] * 2) < 0) {
+		bits = 1;
+		ShiftRightMantissa (mantissa, 1);
+	} else {
+		while (!mantissa[0] && bits > (-64 * DG_GOOGOL_SIZE)) {
+			bits -= 64;
+			for (dgInt32 i = 1; i < DG_GOOGOL_SIZE; i ++) {
+				mantissa[i - 1] = mantissa[i];
+			}
+			mantissa[DG_GOOGOL_SIZE - 1] = 0;
+		}
+
+		if (bits > (-64 * DG_GOOGOL_SIZE)) {
+			dgInt32 n = LeadingZeros (mantissa[0]) - 2;
+			if (n > 0) {
+				dgAssert (n > 0);
+				dgUnsigned64 carrier = 0;
+				for (dgInt32 i = DG_GOOGOL_SIZE-1; i >= 0; i --) {
+					dgUnsigned64 a = mantissa[i];
+					mantissa[i] = (a << n) | carrier;
+					carrier = a >> (64 - n);
+				}
+				bits -= n;
+			} else if (n < 0) {
+				// this is very rare but it does happens, whee the leading zeros of the mantissa is an exact multiple of 64
+				dgAssert (mantissa[0] & dgUnsigned64(3)<<62);
+				dgUnsigned64 carrier = 0;
+				dgInt32 shift = -n;
+				for (dgInt32 i = 0; i < DG_GOOGOL_SIZE; i ++) {
+					dgUnsigned64 a = mantissa[i];
+					mantissa[i] = (a >> shift) | carrier;
+					carrier = a << (64 - shift);
+				}
+				bits -= n;
+
+			}
+		}
+	}
+	return bits;
+}
+
+dgUnsigned64 dgGoogol::CheckCarrier (dgUnsigned64 a, dgUnsigned64 b) const
+{
+	return ((dgUnsigned64 (-1) - b) < a) ? dgUnsigned64(1) : 0;
+}
+
+
+void dgGoogol::ExtendeMultiply (dgUnsigned64 a, dgUnsigned64 b, dgUnsigned64& high, dgUnsigned64& low) const
+{
+	dgUnsigned64 bLow = b & 0xffffffff; 
+	dgUnsigned64 bHigh = b >> 32; 
+	dgUnsigned64 aLow = a & 0xffffffff; 
+	dgUnsigned64 aHigh = a >> 32; 
+
+	dgUnsigned64 l = bLow * aLow;
+
+	dgUnsigned64 c1 = bHigh * aLow;
+	dgUnsigned64 c2 = bLow * aHigh;
+	dgUnsigned64 m = c1 + c2;
+	dgUnsigned64 carrier = CheckCarrier (c1, c2) << 32;
+
+	dgUnsigned64 h = bHigh * aHigh + carrier;
+
+	dgUnsigned64 ml = m << 32;	
+	dgUnsigned64 ll = l + ml;
+	dgUnsigned64 mh = (m >> 32) + CheckCarrier (l, ml);	
+	dgAssert ((mh & ~0xffffffff) == 0);
+
+	dgUnsigned64 hh = h + mh;
+
+	low = ll;
+	high = hh;
+}
+
+
+
+
+
+dgGoogol dgGoogol::operator+= (const dgGoogol &A)
+{
+	*this = *this + A;
+	return *this;
+}
+
+dgGoogol dgGoogol::operator-= (const dgGoogol &A)
+{
+	*this = *this - A;
+	return *this;
+}
+
+
+bool dgGoogol::operator> (const dgGoogol &A) const
+{
+	dgGoogol tmp (*this - A);
+	return dgFloat64(tmp) > 0.0;
+}
+
+bool dgGoogol::operator>= (const dgGoogol &A) const 
+{
+	dgGoogol tmp (*this - A);
+	return dgFloat64 (tmp) >= 0.0;
+}
+
+bool dgGoogol::operator< (const dgGoogol &A) const 
+{
+	dgGoogol tmp (*this - A);
+	return dgFloat64 (tmp) < 0.0;
+}
+
+bool dgGoogol::operator<= (const dgGoogol &A) const 
+{
+	dgGoogol tmp (*this - A);
+	return dgFloat64 (tmp) <= 0.0;
+}
+
+bool dgGoogol::operator== (const dgGoogol &A) const 
+{
+	dgGoogol tmp (*this - A);
+	return dgFloat64 (tmp) == 0.0;
+}
+
+bool dgGoogol::operator!= (const dgGoogol &A) const 
+{
+	dgGoogol tmp (*this - A);
+	return dgFloat64 (tmp) != 0.0;
+}
+
+void dgGoogol::Trace () const
+{
+	dgTrace (("%f ", dgFloat64 (*this)));
+}
+
+
+
+
+
+
+

thirdparty/src/newton/dgCore/dgGoogol.h

@@ -0,0 +1,144 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgGoogol__
+#define __dgGoogol__
+
+
+#include "dgStdafx.h"
+#include "dgMemory.h"
+#include "dgArray.h"
+#include "dgVector.h"
+
+#define _IMPLEMENT_USING_INTEGER_ARITHMETIC_
+
+//#define DG_GOOGOL_SIZE	16
+#define DG_GOOGOL_SIZE		4
+
+class dgGoogol
+{
+	public:
+	dgGoogol(void);
+	dgGoogol(dgFloat64 value);
+
+	operator double() const;	
+	dgGoogol operator+ (const dgGoogol &A) const; 
+	dgGoogol operator- (const dgGoogol &A) const; 
+	dgGoogol operator* (const dgGoogol &A) const; 
+	dgGoogol operator/ (const dgGoogol &A) const; 
+
+	dgGoogol operator+= (const dgGoogol &A); 
+	dgGoogol operator-= (const dgGoogol &A); 
+
+	bool operator> (const dgGoogol &A) const; 
+	bool operator>= (const dgGoogol &A) const; 
+	bool operator< (const dgGoogol &A) const; 
+	bool operator<= (const dgGoogol &A) const; 
+	bool operator== (const dgGoogol &A) const; 
+	bool operator!= (const dgGoogol &A) const; 
+
+	dgGoogol Abs () const;
+	dgGoogol Sqrt () const;
+	dgGoogol InvSqrt () const;
+	dgGoogol Floor () const;
+	
+	void Trace () const;
+	void ToString (char* const string) const;
+
+
+	private:
+	void InitFloatFloat (dgFloat64 value);
+	void NegateMantissa (dgUnsigned64* const mantissa) const;
+	void CopySignedMantissa (dgUnsigned64* const mantissa) const;
+	dgInt32 NormalizeMantissa (dgUnsigned64* const mantissa) const;
+	dgUnsigned64 CheckCarrier (dgUnsigned64 a, dgUnsigned64 b) const;
+	void ShiftRightMantissa (dgUnsigned64* const mantissa, dgInt32 bits) const;
+
+	dgInt32 LeadingZeros (dgUnsigned64 a) const;
+	void ExtendeMultiply (dgUnsigned64 a, dgUnsigned64 b, dgUnsigned64& high, dgUnsigned64& low) const;
+	void ScaleMantissa (dgUnsigned64* const out, dgUnsigned64 scale) const;
+
+#ifdef _IMPLEMENT_USING_INTEGER_ARITHMETIC_
+	dgInt32 m_sign;
+	dgInt32 m_exponent;
+	dgUnsigned64 m_mantissa[DG_GOOGOL_SIZE];
+#else
+	dgFloat64 m_value;
+#endif
+	
+
+	public:
+	static dgGoogol m_zero; 
+	static dgGoogol m_one; 
+	static dgGoogol m_two; 
+	static dgGoogol m_three; 
+	static dgGoogol m_half; 
+};
+
+
+class dgHugeVector: public dgTemplateVector<dgGoogol>
+{
+	public:
+	dgHugeVector ()
+		:dgTemplateVector<dgGoogol>()
+	{
+	}
+
+	dgHugeVector (const dgBigVector& a)
+		:dgTemplateVector<dgGoogol>(dgGoogol (a.m_x), dgGoogol (a.m_y), dgGoogol (a.m_z), dgGoogol (a.m_w))
+	{
+	}
+
+	dgHugeVector (const dgTemplateVector<dgGoogol>& a)
+		:dgTemplateVector<dgGoogol>(a)
+	{
+	}
+
+	dgHugeVector (dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w)
+		:dgTemplateVector<dgGoogol>(x, y, z, w)
+	{
+	}
+
+	dgHugeVector(const dgGoogol& x, const dgGoogol& y, const dgGoogol& z, const dgGoogol& w)
+		:dgTemplateVector<dgGoogol>(x, y, z, w)
+	{
+	}
+
+	dgGoogol EvaluePlane (const dgHugeVector& point) const 
+	{
+		//return (point % (*this)) + m_w;
+		return DotProduct3(point) + m_w;
+	}
+
+#ifdef _DEBUG
+	void Trace () const
+	{
+		m_x.Trace();
+		m_y.Trace();
+		m_z.Trace();
+		m_w.Trace();
+		dgTrace (("\n"));
+	}
+#endif
+};
+
+
+#endif

thirdparty/src/newton/dgCore/dgGraph.h

@@ -0,0 +1,191 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+/****************************************************************************
+*
+*  Visual C++ 6.0 created by: Julio Jerez
+*
+****************************************************************************/
+#ifndef __dgGraph__
+#define __dgGraph__
+
+#include "dgStdafx.h"
+#include "dgRef.h"
+#include "dgList.h"
+
+
+template<class dgNodeData, class dgEdgeData> class dgGraphEdge;
+template<class dgNodeData, class dgEdgeData> class dgGraphNode;
+
+
+template<class dgNodeData, class dgEdgeData>
+class dgGraph: public dgList<dgGraphNode<dgNodeData, dgEdgeData> >
+{
+	public:
+	dgGraph (dgMemoryAllocator* const allocator);
+	~dgGraph ();
+
+	typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* AddNode ();
+	void DeleteNode (typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node);
+
+	typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* GetNodeFromNodeData(dgNodeData* const nodeData);
+
+	//void Trace () const;
+};
+
+template<class dgNodeData, class dgEdgeData> 
+class dgGraphNode: public dgList<dgGraphEdge<dgNodeData, dgEdgeData> >
+{
+	public:
+	dgGraphNode ();
+	~dgGraphNode ();
+
+	typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* AddEdge(typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node);
+	void DeleteHalfEdge(typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge);
+	void DeleteEdge(typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge);
+
+	//void Trace () const;
+
+	dgNodeData m_nodeData;
+};
+
+template<class dgNodeData, class dgEdgeData> 
+class dgGraphEdge
+{
+	public:
+	dgGraphEdge();
+	~dgGraphEdge();
+
+	typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* m_node;
+	dgEdgeData m_edgeData;
+};
+
+
+
+template<class dgNodeData, class dgEdgeData>
+dgGraph<dgNodeData, dgEdgeData>::dgGraph (dgMemoryAllocator* const allocator) 
+	:dgList<dgGraphNode<dgNodeData, dgEdgeData> >(allocator)
+{
+}
+
+
+template<class dgNodeData, class dgEdgeData>
+dgGraph<dgNodeData, dgEdgeData>::~dgGraph () 
+{
+}
+
+template<class dgNodeData, class dgEdgeData>
+typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* dgGraph<dgNodeData, dgEdgeData>::AddNode ()
+{
+	typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node = dgGraph<dgNodeData, dgEdgeData>::Append();
+
+	node->GetInfo().SetAllocator(dgGraph<dgNodeData, dgEdgeData>::GetAllocator());
+
+	return node;
+}
+
+template<class dgNodeData, class dgEdgeData>
+void dgGraph<dgNodeData, dgEdgeData>::DeleteNode (typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node)
+{
+	for (typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* link = node->GetInfo().GetFirst(); link; link = link->GetNext()) {	
+		typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const twinNode = link->GetInfo().m_node;
+		for (typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* link1 = twinNode->GetInfo().GetFirst(); link1; link1 = link1->GetNext()) {	
+			if (link1->GetInfo().m_node == node) {
+				twinNode->GetInfo().Remove (link1);
+				break;
+			}
+		}
+	}
+	dgList<dgGraphNode<dgNodeData, dgEdgeData> >::Remove (node);
+}
+
+template<class dgNodeData, class dgEdgeData>
+typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* dgGraph<dgNodeData, dgEdgeData>::GetNodeFromNodeData(dgNodeData* const nodeData)
+{
+	dgInt32 size1 = sizeof (*nodeData);
+	dgInt32 size0 = sizeof (dgGraphNode<dgNodeData, dgEdgeData>);
+	void* const ptr = ((char*)nodeData) - (size0 - size1);
+	dgGraphNode<dgNodeData, dgEdgeData>* const nodeInfo = (dgGraphNode<dgNodeData, dgEdgeData>*) ptr;
+	return this->GetNodeFromInfo(*nodeInfo);
+}
+
+
+template<class dgNodeData, class dgEdgeData> 
+dgGraphNode<dgNodeData, dgEdgeData>::dgGraphNode() 
+	:dgList<dgGraphEdge<dgNodeData, dgEdgeData> >(NULL)
+{
+
+}
+
+
+template<class dgNodeData, class dgEdgeData> 
+dgGraphNode<dgNodeData, dgEdgeData>::~dgGraphNode() 
+{
+
+}
+
+template<class dgNodeData, class dgEdgeData> 
+typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* dgGraphNode<dgNodeData, dgEdgeData>::AddEdge (typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node)
+{
+	typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge = dgGraphNode<dgNodeData, dgEdgeData>::Append();
+
+	edge->GetInfo().m_node = node;
+	return edge;
+}
+
+template<class dgNodeData, class dgEdgeData> 
+void dgGraphNode<dgNodeData, dgEdgeData>::DeleteHalfEdge(typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge)
+{
+	dgList<dgGraphEdge<dgNodeData, dgEdgeData> >::Remove (edge);
+}
+
+template<class dgNodeData, class dgEdgeData> 
+void dgGraphNode<dgNodeData, dgEdgeData>::DeleteEdge(typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge)
+{
+	typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node = edge->GetInfo().m_node;
+	
+	for (typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* twinEdge = node->GetInfo().GetFirst(); twinEdge; twinEdge = twinEdge->GetNext()) {	
+		if (&twinEdge->GetInfo().m_node->GetInfo() == this) {
+			node->GetInfo().DeleteHalfEdge(twinEdge);
+			break;
+		}
+	}
+
+	DeleteHalfEdge(edge);
+}	
+
+
+template<class dgNodeData, class dgEdgeData> 
+dgGraphEdge<dgNodeData, dgEdgeData>::dgGraphEdge() 
+{
+
+}
+
+template<class dgNodeData, class dgEdgeData> 
+dgGraphEdge<dgNodeData, dgEdgeData>::~dgGraphEdge() 
+{
+
+}
+
+
+
+#endif
+

thirdparty/src/newton/dgCore/dgHeap.h

@@ -0,0 +1,435 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+/****************************************************************************
+*
+*  Visual C++ 6.0 created by: Julio Jerez
+*
+****************************************************************************/
+#ifndef __dgHeapBase__
+#define __dgHeapBase__
+
+#include "dgStdafx.h"
+#include "dgMemory.h"
+
+//#define DG_HEAP_DEBUG_CHECK
+
+
+template <class OBJECT, class KEY>
+class dgHeapBase
+{
+	protected:
+	struct RECORD 
+	{
+		KEY m_key;
+		OBJECT m_obj;
+
+		RECORD (KEY key, const OBJECT& obj)
+			:m_key(key), m_obj(obj)
+		{
+		}
+	};
+
+	dgHeapBase (dgInt32 maxElements, dgMemoryAllocator* const allocator);
+	dgHeapBase (const void * const buffer, dgInt32 sizeInBytes);
+	~dgHeapBase ();
+	
+	public:
+	DG_CLASS_ALLOCATOR(allocator)
+
+	void Flush (); 
+	KEY MaxValue() const; 
+	KEY Value(dgInt32 i = 0) const;
+	dgInt32 GetCount() const;
+	dgInt32 GetMaxCount() const;
+	const OBJECT& operator[] (dgInt32 i) const;
+	dgInt32 Find (OBJECT &obj);
+	dgInt32 Find (KEY key);
+
+	dgInt32 m_curCount;
+	dgInt32 m_maxCount;
+	dgMemoryAllocator* m_allocator;
+	RECORD *m_pool;
+};
+
+template <class OBJECT, class KEY>
+class dgDownHeap: public dgHeapBase<OBJECT, KEY>
+{
+	public:
+	dgDownHeap (dgInt32 maxElements, dgMemoryAllocator* const allocator);
+	dgDownHeap (const void * const buffer, dgInt32 sizeInBytes);
+
+	void Pop () {Remove (0);}
+	void Push (OBJECT &obj, KEY key);
+	void Sort ();
+	void Remove (dgInt32 Index);
+	bool SanityCheck();
+};
+
+template <class OBJECT, class KEY>
+class dgUpHeap: public dgHeapBase<OBJECT, KEY>
+{
+	public:
+	dgUpHeap (dgInt32 maxElements, dgMemoryAllocator* const allocator);
+	dgUpHeap (const void * const buffer, dgInt32 sizeInBytes);
+
+	void Pop () {Remove (0);}
+	void Push (OBJECT &obj, KEY key);
+	void Sort ();
+	void Remove (dgInt32 Index);
+	bool SanityCheck();
+};
+
+template <class OBJECT, class KEY>
+dgHeapBase<OBJECT,KEY>::dgHeapBase (dgInt32 maxElements, dgMemoryAllocator* const allocator)
+{
+	m_allocator = allocator;
+	m_pool = (RECORD *)m_allocator->Malloc (maxElements * sizeof (RECORD));
+	m_maxCount = maxElements;
+	Flush();
+}
+
+template <class OBJECT, class KEY>
+dgHeapBase<OBJECT,KEY>::dgHeapBase (const void * const buffer, dgInt32 sizeInBytes)
+{
+	m_allocator = NULL;
+	m_pool = (RECORD *) buffer;
+	m_maxCount = dgInt32 (sizeInBytes / sizeof (RECORD));
+	Flush();
+}
+
+template <class OBJECT, class KEY>
+dgHeapBase<OBJECT,KEY>::~dgHeapBase ()
+{   
+	if (m_allocator) {
+		m_allocator->Free (m_pool);
+	}
+}
+
+
+template <class OBJECT, class KEY>
+KEY dgHeapBase<OBJECT,KEY>::Value(dgInt32 i) const
+{
+	return m_pool[i].m_key;
+}
+
+
+template <class OBJECT, class KEY>
+dgInt32 dgHeapBase<OBJECT,KEY>::GetCount() const
+{ 
+	return m_curCount;
+}
+
+template <class OBJECT, class KEY>
+void dgHeapBase<OBJECT,KEY>::Flush () 
+{
+	m_curCount = 0;
+
+	#ifdef _DEBUG
+//	dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_key = KEY (0);
+	#endif
+}
+
+template <class OBJECT, class KEY>
+KEY dgHeapBase<OBJECT,KEY>::MaxValue() const 
+{
+	return m_pool[0].m_key;
+}
+
+template <class OBJECT, class KEY>
+dgInt32 dgHeapBase<OBJECT,KEY>::GetMaxCount() const
+{ 
+	return m_maxCount;
+}
+
+
+template <class OBJECT, class KEY>
+dgInt32 dgHeapBase<OBJECT,KEY>::Find (OBJECT &obj)
+{
+	// For now let perform a linear search
+	// this is efficient if the size of the heap is small
+	// ex: m_curCount < 32
+	// this will be change to a binary search in the heap should the 
+	// the size of the heap get larger than 32
+	//	dgAssert (m_curCount <= 32);
+	for (dgInt32 i = 0; i < m_curCount; i ++) {
+		if (m_pool[i].obj == obj) {
+			return i;
+		}
+	}
+	return - 1;
+}
+
+
+template <class OBJECT, class KEY>
+dgInt32 dgHeapBase<OBJECT,KEY>::Find (KEY key)
+{
+	// ex: m_curCount < 32
+	// this will be change to a binary search in the heap shoud the 
+	// the size of the heap get larger than 32
+	dgAssert (m_curCount <= 32);
+	for (dgInt32 i = 0; i < m_curCount; i ++)	{
+		if (m_pool[i].m_key == key) {
+			return i;
+		}
+	}
+	return - 1;
+}
+
+
+template <class OBJECT, class KEY>
+const OBJECT& dgHeapBase<OBJECT,KEY>::operator[] (dgInt32 i) const
+{ 
+	dgAssert (i<= m_curCount);
+	return m_pool[i].m_obj;
+}
+
+
+// **************************************************************************
+//
+// down Heap
+//
+// **************************************************************************
+template <class OBJECT, class KEY>
+dgDownHeap<OBJECT,KEY>::dgDownHeap (dgInt32 maxElements, dgMemoryAllocator* const allocator)
+	:dgHeapBase<OBJECT, KEY> (maxElements, allocator)
+{
+}
+
+template <class OBJECT, class KEY>
+dgDownHeap<OBJECT,KEY>::dgDownHeap (const void * const buffer, dgInt32 sizeInBytes)
+	:dgHeapBase<OBJECT, KEY> (buffer, sizeInBytes)
+{
+}
+
+
+template <class OBJECT, class KEY>
+void dgDownHeap<OBJECT,KEY>::Push (OBJECT &obj, KEY key)
+{
+	dgHeapBase<OBJECT,KEY>::m_curCount ++;
+
+	dgInt32 j;
+	dgInt32 i = dgHeapBase<OBJECT,KEY>::m_curCount;
+	for (; i; i = j) {
+		j = i >> 1;
+		if (!j || (dgHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key > key)) {
+			break;
+		}
+		dgHeapBase<OBJECT,KEY>::m_pool[i - 1] = dgHeapBase<OBJECT,KEY>::m_pool[j - 1];
+	}
+	dgAssert (i);
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_key = key;
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_obj = obj;
+
+	dgAssert (SanityCheck());
+}
+
+template <class OBJECT, class KEY>
+void dgDownHeap<OBJECT,KEY>::Remove (dgInt32 index)
+{
+	dgHeapBase<OBJECT, KEY>::m_curCount--;
+	dgHeapBase<OBJECT, KEY>::m_pool[index] = dgHeapBase<OBJECT, KEY>::m_pool[dgHeapBase<OBJECT, KEY>::m_curCount];
+	while (index && dgHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1].m_key < dgHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
+		dgSwap(dgHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1], dgHeapBase<OBJECT, KEY>::m_pool[index]);
+		index = (index - 1) >> 1;
+	}
+
+	while ((2 * index + 1) < dgHeapBase<OBJECT, KEY>::m_curCount) {
+		dgInt32 i0 = 2 * index + 1;
+		dgInt32 i1 = 2 * index + 2;
+		if (i1 < dgHeapBase<OBJECT, KEY>::m_curCount) {
+			i0 = (dgHeapBase<OBJECT, KEY>::m_pool[i0].m_key > dgHeapBase<OBJECT, KEY>::m_pool[i1].m_key) ? i0 : i1;
+			if (dgHeapBase<OBJECT, KEY>::m_pool[i0].m_key <= dgHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
+				break;
+			}
+			dgSwap(dgHeapBase<OBJECT, KEY>::m_pool[i0], dgHeapBase<OBJECT, KEY>::m_pool[index]);
+			index = i0;
+		} else {
+			if (dgHeapBase<OBJECT, KEY>::m_pool[i0].m_key > dgHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
+				dgSwap(dgHeapBase<OBJECT, KEY>::m_pool[i0], dgHeapBase<OBJECT, KEY>::m_pool[index]);
+			}
+			index = i0;
+		}
+	}
+	dgAssert (SanityCheck());
+}
+
+template <class OBJECT, class KEY>
+void dgDownHeap<OBJECT,KEY>::Sort ()
+{
+	dgInt32 count = dgHeapBase<OBJECT,KEY>::m_curCount;
+	for (dgInt32 i = 1; i < count; i ++) {
+		KEY key (dgHeapBase<OBJECT,KEY>::m_pool[0].m_key);
+		OBJECT obj (dgHeapBase<OBJECT,KEY>::m_pool[0].m_obj);
+
+		Pop();
+
+		dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_key = key;
+		dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_obj = obj;
+	}
+
+	dgHeapBase<OBJECT,KEY>::m_curCount = count;
+	for (dgInt32 i = 0; i < count / 2; i ++) {
+		KEY key (dgHeapBase<OBJECT,KEY>::m_pool[i].m_key);
+		OBJECT obj (dgHeapBase<OBJECT,KEY>::m_pool[i].m_obj);
+
+		dgHeapBase<OBJECT,KEY>::m_pool[i].m_key = dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key;
+		dgHeapBase<OBJECT,KEY>::m_pool[i].m_obj = dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj;
+
+		dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key = key;
+		dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj = obj;
+	}
+	dgAssert (SanityCheck());
+}
+
+template <class OBJECT, class KEY>
+bool dgDownHeap<OBJECT,KEY>::SanityCheck()
+{
+#ifdef DG_HEAP_DEBUG_CHECK
+	for (dgInt32 i = 0; i < m_curCount; i++) {
+		dgInt32 i1 = 2 * i + 1;
+		dgInt32 i2 = 2 * i + 2;
+		if ((i1 < m_curCount) && (dgHeapBase<OBJECT, KEY>::m_pool[i].m_key < dgHeapBase<OBJECT, KEY>::m_pool[i1].m_key)) {
+			return false;
+		}
+		if ((i2 < m_curCount) && (dgHeapBase<OBJECT, KEY>::m_pool[i].m_key < dgHeapBase<OBJECT, KEY>::m_pool[i2].m_key)) {
+			return false;
+		}
+	}
+#endif
+	return true;
+}
+
+// **************************************************************************
+//
+// down Heap
+//
+// **************************************************************************
+template <class OBJECT, class KEY>
+dgUpHeap<OBJECT,KEY>::dgUpHeap (dgInt32 maxElements, dgMemoryAllocator* const allocator)
+	:dgHeapBase<OBJECT, KEY> (maxElements, allocator)
+{
+}
+
+template <class OBJECT, class KEY>
+dgUpHeap<OBJECT,KEY>::dgUpHeap (const void * const buffer, dgInt32 sizeInBytes)
+	:dgHeapBase<OBJECT, KEY> (buffer, sizeInBytes)
+{
+}
+
+template <class OBJECT, class KEY>
+bool dgUpHeap<OBJECT,KEY>::SanityCheck()
+{
+#ifdef DG_HEAP_DEBUG_CHECK
+	for (dgInt32 i = 0; i < m_curCount; i ++) {
+		dgInt32 i1 = 2 * i + 1; 
+		dgInt32 i2 = 2 * i + 2; 
+		if ((i1 < m_curCount) && (dgHeapBase<OBJECT,KEY>::m_pool[i].m_key > dgHeapBase<OBJECT,KEY>::m_pool[i1].m_key)) {
+			return false;
+		}
+		if ((i2 < m_curCount) && (dgHeapBase<OBJECT,KEY>::m_pool[i].m_key > dgHeapBase<OBJECT,KEY>::m_pool[i2].m_key)) {
+			return false;
+		}
+	}
+#endif
+	return true;
+}
+
+template <class OBJECT, class KEY>
+void dgUpHeap<OBJECT,KEY>::Push (OBJECT &obj, KEY key)
+{
+	dgHeapBase<OBJECT,KEY>::m_curCount ++;
+
+	dgInt32 j;
+	dgInt32 i = dgHeapBase<OBJECT,KEY>::m_curCount;
+	for (; i; i = j) {
+		j = i >> 1;
+		if (!j || (dgHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key < key)) {
+			break;
+		}
+		dgHeapBase<OBJECT,KEY>::m_pool[i - 1] = dgHeapBase<OBJECT,KEY>::m_pool[j - 1];
+	}
+	dgAssert (i);
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_key = key;
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_obj = obj;
+	dgAssert (SanityCheck());
+}
+
+template <class OBJECT, class KEY>
+void dgUpHeap<OBJECT,KEY>::Sort ()
+{
+	dgInt32 count = dgHeapBase<OBJECT,KEY>::m_curCount;
+	for (dgInt32 i = 1; i < count; i ++) {
+		KEY key (dgHeapBase<OBJECT,KEY>::m_pool[0].m_key);
+		OBJECT obj (dgHeapBase<OBJECT,KEY>::m_pool[0].m_obj);
+
+		Pop();
+
+		dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_key = key;
+		dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_obj = obj;
+	}
+
+	dgHeapBase<OBJECT,KEY>::m_curCount = count;
+	for (dgInt32 i = 0; i < count / 2; i ++) {
+		KEY key (dgHeapBase<OBJECT,KEY>::m_pool[i].m_key);
+		OBJECT obj (dgHeapBase<OBJECT,KEY>::m_pool[i].m_obj);
+
+		dgHeapBase<OBJECT,KEY>::m_pool[i].m_key = dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key;
+		dgHeapBase<OBJECT,KEY>::m_pool[i].m_obj = dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj;
+
+		dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key = key;
+		dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj = obj;
+	}
+	dgAssert (SanityCheck());
+}
+
+template <class OBJECT, class KEY>
+void dgUpHeap<OBJECT,KEY>::Remove (dgInt32 index)
+{
+	dgHeapBase<OBJECT, KEY>::m_curCount--;
+	dgHeapBase<OBJECT, KEY>::m_pool[index] = dgHeapBase<OBJECT, KEY>::m_pool[dgHeapBase<OBJECT, KEY>::m_curCount];
+	while (index && dgHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1].m_key > dgHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
+		dgSwap(dgHeapBase<OBJECT, KEY>::m_pool[(index - 1) >> 1], dgHeapBase<OBJECT, KEY>::m_pool[index]);
+		index = (index - 1) >> 1;
+	}
+
+	while ((2 * index + 1) < dgHeapBase<OBJECT, KEY>::m_curCount) {
+		dgInt32 i0 = 2 * index + 1;
+		dgInt32 i1 = 2 * index + 2;
+		if (i1 < dgHeapBase<OBJECT, KEY>::m_curCount) {
+			i0 = (dgHeapBase<OBJECT, KEY>::m_pool[i0].m_key < dgHeapBase<OBJECT, KEY>::m_pool[i1].m_key) ? i0 : i1;
+			if (dgHeapBase<OBJECT, KEY>::m_pool[i0].m_key >= dgHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
+				break;
+			}
+			dgSwap(dgHeapBase<OBJECT, KEY>::m_pool[i0], dgHeapBase<OBJECT, KEY>::m_pool[index]);
+			index = i0;
+		} else {
+			if (dgHeapBase<OBJECT, KEY>::m_pool[i0].m_key < dgHeapBase<OBJECT, KEY>::m_pool[index].m_key) {
+				dgSwap(dgHeapBase<OBJECT, KEY>::m_pool[i0], dgHeapBase<OBJECT, KEY>::m_pool[index]);
+			}
+			index = i0;
+		}
+	}
+	dgAssert (SanityCheck());
+}
+
+
+#endif

thirdparty/src/newton/dgCore/dgIntersections.cpp

@@ -0,0 +1,400 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgMemory.h"
+#include "dgVector.h"
+#include "dgPlane.h"
+#include "dgGoogol.h"
+#include "dgIntersections.h"
+
+#define USE_FLOAT_VERSION
+
+
+#define DG_RAY_TOL_ERROR (dgFloat32 (-1.0e-3f))
+#define DG_RAY_TOL_ADAPTIVE_ERROR (dgFloat32 (1.0e-1f))
+
+
+
+dgFloat32 dgFastRayTest::PolygonIntersect (const dgVector& faceNormal, dgFloat32 maxT, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const
+{
+	dgAssert (m_p0.m_w == dgFloat32 (0.0f));
+	dgAssert (m_p1.m_w == dgFloat32 (0.0f));
+
+	if (faceNormal.DotProduct(m_unitDir).GetScalar() < dgFloat32 (0.0f)) {
+		dgInt32 stride = dgInt32(strideInBytes / sizeof (dgFloat32));
+		dgBigVector v0(dgVector(&polygon[indexArray[indexCount - 1] * stride]) & dgVector::m_triplexMask);
+		dgBigVector p0(m_p0);
+		dgBigVector p0v0(v0 - p0);
+
+		dgBigVector diff(m_diff);
+		dgBigVector normal(faceNormal);
+		dgFloat64 tOut = normal.DotProduct(p0v0).GetScalar() / normal.DotProduct(diff).GetScalar();
+		if ((tOut >= dgFloat64(0.0f)) && (tOut <= maxT)) {
+			dgBigVector p (p0 + diff.Scale (tOut));
+			dgBigVector unitDir(m_unitDir);
+			for (dgInt32 i = 0; i < indexCount; i++) {
+				dgInt32 i2 = indexArray[i] * stride;
+				dgBigVector v1(dgVector(&polygon[i2]) & dgVector::m_triplexMask);
+
+				dgBigVector edge0(p - v0);
+				dgBigVector edge1(v1 - v0);
+				dgFloat64 area = unitDir.DotProduct (edge0.CrossProduct(edge1)).GetScalar();
+				if (area < dgFloat32 (0.0f)) {
+					return 1.2f;
+				}
+				v0 = v1;
+			}
+
+			return dgFloat32(tOut);
+		}
+	}
+
+	return dgFloat32 (1.2f);
+}
+
+bool dgApi dgRayBoxClip (dgVector& p0, dgVector& p1, const dgVector& boxP0, const dgVector& boxP1) 
+{	
+	dgAssert (p0.m_w == dgFloat32(0.0f));
+	dgAssert (p1.m_w == dgFloat32(0.0f));
+	dgAssert (boxP0.m_w == dgFloat32(0.0f));
+	dgAssert (boxP1.m_w == dgFloat32(0.0f));
+	for (int i = 0; i < 3; i ++) {
+		dgFloat32 tmp0 = boxP1[i] - p0[i];
+		if (tmp0 > dgFloat32 (0.0f)) {
+			dgFloat32 tmp1 = boxP1[i] - p1[i];
+			if (tmp1 < dgFloat32 (0.0f)) {
+				p1 = p0 + (p1 - p0).Scale (tmp0 / (p1[i] - p0[i])); 
+				p1[i] = boxP1[i];
+			}
+		} else {
+			dgFloat32 tmp1 = boxP1[i] - p1[i];
+			if (tmp1 > dgFloat32 (0.0f)) {
+				p0 += (p1 - p0).Scale (tmp0 / (p1[i] - p0[i])); 
+				p0[i] = boxP1[i];
+			} else {
+				return false;
+			}
+		}
+
+		tmp0 = boxP0[i] - p0[i];
+		if (tmp0 < dgFloat32 (0.0f)) {
+			dgFloat32 tmp1 = boxP0[i] - p1[i];
+			if (tmp1 > dgFloat32 (0.0f)) {
+				p1 = p0 + (p1 - p0).Scale (tmp0 / (p1[i] - p0[i])); 
+				p1[i] = boxP0[i];
+			}
+		} else {
+			dgFloat32 tmp1 = boxP0[i] - p1[i];
+			if (tmp1 < dgFloat32 (0.0f)) {
+				p0 += (p1 - p0).Scale (tmp0 / (p1[i] - p0[i])); 
+				p0[i] = boxP0[i];
+			} else {
+				return false;
+			}
+		}
+	}
+	return true;
+}
+
+dgBigVector dgPointToRayDistance (const dgBigVector& point, const dgBigVector& ray_p0, const dgBigVector& ray_p1)
+{
+	dgBigVector dp (ray_p1 - ray_p0);
+	dgAssert (dp.m_w == dgFloat32 (0.0f));
+	dgFloat64 t = dgClamp (dp.DotProduct(point - ray_p0).GetScalar() / dp.DotProduct(dp).GetScalar(), dgFloat64(0.0f), dgFloat64 (1.0f));
+	return ray_p0 + dp.Scale (t);
+}
+
+dgBigVector dgPointToTriangleDistance(const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2)
+{
+	const dgBigVector e10(p1 - p0);
+	const dgBigVector e20(p2 - p0);
+	const dgFloat64 a00 = e10.DotProduct(e10).GetScalar();
+	const dgFloat64 a11 = e20.DotProduct(e20).GetScalar();
+	const dgFloat64 a01 = e10.DotProduct(e20).GetScalar();
+
+	const dgFloat64 det = a00 * a11 - a01 * a01;
+	dgAssert(det >= dgFloat32(0.0f));
+	if (dgAbs(det) > dgFloat32(1.0e-24f)) {
+		dgBigVector p0Point (point - p0);
+		const dgFloat64 b0 = e10.DotProduct(p0Point).GetScalar();
+		const dgFloat64 b1 = e20.DotProduct(p0Point).GetScalar();
+
+		const dgFloat64 beta = b1 * a00 - a01 * b0;
+		const dgFloat64 alpha = b0 * a11 - a01 * b1;
+		if (beta < dgFloat32(0.0f)) {
+			return dgPointToRayDistance (point, p0, p1);
+		} else if (alpha < dgFloat32(0.0f)) {
+			return dgPointToRayDistance (point, p0, p2);
+		} else if ((alpha + beta) > det) {
+			return dgPointToRayDistance (point, p1, p2);
+		}
+		return p0 + (e10.Scale(alpha) + e20.Scale(beta)).Scale(dgFloat64(1.0f) / det);
+	}
+	// this is a degenerated triangle. this should never happens
+	dgAssert(0);
+	return p0;
+}
+
+dgBigVector dgPointToTetrahedrumDistance (const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3)
+{
+	const dgBigVector e10(p1 - p0);
+	const dgBigVector e20(p2 - p0);
+	const dgBigVector e30(p3 - p0);
+
+	const dgFloat64 d0 = sqrt(e10.DotProduct(e10).GetScalar());
+	if (d0 > dgFloat64(0.0f)) {
+		const dgFloat64 invd0 = dgFloat64(1.0f) / d0;
+		const dgFloat64 l10 = e20.DotProduct(e10).GetScalar() * invd0;
+		const dgFloat64 l20 = e30.DotProduct(e10).GetScalar() * invd0;
+		const dgFloat64 desc11 = e20.DotProduct(e20).GetScalar() - l10 * l10;
+		if (desc11 > dgFloat64(0.0f)) {
+			const dgFloat64 d1 = sqrt(desc11);
+			const dgFloat64 invd1 = dgFloat64(1.0f) / d1;
+			const dgFloat64 l21 = (e30.DotProduct(e20).GetScalar() - l20 * l10) * invd1;
+			const dgFloat64 desc22 = e30.DotProduct(e30).GetScalar() - l20 * l20 - l21 * l21;
+			if (desc22 > dgFloat64(0.0f)) {
+				dgBigVector p0Point (point - p0);
+				const dgFloat64 d2 = sqrt(desc22);
+				const dgFloat64 invd2 = dgFloat64(1.0f) / d2;
+				
+				const dgFloat64 b0 = e10.DotProduct(p0Point).GetScalar();
+				const dgFloat64 b1 = e20.DotProduct(p0Point).GetScalar();
+				const dgFloat64 b2 = e30.DotProduct(p0Point).GetScalar();
+
+				dgFloat64 u1 = b0 * invd0;
+				dgFloat64 u2 = (b1 - l10 * u1) * invd1;
+				dgFloat64 u3 = (b2 - l20 * u1 - l21 * u2) * invd2 * invd2;
+				u2 = (u2 - l21 * u3) * invd1;
+				u1 = (u1 - l10 * u2 - l20 * u3) * invd0;
+				if (u3 < dgFloat64(0.0f)) {
+					// this looks funny but it is correct
+					return dgPointToTriangleDistance(point, p0, p1, p2);
+				} else if (u2 < dgFloat64(0.0f)) {
+					return dgPointToTriangleDistance(point, p0, p1, p3);
+				} else if (u1 < dgFloat64(0.0f)) {
+					return dgPointToTriangleDistance(point, p0, p2, p3);
+				} else if (u1 + u2 + u3 > dgFloat64(1.0f)) {
+					return dgPointToTriangleDistance(point, p1, p2, p3);
+				}
+				return p0 + e10.Scale(u1) + e20.Scale(u2) + e30.Scale(u3);
+			}
+		}
+	}
+	// this is a degenerated tetra. this should never happens
+	dgAssert(0);
+	return p0;
+}
+
+void dgApi dgRayToRayDistance (const dgVector& ray_p0, const dgVector& ray_p1, const dgVector& ray_q0, const dgVector& ray_q1, dgVector& pOut, dgVector& qOut)
+{
+	dgFloat32 sN;
+	dgFloat32 tN;
+
+	dgVector u (ray_p1 - ray_p0);
+	dgVector v (ray_q1 - ray_q0);
+	dgVector w (ray_p0 - ray_q0);
+	dgAssert (u.m_w == dgFloat32 (0.0f));
+	dgAssert (v.m_w == dgFloat32 (0.0f));
+	dgAssert (w.m_w == dgFloat32 (0.0f));
+
+	dgFloat32 a = u.DotProduct(u).GetScalar();        
+	dgFloat32 b = u.DotProduct(v).GetScalar();
+	dgFloat32 c = v.DotProduct(v).GetScalar();        
+	dgFloat32 d = u.DotProduct(w).GetScalar();
+	dgFloat32 e = v.DotProduct(w).GetScalar();
+	dgFloat32 D = a*c - b*b;   
+	dgFloat32 sD = D;			
+	dgFloat32 tD = D;			
+
+	// compute the line parameters of the two closest points
+	if (D < dgFloat32 (1.0e-8f)) { 
+		sN = dgFloat32 (0.0f);        
+		sD = dgFloat32 (1.0f);        
+		tN = e;
+		tD = c;
+	} else {                
+		// get the closest points on the infinite lines
+		sN = (b*e - c*d);
+		tN = (a*e - b*d);
+		if (sN < dgFloat32 (0.0f)) {       
+			// sc < 0 => the s=0 edge is visible
+			sN = dgFloat32 (0.0f);
+			tN = e;
+			tD = c;
+		} else if (sN > sD) {  
+			// sc > 1 => the s=1 edge is visible
+			sN = sD;
+			tN = e + b;
+			tD = c;
+		}
+	}
+
+
+	if (tN < dgFloat32 (0.0f)) {           
+		// tc < 0 => the t=0 edge is visible
+		tN = dgFloat32 (0.0f);
+		// recompute sc for this edge
+		if (-d < dgFloat32 (0.0f)) {
+			sN = dgFloat32 (0.0f);
+		} else if (-d > a) {
+			sN = sD;
+		} else {
+			sN = -d;
+			sD = a;
+		}
+	} else if (tN > tD) {      
+		// tc > 1 => the t=1 edge is visible
+		tN = tD;
+		// recompute sc for this edge
+		if ((-d + b) < dgFloat32 (0.0f)) {
+			sN = dgFloat32 (0.0f);
+		} else if ((-d + b) > a) {
+			sN = sD;
+		} else {
+			sN = (-d + b);
+			sD = a;
+		}
+	}
+
+	// finally do the division to get sc and tc
+	dgFloat32 sc = (dgAbs(sN) < dgFloat32(1.0e-8f) ? dgFloat32 (0.0f) : sN / sD);
+	dgFloat32 tc = (dgAbs(tN) < dgFloat32(1.0e-8f) ? dgFloat32 (0.0f) : tN / tD);
+
+	dgAssert (u.m_w == dgFloat32 (0.0f));
+	dgAssert (v.m_w == dgFloat32 (0.0f));
+	pOut = ray_p0 + u.Scale (sc);
+	qOut = ray_q0 + v.Scale (tc);
+}
+
+
+dgFloat32 dgRayCastSphere (const dgVector& p0, const dgVector& p1, const dgVector& origin, dgFloat32 radius)
+{
+	dgVector p0Origin (p0 - origin);
+	dgAssert (p0Origin.m_w == dgFloat32 (0.0f));
+	if (p0Origin.DotProduct(p0Origin).GetScalar() < (dgFloat32 (100.0f) * radius * radius)) {
+		dgVector dp (p1 - p0);
+		dgAssert (dp.m_w == dgFloat32 (0.0f));
+		dgFloat32 a = dp.DotProduct(dp).GetScalar();
+		dgFloat32 b = dgFloat32 (2.0f) * p0Origin.DotProduct(dp).GetScalar();
+		dgFloat32 c = p0Origin.DotProduct(p0Origin).GetScalar() - radius * radius;
+		dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c;
+		if (desc >= 0.0f) {
+			desc = dgSqrt (desc);
+			dgFloat32 den = dgFloat32 (0.5f) / a;
+			dgFloat32 t0 = (-b + desc) * den;
+			dgFloat32 t1 = (-b - desc) * den;
+			if ((t0 >= dgFloat32 (0.0f)) && (t1 >= dgFloat32 (0.0f))) {
+				t0 =  dgMin(t0, t1);
+				if (t0 <= dgFloat32 (1.0f)) {
+					return t0;
+				}
+			} else if (t0 >= dgFloat32 (0.0f)) {
+				if (t0 <= dgFloat32 (1.0f)) {
+					return t0;
+				}
+			} else {
+				if ((t1 >= dgFloat32 (0.0f)) && (t1 <= dgFloat32 (1.0f))) {
+					return t1;
+				}
+			}
+		}
+	} else {
+		dgBigVector p0Origin1 (p0Origin);
+		dgBigVector dp (p1 - p0);
+		dgAssert (dp.m_w == dgFloat32 (0.0f));
+		dgFloat64 a = dp.DotProduct(dp).GetScalar();
+		dgFloat64 b = dgFloat32 (2.0f) * p0Origin1.DotProduct(dp).GetScalar();
+		dgFloat64 c = p0Origin1.DotProduct(p0Origin1).GetScalar() - dgFloat64(radius) * radius;
+		dgFloat64 desc = b * b - dgFloat32 (4.0f) * a * c;
+		if (desc >= 0.0f) {
+			desc = sqrt (desc);
+			dgFloat64 den = dgFloat32 (0.5f) / a;
+			dgFloat64 t0 = (-b + desc) * den;
+			dgFloat64 t1 = (-b - desc) * den;
+			if ((t0 >= dgFloat32 (0.0f)) && (t1 >= dgFloat32 (0.0f))) {
+				t0 =  dgMin(t0, t1);
+				if (t0 <= dgFloat32 (1.0f)) {
+					return dgFloat32 (t0);
+				}
+			} else if (t0 >= dgFloat32 (0.0f)) {
+				if (t0 <= dgFloat32 (1.0f)) {
+					return dgFloat32 (t0);
+				}
+			} else {
+				if ((t1 >= dgFloat32 (0.0f)) && (t1 <= dgFloat32 (1.0f))) {
+					return dgFloat32 (t1);
+				}
+			}
+		}
+	}
+	return dgFloat32 (1.2f);
+}
+
+dgFloat32 dgRayCastBox (const dgVector& p0, const dgVector& p1, const dgVector& boxP0, const dgVector& boxP1, dgVector& normalOut)
+{
+	dgInt32 index = 0;
+	dgFloat32 signDir = dgFloat32 (0.0f);
+	dgFloat32 tmin = dgFloat32 (0.0f);
+	dgFloat32 tmax = dgFloat32 (1.0f);
+
+	//dgVector size (boxP1 - boxP0);
+	for (dgInt32 i = 0; i < 3; i++) {
+		dgFloat32 dp = p1[i] - p0[i];
+		if (dgAbs (dp) < dgFloat32 (1.0e-8f)) {
+			if (p0[i] <= boxP0[i] || p0[i] >= boxP1[i]) {
+				return dgFloat32 (1.2f);
+			}
+		} else {
+			dp = dgFloat32 (1.0f) / dp; 
+			dgFloat32 t1 = (boxP0[i] - p0[i]) * dp;
+			dgFloat32 t2 = (boxP1[i] - p0[i]) * dp;
+
+			dgFloat32 sign = dgFloat32 (-1.0f);
+			if (t1 > t2) {
+				sign = 1;
+				dgSwap(t1, t2);
+			}
+			if (t1 > tmin) {
+				signDir = sign;
+				index = i;
+				tmin = t1;
+			}
+			if (t2 < tmax) {
+				tmax = t2;
+			}
+			if (tmin > tmax) {
+				return dgFloat32 (1.2f);
+			}
+		}
+	}
+
+	if (tmin > dgFloat32 (0.0f)) {
+		dgAssert (tmin < 1.0f);
+		normalOut = dgVector (dgFloat32 (0.0f));
+		normalOut[index] = signDir;
+	} else {
+		tmin = dgFloat32 (1.2f);
+	}
+	return tmin;
+
+}
+

thirdparty/src/newton/dgCore/dgIntersections.h

@@ -0,0 +1,393 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgIntersections__
+#define __dgIntersections__
+
+#include "dgStdafx.h"
+#include "dgObb.h"
+#include "dgDebug.h"
+#include "dgVector.h"
+#include "dgMatrix.h"
+
+class dgPlane;
+class dgObject;
+class dgPolyhedra;
+
+enum dgIntersectStatus
+{
+	t_StopSearh,
+	t_ContinueSearh
+};
+
+typedef dgIntersectStatus (*dgAABBIntersectCallback) (void* const context, 
+													  const dgFloat32* const polygon, dgInt32 strideInBytes,
+													  const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance);
+
+typedef dgFloat32 (*dgRayIntersectCallback) (void* const context, 
+											 const dgFloat32* const polygon, dgInt32 strideInBytes,
+											 const dgInt32* const indexArray, dgInt32 indexCount);
+
+dgBigVector dgPointToRayDistance (const dgBigVector& point, const dgBigVector& ray_p0, const dgBigVector& ray_p1); 
+dgBigVector dgPointToTriangleDistance (const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2);
+dgBigVector dgPointToTetrahedrumDistance (const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3);
+
+bool dgRayBoxClip (dgVector& ray_p0, dgVector& ray_p1, const dgVector& boxP0, const dgVector& boxP1); 
+void dgRayToRayDistance (const dgVector& ray_p0, const dgVector& ray_p1, const dgVector& ray_q0, const dgVector& ray_q1, dgVector& p0Out, dgVector& p1Out); 
+dgFloat32 dgRayCastBox (const dgVector& p0, const dgVector& p1, const dgVector& boxP0, const dgVector& boxP1, dgVector& normalOut);
+dgFloat32 dgRayCastSphere (const dgVector& p0, const dgVector& p1, const dgVector& origin, dgFloat32 radius);
+
+
+DG_INLINE dgInt32 dgOverlapTest (const dgVector& p0, const dgVector& p1, const dgVector& q0, const dgVector& q1)
+{
+	dgVector val((p0 - q1) * (p1 - q0));
+	dgInt32 mask = val.GetSignMask();
+	return ((mask & 0x07) == 0x07);
+}
+
+DG_INLINE dgInt32 dgBoxInclusionTest (const dgVector& p0, const dgVector& p1, const dgVector& q0, const dgVector& q1)
+{
+	dgVector val (dgVector::m_negOne & ((p0 >= q0) & (p1 <= q1)));
+	dgInt32 mask = val.GetSignMask();
+	return ((mask & 0x07) == 0x07);
+}
+
+DG_INLINE dgInt32 dgCompareBox (const dgVector& p0, const dgVector& p1, const dgVector& q0, const dgVector& q1)
+{
+	dgAssert(0);
+	return (p0.m_x != q0.m_x) || (p0.m_y != q0.m_y) || (p0.m_z != q0.m_z) || (p1.m_x != q1.m_x) || (p1.m_y != q1.m_y) || (p1.m_z != q1.m_z);
+}
+
+DG_INLINE void dgMovingAABB (dgVector& p0, dgVector& p1, const dgVector& veloc, const dgVector& omega, dgFloat32 timestep, dgFloat32 maxRadius, dgFloat32 minRadius)
+{
+	dgVector linearStep (veloc.Scale (timestep));
+
+	// estimate the maximum effect of the angular velocity and enlarge that box by that value (use 45 degrees as max angle not 90)
+	dgAssert (omega.m_w == dgFloat32 (0.0f));
+	dgFloat32 maxAngle = dgMin (dgSqrt (omega.DotProduct(omega).GetScalar() * timestep * timestep), dgFloat32 (45.0f * dgDegreeToRad));
+
+	dgFloat32 angularTravel = (maxRadius - minRadius) * maxAngle;
+	dgVector angularStep (angularTravel, angularTravel, angularTravel, dgFloat32 (0.0f));
+	
+	dgVector r0 (p0 - angularStep);
+	dgVector r1 (p1 + angularStep);
+	dgVector q0 (r0 + linearStep);
+	dgVector q1 (r1 + linearStep);
+	p0 = r0.GetMin (q0) & dgVector::m_triplexMask;
+	p1 = r1.GetMax (q1) & dgVector::m_triplexMask;
+}
+
+DG_INLINE dgFloat32 BoxPenetration (const dgVector& minBox, const dgVector& maxBox)
+{
+	dgAssert(maxBox.m_x >= minBox.m_x);
+	dgAssert(maxBox.m_y >= minBox.m_y);
+	dgAssert(maxBox.m_z >= minBox.m_z);
+
+	dgVector mask ((minBox * maxBox) < dgVector::m_zero);
+	dgVector dist (maxBox.GetMin (minBox.Abs()) & mask);
+	dist = dist.GetMin(dist.ShiftTripleRight());
+	dist = dist.GetMin(dist.ShiftTripleRight());
+	return dist.GetScalar();
+}
+
+DG_INLINE dgFloat32 dgBoxDistanceToOrigin2 (const dgVector& minBox, const dgVector& maxBox)
+{
+	dgAssert(maxBox.m_x >= minBox.m_x);
+	dgAssert(maxBox.m_y >= minBox.m_y);
+	dgAssert(maxBox.m_z >= minBox.m_z);
+	dgVector mask ((minBox * maxBox) > dgVector::m_zero);
+	dgVector dist (maxBox.Abs().GetMin (minBox.Abs()) & mask);
+	return dist.DotProduct(dist).GetScalar();
+}
+
+DG_MSC_VECTOR_ALIGNMENT
+class dgFastRayTest
+{
+	public:
+	DG_INLINE dgFastRayTest(const dgVector& l0, const dgVector& l1)
+		:m_p0(l0)
+		,m_p1(l1)
+		,m_diff((l1 - l0) & dgVector::m_triplexMask)
+		,m_minT(dgFloat32(0.0f))
+		,m_maxT(dgFloat32(1.0f))
+	{
+		dgAssert(m_p0.m_w == dgFloat32(0.0f));
+		dgAssert(m_p1.m_w == dgFloat32(0.0f));
+		dgAssert(m_diff.m_w == dgFloat32(0.0f));
+
+		dgAssert (m_diff.DotProduct(m_diff).GetScalar() > dgFloat32 (0.0f));
+		m_isParallel = (m_diff.Abs() < dgVector(1.0e-8f));
+		//m_dpInv = (((dgVector(dgFloat32(1.0e-20)) & m_isParallel) | m_diff.AndNot(m_isParallel)).Reciproc()) & dgVector::m_triplexMask;
+		m_dpInv = m_diff.Select (dgVector(dgFloat32(1.0e-20f)), m_isParallel).Reciproc() & dgVector::m_triplexMask;
+		m_unitDir = m_diff.Normalize();
+	}
+
+	dgFloat32 PolygonIntersect(const dgVector& normal, dgFloat32 maxT, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const;
+
+	DG_INLINE dgInt32 BoxTest(const dgVector& minBox, const dgVector& maxBox) const
+	{
+#if 1
+		dgVector test(((m_p0 <= minBox) | (m_p0 >= maxBox)) & m_isParallel);
+		if (test.GetSignMask() & 0x07) {
+			return 0;
+		}
+
+		dgVector tt0(m_dpInv * (minBox - m_p0));
+		dgVector tt1(m_dpInv * (maxBox - m_p0));
+
+		dgVector t0(m_minT.GetMax(tt0.GetMin(tt1)));
+		dgVector t1(m_maxT.GetMin(tt0.GetMax(tt1)));
+		t0 = t0.GetMax(t0.ShiftTripleRight());
+		t1 = t1.GetMin(t1.ShiftTripleRight());
+		t0 = t0.GetMax(t0.ShiftTripleRight());
+		t1 = t1.GetMin(t1.ShiftTripleRight());
+		return ((t0 < t1).GetSignMask() & 1);
+
+#else
+
+		dgFloat32 tmin = 0.0f;
+		dgFloat32 tmax = 1.0f;
+
+		for (dgInt32 i = 0; i < 3; i++) {
+			if (m_isParallel[i]) {
+				if (m_p0[i] <= minBox[i] || m_p0[i] >= maxBox[i]) {
+					return 0;
+				}
+			} else {
+				dgFloat32 t1 = (minBox[i] - m_p0[i]) * m_dpInv[i];
+				dgFloat32 t2 = (maxBox[i] - m_p0[i]) * m_dpInv[i];
+
+				if (t1 > t2) {
+					dgSwap(t1, t2);
+				}
+				if (t1 > tmin) {
+					tmin = t1;
+				}
+				if (t2 < tmax) {
+					tmax = t2;
+				}
+				if (tmin > tmax) {
+					return 0;
+				}
+			}
+		}
+		return 0x1;
+#endif
+	}
+
+	DG_INLINE dgFloat32 BoxIntersect(const dgVector& minBox, const dgVector& maxBox) const
+	{
+		dgVector test(((m_p0 <= minBox) | (m_p0 >= maxBox)) & m_isParallel);
+		if (test.GetSignMask() & 0x07) {
+			return dgFloat32(1.2f);
+		}
+		dgVector tt0(m_dpInv * (minBox - m_p0));
+		dgVector tt1(m_dpInv * (maxBox - m_p0));
+		dgVector t0(m_minT.GetMax(tt0.GetMin(tt1)));
+		dgVector t1(m_maxT.GetMin(tt0.GetMax(tt1)));
+		t0 = t0.GetMax(t0.ShiftTripleRight());
+		t1 = t1.GetMin(t1.ShiftTripleRight());
+		t0 = t0.GetMax(t0.ShiftTripleRight());
+		t1 = t1.GetMin(t1.ShiftTripleRight());
+		dgVector mask(t0 < t1);
+		dgVector maxDist(dgFloat32(1.2f));
+		//t0 = (t0 & mask) | maxDist.AndNot(mask);
+		t0 = maxDist.Select(t0, mask);
+		dgAssert((mask.GetSignMask() & 1) == (t0.m_x < dgFloat32(1.0f)));
+		return t0.GetScalar();
+	}
+
+	dgVector m_p0;
+	dgVector m_p1;
+	dgVector m_diff;
+	dgVector m_dpInv;
+	dgVector m_minT;
+	dgVector m_maxT;
+	dgVector m_unitDir;
+	dgVector m_isParallel;
+} DG_GCC_VECTOR_ALIGNMENT;
+
+
+DG_MSC_VECTOR_ALIGNMENT 
+class dgFastAABBInfo: public dgObb
+{
+	public:
+	DG_INLINE dgFastAABBInfo()
+		:dgObb()
+		,m_absDir(dgGetIdentityMatrix())
+		,m_separationDistance(dgFloat32(1.0e10f))
+	{
+	}
+
+	DG_INLINE dgFastAABBInfo(const dgMatrix& matrix, const dgVector& size)
+		:dgObb(matrix, size)
+		,m_separationDistance(dgFloat32(1.0e10f))
+	{
+		SetTransposeAbsMatrix (matrix);
+		dgVector size1 (matrix[0].Abs().Scale(size.m_x) + matrix[1].Abs().Scale(size.m_y) + matrix[2].Abs().Scale(size.m_z));
+		m_p0 = (matrix[3] - size1) & dgVector::m_triplexMask;
+		m_p1 = (matrix[3] + size1) & dgVector::m_triplexMask;
+	}
+
+	DG_INLINE dgFastAABBInfo(const dgVector& p0, const dgVector& p1)
+		:dgObb(dgGetIdentityMatrix(), dgVector::m_half * (p1 - p0))
+		,m_absDir(dgGetIdentityMatrix())
+		,m_separationDistance(dgFloat32(1.0e10f))
+		,m_p0(p0)
+		,m_p1(p1)
+	{
+		m_posit = ((dgVector::m_half * (p1 + p0)) & dgVector::m_triplexMask) | dgVector::m_wOne;
+	}
+
+	DG_INLINE void SetTransposeAbsMatrix (const dgMatrix& matrix)
+	{
+		m_absDir = matrix.Transpose();
+		m_absDir[0] = m_absDir[0].Abs();
+		m_absDir[1] = m_absDir[1].Abs();
+		m_absDir[2] = m_absDir[2].Abs();
+		//m_absDir[3] = dgVector::m_wOne;
+	}
+
+	DG_INLINE dgFloat32 PolygonBoxRayDistance (const dgVector& faceNormal, dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray, const dgFastRayTest& ray) const
+	{
+		dgVector minBox;
+		dgVector maxBox;
+		MakeBox1 (indexCount, indexArray, stride, vertexArray, minBox, maxBox);
+		dgFloat32 dist0 = ray.BoxIntersect(minBox, maxBox);
+		if (dist0 < dgFloat32 (1.0f)) {
+			dgMatrix faceMatrix (MakeFaceMatrix (faceNormal, indexCount, indexArray, stride, vertexArray));
+
+			MakeBox2 (faceMatrix, indexCount, indexArray, stride, vertexArray, minBox, maxBox);
+			dgVector veloc (faceMatrix.RotateVector(ray.m_diff) & dgVector::m_triplexMask);
+			dgFastRayTest localRay (dgVector (dgFloat32 (0.0f)), veloc);
+			dgFloat32 dist1 = localRay.BoxIntersect(minBox, maxBox);
+			dist0 = dgMax (dist1, dist0);
+		}
+		return dist0;
+	}
+
+
+	DG_INLINE dgFloat32 PolygonBoxDistance (const dgVector& faceNormal, dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray) const
+	{
+		dgVector minBox;
+		dgVector maxBox;
+		MakeBox1 (indexCount, indexArray, stride, vertexArray, minBox, maxBox);
+		dgVector mask(minBox * maxBox < dgVector(dgFloat32(0.0f)));
+		dgVector dist(maxBox.GetMin(minBox.Abs()) & mask);
+		dist = dist.GetMin(dist.ShiftTripleRight());
+		dist = dist.GetMin(dist.ShiftTripleRight());
+		dgFloat32 dist0 = dist.GetScalar();
+		if (dist0 > dgFloat32 (0.0f)) {
+			dgMatrix faceMatrix (MakeFaceMatrix (faceNormal, indexCount, indexArray, stride, vertexArray));
+			MakeBox2 (faceMatrix, indexCount, indexArray, stride, vertexArray, minBox, maxBox);
+			dgVector mask2(minBox * maxBox < dgVector(dgFloat32(0.0f)));
+			dgVector dist2(maxBox.GetMin(minBox.Abs()) & mask2);
+			dist2 = dist2.GetMin(dist2.ShiftTripleRight());
+			dist2 = dist2.GetMin(dist2.ShiftTripleRight());
+			dgFloat32 dist1 = dist2.GetScalar();
+			dist0 = (dist1 > dgFloat32 (0.0f)) ? dgMax (dist0, dist1) : dgFloat32 (0.0f);
+			if (dist0 <= dgFloat32(0.0f)) {
+				dgVector p1p0((minBox.Abs()).GetMin(maxBox.Abs()).AndNot(mask2));
+				dist2 = p1p0.DotProduct(p1p0);
+				dist2 = dist2.Sqrt() * dgVector::m_negOne;
+				dist0 = dist2.GetScalar();
+			}
+		} else {
+			dgVector p1p0((minBox.Abs()).GetMin(maxBox.Abs()).AndNot(mask));
+			dist = p1p0.DotProduct(p1p0);
+			dist = dist.Sqrt() * dgVector::m_negOne;
+			dist0 = dist.GetScalar();
+		}
+		return	dist0;
+	}
+
+	private:
+	DG_INLINE void MakeBox1 (dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray, dgVector& minBox, dgVector& maxBox) const
+	{
+		dgVector faceBoxP0 (&vertexArray[indexArray[0] * stride]);
+		faceBoxP0 = faceBoxP0 & dgVector::m_triplexMask;
+		dgVector faceBoxP1 (faceBoxP0);
+		for (dgInt32 i = 1; i < indexCount; i ++) {
+			dgVector p (&vertexArray[indexArray[i] * stride]);
+			p = p & dgVector::m_triplexMask;
+			faceBoxP0 = faceBoxP0.GetMin(p); 
+			faceBoxP1 = faceBoxP1.GetMax(p); 
+		}
+
+		minBox = faceBoxP0 - m_p1;
+		maxBox = faceBoxP1 - m_p0;
+	}
+
+	DG_INLINE void MakeBox2 (const dgMatrix& faceMatrix, dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray, dgVector& minBox, dgVector& maxBox) const
+	{
+		dgVector faceBoxP0 (faceMatrix.TransformVector (dgVector (&vertexArray[indexArray[0] * stride]) & dgVector::m_triplexMask));
+		dgVector faceBoxP1 (faceBoxP0);
+		for (dgInt32 i = 1; i < indexCount; i ++) {
+			dgVector p (faceMatrix.TransformVector (dgVector (&vertexArray[indexArray[i] * stride]) & dgVector::m_triplexMask));
+			faceBoxP0 = faceBoxP0.GetMin(p); 
+			faceBoxP1 = faceBoxP1.GetMax(p); 
+		}
+		faceBoxP0 = faceBoxP0 & dgVector::m_triplexMask;
+		faceBoxP1 = faceBoxP1 & dgVector::m_triplexMask;
+
+		dgMatrix matrix = *this * faceMatrix;
+		dgVector size (matrix[0].Abs().Scale(m_size.m_x) + matrix[1].Abs().Scale(m_size.m_y) + matrix[2].Abs().Scale(m_size.m_z));
+		dgVector boxP0 ((matrix.m_posit - size) & dgVector::m_triplexMask);
+		dgVector boxP1 ((matrix.m_posit + size) & dgVector::m_triplexMask);
+
+		minBox = faceBoxP0 - boxP1;
+		maxBox = faceBoxP1 - boxP0;
+	}
+
+
+	DG_INLINE dgMatrix MakeFaceMatrix (const dgVector& faceNormal, dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray) const
+	{
+		dgMatrix faceMatrix;
+		dgVector origin (&vertexArray[indexArray[0] * stride]);
+		dgVector pin (&vertexArray[indexArray[0] * stride]);
+		pin = pin & dgVector::m_triplexMask;
+		origin = origin & dgVector::m_triplexMask;
+
+		dgVector pin1 (&vertexArray[indexArray[1] * stride]);
+		pin1 = pin1 & dgVector::m_triplexMask;
+
+		faceMatrix[0] = faceNormal;
+		faceMatrix[1] = pin1 - origin;
+		faceMatrix[1] = faceMatrix[1].Normalize();
+		faceMatrix[2] = faceMatrix[0].CrossProduct(faceMatrix[1]);
+		faceMatrix[3] = origin | dgVector::m_wOne; 
+		return faceMatrix.Inverse();
+	}
+
+	protected:
+	dgMatrix m_absDir;
+	mutable dgVector m_separationDistance;
+	dgVector m_p0;
+	dgVector m_p1;
+
+	friend class dgAABBPolygonSoup;
+	friend class dgCollisionUserMesh;
+	friend class dgCollisionHeightField;
+} DG_GCC_VECTOR_ALIGNMENT;
+
+
+#endif
+

thirdparty/src/newton/dgCore/dgList.h

@@ -0,0 +1,701 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgList__
+#define __dgList__
+
+#include "dgStdafx.h"
+#include "dgRef.h"
+#include "dgDebug.h"
+#include "dgMemory.h"
+
+
+
+template<class T>
+class dgList 
+{
+	public:
+	class dgListNode
+	{
+		DG_CLASS_ALLOCATOR(allocator)
+
+		dgListNode (dgListNode* const prev, dgListNode* const next) 
+			:m_info () 
+		{
+			m_prev = prev;
+			m_next = next;
+			if (m_prev) {
+				m_prev->m_next = this;
+			}
+			if (m_next) {
+				m_next->m_prev = this;
+			}
+		}
+
+		dgListNode (const T &info, dgListNode* const prev, dgListNode* const next) 
+			:m_info (info) 
+		{
+			m_prev = prev;
+			m_next = next;
+			if (m_prev) {
+				m_prev->m_next = this;
+			}
+			if (m_next) {
+				m_next->m_prev = this;
+			}
+		}
+
+		virtual ~dgListNode()
+		{
+		}
+
+		void Unlink ()
+		{
+			if (m_prev) {
+				m_prev->m_next = m_next;
+			}
+
+			if (m_next) {
+				m_next->m_prev = m_prev;
+			}
+			m_prev = NULL;
+			m_next = NULL;
+		}
+
+		void AddLast(dgListNode* const node) 
+		{
+			m_next = node;
+			node->m_prev = this;
+		}
+
+		void AddFirst(dgListNode* const node) 
+		{
+			m_prev = node;
+			node->m_next = this;
+		}
+
+		public:
+		T& GetInfo()
+		{
+			return m_info;
+		}
+
+		dgListNode *GetNext() const
+		{
+			return m_next;
+		}
+
+		dgListNode *GetPrev() const
+		{
+			return m_prev;
+		}
+
+		private:
+		T m_info;
+		dgListNode *m_next;
+		dgListNode *m_prev;
+		friend class dgList<T>;
+
+	};
+
+	class Iterator
+	{
+		public:
+		Iterator (const dgList<T> &me)
+		{
+			m_ptr = NULL;
+			m_list = (dgList *)&me;
+		}
+
+		~Iterator ()
+		{
+		}
+
+		operator dgInt32() const
+		{
+			return m_ptr != NULL;
+		}
+
+		bool operator== (const Iterator &target) const
+		{
+			return (m_ptr == target.m_ptr) && (m_list == target.m_list);
+		}
+
+		void Begin()
+		{
+			m_ptr = m_list->GetFirst();
+		}
+
+		void End()
+		{
+			m_ptr = m_list->GetLast();
+		}
+
+		void Set (dgListNode* const node)
+		{
+			m_ptr = node;
+		}
+
+		void operator++ ()
+		{
+			dgAssert (m_ptr);
+			m_ptr = m_ptr->m_next();
+		}
+
+		void operator++ (dgInt32)
+		{
+			dgAssert (m_ptr);
+			m_ptr = m_ptr->GetNext();
+		}
+
+		void operator-- () 
+		{
+			dgAssert (m_ptr);
+			m_ptr = m_ptr->GetPrev();
+		}
+
+		void operator-- (dgInt32) 
+		{
+			dgAssert (m_ptr);
+			m_ptr = m_ptr->GetPrev();
+		}
+
+		T &operator* () const
+		{
+			return m_ptr->GetInfo();
+		}
+
+		dgListNode *GetNode() const
+		{
+			return m_ptr;
+		}
+
+		private:
+		dgList *m_list;
+		dgListNode *m_ptr;
+	};
+
+	// ***********************************************************
+	// member functions
+	// ***********************************************************
+	public:
+	DG_CLASS_ALLOCATOR(allocator)
+
+	dgList (dgMemoryAllocator* const allocator);
+	virtual ~dgList ();
+
+	dgMemoryAllocator* GetAllocator () const;
+	void SetAllocator (dgMemoryAllocator* const allocator);
+
+	operator dgInt32() const;
+	dgInt32 GetCount() const;
+	dgListNode* GetLast() const;
+	dgListNode* GetFirst() const;
+	dgListNode* Append ();
+	dgListNode* Append (dgListNode* const node);
+	dgListNode* Append (const T &element);
+	dgListNode* Addtop ();
+	dgListNode* Addtop (dgListNode* const node);
+	dgListNode* Addtop (const T &element);
+	
+	void RotateToEnd (dgListNode* const node);
+	void RotateToBegin (dgListNode* const node);
+	void InsertAfter (dgListNode* const root, dgListNode* const node);
+	void InsertBefore (dgListNode* const root, dgListNode* const node);
+
+
+	dgListNode* Find (const T &element) const;
+	dgListNode* GetNodeFromInfo (T &m_info) const;
+	void Remove (dgListNode* const node);
+	void Remove (const T &element);
+	void RemoveAll ();
+
+	void Merge (dgList<T>& list);
+	void Unlink (dgListNode* const node);
+	bool SanityCheck () const;
+
+	protected:
+//	dgListNode* SafeAddtop(const T &element);
+
+
+	// ***********************************************************
+	// member variables
+	// ***********************************************************
+	private:
+	dgInt32 m_count;
+	dgListNode* m_first;
+	dgListNode* m_last;
+	dgMemoryAllocator* m_allocator;
+
+//	static dgInt32 m_size;
+//	static dgMemoryAllocator* m_staticAllocator;
+	friend class dgListNode;
+};
+
+/*
+template<class T>
+dgList<T>::dgList ()
+{
+	m_count = 0;
+	m_first = NULL;
+	m_last = NULL;
+	m_allocator = NULL;
+}
+*/
+
+template<class T>
+dgList<T>::dgList (dgMemoryAllocator* const allocator)
+	:m_count(0)
+	,m_first(NULL)
+	,m_last(NULL)
+	,m_allocator(allocator)
+{
+}
+
+
+template<class T>
+dgList<T>::~dgList () 
+{
+	RemoveAll ();
+}
+
+template<class T>
+void dgList<T>::SetAllocator (dgMemoryAllocator* const allocator)
+{
+	if ((m_count == 0) && (m_allocator == NULL)) {
+		m_allocator = allocator;
+	}
+}
+
+template<class T>
+dgMemoryAllocator* dgList<T>::GetAllocator () const
+{
+	return m_allocator;
+}
+
+
+template<class T>
+dgInt32 dgList<T>::GetCount() const
+{
+	return m_count;
+}
+
+template<class T>
+dgList<T>::operator dgInt32() const
+{
+	return m_first != NULL;
+}
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::GetFirst() const
+{
+	return m_first;
+}
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::GetLast() const
+{
+	return m_last;
+}
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::Append (dgListNode* const node)
+{
+	dgAssert (node->m_next == NULL);
+	dgAssert (node->m_prev == NULL);
+	m_count	++;
+	if (m_first == NULL) {
+		m_last = node;
+		m_first = node;
+	} else {
+		m_last->AddLast (node);
+		m_last = node;
+	}
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+	return m_last;
+}
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::Append ()
+{
+	m_count	++;
+	if (m_first == NULL) {
+		m_first = new (m_allocator) dgListNode(NULL, NULL);
+		m_last = m_first;
+	} else {
+		m_last = new (m_allocator) dgListNode(m_last, NULL);
+	}
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+	return m_last;
+}
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::Append (const T &element)
+{
+	m_count	++;
+	if (m_first == NULL) {
+		m_first = new (m_allocator) dgListNode(element, NULL, NULL);
+		m_last = m_first;
+	} else {
+		m_last = new (m_allocator) dgListNode(element, m_last, NULL);
+	}
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+
+	return m_last;
+}
+
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::Addtop (dgListNode* const node)
+{
+	dgAssert (node->m_next == NULL);
+	dgAssert (node->m_prev == NULL);
+	m_count	++;
+	if (m_last == NULL) {
+		m_last = node;
+		m_first = node;
+	} else {
+		m_first->AddFirst(node);
+		m_first = node;
+	}
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+	return m_first;
+}
+
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::Addtop ()
+{
+	m_count	++;
+	if (m_last == NULL) {
+		m_last = new (m_allocator) dgListNode(NULL, NULL);
+		m_first = m_last;
+	} else {
+		m_first = new (m_allocator) dgListNode(NULL, m_first);
+	}
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+	return m_first;
+}
+
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::Addtop (const T &element)
+{
+	m_count	++;
+	if (m_last == NULL) {
+		m_last = new (m_allocator) dgListNode(element, NULL, NULL);
+		m_first = m_last;
+	} else {
+		m_first = new (m_allocator) dgListNode(element, NULL, m_first);
+	}
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+	return m_first;
+}
+
+/*
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::SafeAddtop(const T& element)
+{
+	dgAssert (m_last);
+	m_count++;
+
+	dgListNode* const node = new (m_allocator) dgListNode(element, NULL, NULL);
+	dgListNode* const first = (dgListNode*) dgInterlockedExchange ((void**) &m_first, (void*) node);
+
+	node->m_next = first;
+	first->m_prev = node;
+	dgAssert (m_last);
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert(SanityCheck());
+#endif
+	return node;
+}
+*/
+
+template<class T>
+void dgList<T>::InsertAfter (dgListNode* const root, dgListNode* const node)
+{
+	dgAssert (root);
+	if (node != root) {
+		if (root->m_next != node) {
+			if (node == m_first) {
+				m_first = node->m_next;
+			}
+			if (node == m_last) {
+				m_last = node->m_prev;
+			}
+			node->Unlink ();
+		
+			node->m_prev = root;
+			node->m_next = root->m_next;
+			if (root->m_next) {
+				root->m_next->m_prev = node;
+			} 
+			root->m_next = node;
+
+			if (node->m_next == NULL) {
+				m_last = node;
+			}
+
+			dgAssert (m_last);
+			dgAssert (!m_last->m_next);
+			dgAssert (m_first);
+			dgAssert (!m_first->m_prev);
+			dgAssert (SanityCheck ());
+		}
+	}
+}
+
+template<class T>
+void dgList<T>::InsertBefore (dgListNode* const root, dgListNode* const node)
+{
+	dgAssert (root);
+	if (node != root) {
+		if (root->m_prev != node) {
+			if (node == m_last) {
+				m_last = node->m_prev;
+			}
+			if (node == m_first) {
+				m_first = node->m_next;
+			}
+			node->Unlink ();
+		
+			node->m_next = root;
+			node->m_prev = root->m_prev;
+			if (root->m_prev) {
+				root->m_prev->m_next = node;
+			} 
+			root->m_prev = node;
+
+			if (node->m_prev == NULL) {
+				m_first = node;
+			}
+
+			dgAssert (m_first);
+			dgAssert (!m_first->m_prev);
+			dgAssert (m_last);
+			dgAssert (!m_last->m_next);
+			dgAssert (SanityCheck ());
+		}
+	}
+}
+
+template<class T>
+void dgList<T>::RotateToEnd (dgListNode* const node)
+{
+	if (node != m_last) {
+		if (m_last != m_first) {
+			if (node == m_first) {
+				m_first = m_first->GetNext();
+			}
+			node->Unlink();
+			m_last->AddLast(node);
+			m_last = node; 
+		}
+	}
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+}
+
+template<class T>
+void dgList<T>::RotateToBegin (dgListNode* const node)
+{
+	if (node != m_first) {
+		if (m_last != m_first) {
+			if (node == m_last) {
+				m_last = m_last->GetPrev();
+			}
+			node->Unlink();
+			m_first->AddFirst(node);
+			m_first = node; 
+		}
+	}
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+}
+
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::Find (const T &element) const
+{
+	dgListNode *node;
+	for (node = m_first; node; node = node->GetNext()) {
+		if (element	== node->m_info) {
+			break;
+		}
+	}
+	return node;
+}
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::GetNodeFromInfo (T &info) const
+{
+	dgListNode* const node = (dgListNode *) &info;
+	dgInt64 offset = ((char*) &node->m_info) - ((char *) node);
+	dgListNode* const retnode = (dgListNode *) (((char *) node) - offset);
+
+	dgAssert (&retnode->GetInfo () == &info);
+	return retnode;
+}
+
+
+template<class T> 
+void dgList<T>::Remove (const T &element)
+{
+	dgListNode *const node = Find (element);
+	if (node) {
+		Remove (node);
+	}
+}
+
+template<class T> 
+void dgList<T>::Unlink (dgListNode* const node)
+{
+	dgAssert (node);
+
+	m_count --;
+	dgAssert (m_count >= 0);
+
+	if (node == m_first) {
+		m_first = m_first->GetNext();
+	}
+	if (node == m_last) {
+		m_last = m_last->GetPrev();
+	}
+	node->Unlink();
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+}
+
+template<class T> 
+void dgList<T>::Merge (dgList<T>& list)
+{
+	m_count += list.m_count;
+	if (list.m_first) {
+		list.m_first->m_prev = m_last; 
+	}
+	if (m_last) {
+		m_last->m_next = list.m_first;
+	}
+	m_last = list.m_last;
+	if (!m_first) {
+		m_first = list.m_first;
+	}
+
+	list.m_count = 0;
+	list.m_last = NULL;
+	list.m_first = NULL;
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+}
+
+
+template<class T>
+void dgList<T>::Remove (dgListNode* const node)
+{
+	Unlink (node);
+	delete node;
+}
+
+
+template<class T>
+void dgList<T>::RemoveAll ()
+{
+	for (dgListNode *node = m_first; node; node = m_first) {
+		m_count --;
+		m_first = node->GetNext();
+		node->Unlink();
+		delete node;
+	}
+	dgAssert (m_count == 0);
+	m_last = NULL;
+	m_first = NULL;
+}
+
+template<class T>
+bool dgList<T>::SanityCheck () const
+{
+	#ifdef _DEBUG
+	dgInt32 tCount = 0;
+	for (dgListNode * node = m_first; node; node = node->GetNext()) {
+		tCount ++;
+		if (node->GetPrev()) {
+			dgAssert (node->GetPrev() != node->GetNext());
+			if (node->GetPrev()->GetNext() != node) {
+				dgAssert (0);
+				return false; 
+			}
+		}
+		if (node->GetNext()) {
+			dgAssert (node->GetPrev() != node->GetNext());
+			if (node->GetNext()->GetPrev() != node)	{
+				dgAssert (0);
+				return false;
+			}
+		}
+	}
+	if (tCount != m_count) {
+		dgAssert (0);
+		return false;
+	}
+	#endif
+	return true;
+}
+
+
+//template<class T>
+//void dgList<T>::SetAllocator (dgMemoryAllocator * allocator)
+//{
+//	m_allocator = allocator;
+//}
+
+//template<class T>
+//dgMemoryAllocator* dgList<T>::GetAllocator () const
+//{
+//	return m_allocator;
+//}
+//template<class T> dgInt32 dgList <T>::m_size = 0;
+//template<class T> dgMemoryAllocator* dgList<T>::m_staticAllocator = NULL;
+
+
+#endif
+
+

thirdparty/src/newton/dgCore/dgMatrix.cpp

@@ -0,0 +1,778 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgMatrix.h"
+#include "dgQuaternion.h"
+#include "dgGeneralMatrix.h"
+
+#ifndef _NEWTON_USE_DOUBLE
+
+	dgVector dgVector::m_xMask(dgInt32(-1), dgInt32(0), dgInt32(0), dgInt32(0));
+	dgVector dgVector::m_yMask(dgInt32(0), dgInt32(-1), dgInt32(0), dgInt32(0));
+	dgVector dgVector::m_zMask(dgInt32(0), dgInt32(0), dgInt32(-1), dgInt32(0));
+	dgVector dgVector::m_wMask(dgInt32(0), dgInt32(0), dgInt32(0), dgInt32(-1));
+	dgVector dgVector::m_triplexMask(dgInt32(-1), dgInt32(-1), dgInt32(-1), dgInt32(0));
+	dgVector dgVector::m_signMask(dgVector(dgInt32(-1), dgInt32(-1), dgInt32(-1), dgInt32(-1)).ShiftRightLogical(1));
+
+	dgVector dgVector::m_zero(dgFloat32(0.0f));
+	dgVector dgVector::m_one(dgFloat32(1.0f));
+	dgVector dgVector::m_two(dgFloat32(2.0f));
+	dgVector dgVector::m_half(dgFloat32(0.5f));
+	dgVector dgVector::m_three(dgFloat32(3.0f));
+	dgVector dgVector::m_negOne(dgFloat32(-1.0f));
+	dgVector dgVector::m_epsilon(dgFloat32(1.0e-20f));
+	dgVector dgVector::m_wOne(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f));
+#endif
+
+dgBigVector dgBigVector::m_zero (dgFloat64 (0.0f));
+dgBigVector dgBigVector::m_one (dgFloat64 (1.0f));
+dgBigVector dgBigVector::m_two (dgFloat64 (2.0f));
+dgBigVector dgBigVector::m_half (dgFloat32 (0.5f));
+dgBigVector dgBigVector::m_three (dgFloat32 (3.0f));
+dgBigVector dgBigVector::m_negOne (dgFloat32 (-1.0f));
+dgBigVector dgBigVector::m_epsilon(dgFloat32(1.0e-20f));
+dgBigVector dgBigVector::m_wOne (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f));
+dgBigVector dgBigVector::m_triplexMask (dgInt32 (-1), dgInt32 (-1),	dgInt32 (-1), dgInt32 (0));
+dgBigVector dgBigVector::m_signMask (dgBigVector(dgInt32 (-1), dgInt32 (-1), dgInt32 (-1), dgInt32 (-1)).ShiftRightLogical(1));
+
+dgBigVector dgBigVector::m_xMask (dgInt32 (-1), dgInt32 ( 0),	dgInt32 ( 0), dgInt32 ( 0));
+dgBigVector dgBigVector::m_yMask (dgInt32 ( 0), dgInt32 (-1),	dgInt32 ( 0), dgInt32 ( 0));
+dgBigVector dgBigVector::m_zMask (dgInt32 ( 0), dgInt32 ( 0),	dgInt32 (-1), dgInt32 ( 0));
+dgBigVector dgBigVector::m_wMask (dgInt32 ( 0), dgInt32 ( 0),	dgInt32 ( 0), dgInt32 (-1));
+
+dgSpatialVector dgSpatialVector::m_zero (dgFloat32 (0.0f));
+
+
+dgMatrix dgMatrix::m_zeroMatrix (dgVector (dgFloat32(0.0f)),
+								 dgVector (dgFloat32(0.0f)),
+								 dgVector (dgFloat32(0.0f)),
+								 dgVector (dgFloat32(0.0f)));
+
+dgMatrix dgMatrix::m_identityMatrix (dgVector (dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)),
+									 dgVector (dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)),
+									 dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f)),
+									 dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
+
+
+#ifndef _NEWTON_USE_DOUBLE
+#endif
+
+const dgMatrix& dgGetIdentityMatrix()
+{
+	return dgMatrix::m_identityMatrix;
+}
+
+const dgMatrix& dgGetZeroMatrix ()
+{
+	return dgMatrix::m_zeroMatrix;
+}
+
+
+dgMatrix::dgMatrix (const dgQuaternion &quat0, const dgVector &position)
+{
+	dgQuaternion quat1 (quat0);
+	quat1.Scale(dgFloat32 (2.0f));
+
+	dgFloat32 x2 = quat0.m_x * quat1.m_x;
+	dgFloat32 y2 = quat0.m_y * quat1.m_y;
+	dgFloat32 z2 = quat0.m_z * quat1.m_z;
+
+#ifdef _DEBUG
+	dgFloat32 w2 = quat0.m_w * quat1.m_w;
+	dgAssert (dgAbs (w2 + x2 + y2 + z2 - dgFloat32(2.0f)) <dgFloat32 (1.0e-3f));
+#endif
+
+	dgFloat32 xy = quat0.m_x * quat1.m_y;
+	dgFloat32 xz = quat0.m_x * quat1.m_z;
+	dgFloat32 xw = quat0.m_x * quat1.m_w;
+	dgFloat32 yz = quat0.m_y * quat1.m_z;
+	dgFloat32 yw = quat0.m_y * quat1.m_w;
+	dgFloat32 zw = quat0.m_z * quat1.m_w;
+
+	m_front = dgVector (dgFloat32(1.0f) - y2 - z2, xy + zw, xz - yw, dgFloat32(0.0f));
+	m_up    = dgVector (xy - zw, dgFloat32(1.0f) - x2 - z2, yz + xw, dgFloat32(0.0f));
+	m_right = dgVector (xz + yw, yz - xw, dgFloat32(1.0f) - x2 - y2, dgFloat32(0.0f));
+
+	m_posit.m_x = position.m_x;
+	m_posit.m_y = position.m_y;
+	m_posit.m_z = position.m_z;
+	m_posit.m_w = dgFloat32(1.0f);
+}
+
+
+dgMatrix::dgMatrix (const dgMatrix& transformMatrix, const dgVector& scale, const dgMatrix& stretchAxis)
+{
+	dgMatrix scaledAxis;
+	scaledAxis[0] = stretchAxis[0].Scale (scale[0]);
+	scaledAxis[1] = stretchAxis[1].Scale (scale[1]);
+	scaledAxis[2] = stretchAxis[2].Scale (scale[2]);
+	scaledAxis[3] = stretchAxis[3];
+
+	*this = stretchAxis.Transpose() * scaledAxis * transformMatrix;
+}
+
+dgMatrix dgMatrix::Multiply3X3 (const dgMatrix &B) const
+{
+	return dgMatrix (B.m_front * m_front.BroadcastX() + B.m_up * m_front.BroadcastY() + B.m_right * m_front.BroadcastZ(), 
+					 B.m_front * m_up.BroadcastX()    + B.m_up * m_up.BroadcastY()    + B.m_right * m_up.BroadcastZ(), 
+					 B.m_front * m_right.BroadcastX() + B.m_up * m_right.BroadcastY() + B.m_right * m_right.BroadcastZ(), 
+					 dgVector::m_wOne); 
+}
+
+dgMatrix dgMatrix::operator* (const dgMatrix &B) const
+{
+	return dgMatrix (B.m_front * m_front.BroadcastX() + B.m_up * m_front.BroadcastY() + B.m_right * m_front.BroadcastZ() + B.m_posit * m_front.BroadcastW(), 
+					 B.m_front * m_up.BroadcastX()    + B.m_up * m_up.BroadcastY()    + B.m_right * m_up.BroadcastZ()    + B.m_posit * m_up.BroadcastW(), 
+					 B.m_front * m_right.BroadcastX() + B.m_up * m_right.BroadcastY() + B.m_right * m_right.BroadcastZ() + B.m_posit * m_right.BroadcastW(), 
+					 B.m_front * m_posit.BroadcastX() + B.m_up * m_posit.BroadcastY() + B.m_right * m_posit.BroadcastZ() + B.m_posit * m_posit.BroadcastW()); 
+}
+
+void dgMatrix::TransformTriplex (dgFloat32* const dst, dgInt32 dstStrideInBytes, const dgFloat32* const src, dgInt32 srcStrideInBytes, dgInt32 count) const
+{
+	dgInt32 dstStride = dgInt32 (dstStrideInBytes /sizeof (dgFloat32));
+	dgInt32 srcStride = dgInt32 (srcStrideInBytes / sizeof (dgFloat32));
+
+	dgInt32 dstIndex = 0;
+	dgInt32 srcIndex = 0;
+	for (dgInt32 i = 0 ; i < count; i ++ ) {
+		dgFloat32 x = src[srcIndex + 0];
+		dgFloat32 y = src[srcIndex + 1];
+		dgFloat32 z = src[srcIndex + 2];
+		srcIndex += srcStride;
+		dst[dstIndex + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x;
+		dst[dstIndex + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y;
+		dst[dstIndex + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z;
+		dstIndex += dstStride;
+	}
+}
+
+#ifndef _NEWTON_USE_DOUBLE
+void dgMatrix::TransformTriplex (dgFloat64* const dst, dgInt32 dstStrideInBytes, const dgFloat64* const src, dgInt32 srcStrideInBytes, dgInt32 count) const
+{
+	dgInt32 dstStride = dgInt32 (dstStrideInBytes /sizeof (dgFloat64));
+	dgInt32 srcStride = dgInt32 (srcStrideInBytes / sizeof (dgFloat64));
+
+	dgInt32 dstIndex = 0;
+	dgInt32 srcIndex = 0;
+	for (dgInt32 i = 0 ; i < count; i ++ ) {
+		dgFloat64 x = src[srcIndex + 0];
+		dgFloat64 y = src[srcIndex + 1];
+		dgFloat64 z = src[srcIndex + 2];
+		srcIndex += srcStride;
+		dst[dstIndex + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x;
+		dst[dstIndex + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y;
+		dst[dstIndex + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z;
+		dstIndex += dstStride;
+	}
+}
+
+void dgMatrix::TransformTriplex (dgFloat64* const dst, dgInt32 dstStrideInBytes, const dgFloat32* const src, dgInt32 srcStrideInBytes, dgInt32 count) const
+{
+	dgInt32 dstStride = dgInt32 (dstStrideInBytes /sizeof (dgFloat64));
+	dgInt32 srcStride = dgInt32 (srcStrideInBytes / sizeof (dgFloat32));
+
+	dgInt32 dstIndex = 0;
+	dgInt32 srcIndex = 0;
+	for (dgInt32 i = 0 ; i < count; i ++ ) {
+		dgFloat64 x = src[srcIndex + 0];
+		dgFloat64 y = src[srcIndex + 1];
+		dgFloat64 z = src[srcIndex + 2];
+		srcIndex += srcStride;
+		dst[dstIndex + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x;
+		dst[dstIndex + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y;
+		dst[dstIndex + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z;
+		dstIndex += dstStride;
+	}
+}
+#endif
+
+void dgMatrix::TransformBBox (const dgVector& p0local, const dgVector& p1local, dgVector& p0, dgVector& p1) const
+{
+	const dgMatrix& matrix = *this;
+	//dgVector size ((p1local - p0local).Scale (dgFloat32 (0.5f)));
+	//dgVector center (TransformVector ((p1local + p0local).Scale (dgFloat32 (0.5f))));
+	dgVector size ((p1local - p0local) * dgVector::m_half);
+	dgVector center (TransformVector ((p1local + p0local) * dgVector::m_half));
+	dgVector extends (size.m_x * dgAbs(matrix[0][0]) + size.m_y * dgAbs(matrix[1][0]) + size.m_z * dgAbs(matrix[2][0]),  
+					  size.m_x * dgAbs(matrix[0][1]) + size.m_y * dgAbs(matrix[1][1]) + size.m_z * dgAbs(matrix[2][1]),  
+	                  size.m_x * dgAbs(matrix[0][2]) + size.m_y * dgAbs(matrix[1][2]) + size.m_z * dgAbs(matrix[2][2]), dgFloat32 (0.0f));  
+
+	p0 = center - extends;
+	p1 = center + extends;
+}
+
+dgMatrix dgMatrix::Inverse4x4 () const
+{
+	dgMatrix tmp (*this);
+	dgMatrix inv (dgGetIdentityMatrix());
+	for (dgInt32 i = 0; i < 4; i++) {
+		dgFloat32 pivot = dgAbs(tmp[i][i]);
+		if (pivot < dgFloat32(0.01f)) {
+			dgInt32 permute = i;
+			for (dgInt32 j = i + 1; j < 4; j++) {
+				dgFloat32 pivot1 = dgAbs(tmp[j][i]);
+				if (pivot1 > pivot) {
+					permute = j;
+					pivot = pivot1;
+				}
+			}
+			if (permute != i) {
+				dgAssert(pivot > dgFloat32(0.0f));
+				dgAssert((pivot > dgFloat32(1.0e-6f)) || (dgConditionNumber(4, 4, (dgFloat32*)&(*this)[0][0]) < dgFloat32(1.0e5f)));
+				dgSwap(inv[i], inv[permute]);
+				dgSwap(tmp[i], tmp[permute]);
+			}
+		}
+
+		for (dgInt32 j = i + 1; j < 4; j++) {
+			dgVector scale (tmp[j][i] / tmp[i][i]);
+			tmp[j] -= tmp[i] * scale;
+			inv[j] -= inv[i] * scale;
+			tmp[j][i] = dgFloat32 (0.0f);
+		}
+	}
+
+	for (dgInt32 i = 3; i >= 0; i--) {
+		dgVector acc(dgVector::m_zero);
+		for (dgInt32 j = i + 1; j < 4; j++) {
+			dgVector pivot(tmp[i][j]);
+			acc += pivot * inv[j];
+		}
+		dgVector den(dgFloat32(1.0f) / tmp[i][i]);
+		inv[i] = den * (inv[i] - acc);
+	}
+
+#ifdef _DEBUG
+	tmp = *this * inv;
+	for (dgInt32 i = 0; i < 4; i++) {
+		dgAssert(dgAbs(tmp[i][i] - dgFloat32(1.0f)) < dgFloat32(1.0e-6f));
+		for (dgInt32 j = i + 1; j < 4; j++) {
+			dgAssert(dgAbs(tmp[i][j]) < dgFloat32(1.0e-5f));
+			dgAssert(dgAbs(tmp[j][i]) < dgFloat32(1.0e-5f));
+		}
+	}
+#endif
+
+	return inv;
+}
+
+dgVector dgMatrix::SolveByGaussianElimination(const dgVector &v) const
+{
+//	return  Inverse4x4().UnrotateVector(v);
+	dgMatrix tmp(*this);
+	dgVector ret(v);
+	for (dgInt32 i = 0; i < 4; i++) {
+		dgFloat32 pivot = dgAbs(tmp[i][i]);
+		if (pivot < dgFloat32(0.01f)) {
+			dgInt32 permute = i;
+			for (dgInt32 j = i + 1; j < 4; j++) {
+				dgFloat32 pivot1 = dgAbs(tmp[j][i]);
+				if (pivot1 > pivot) {
+					permute = j;
+					pivot = pivot1;
+				}
+			}
+			
+			if (permute != i) {
+				dgAssert(pivot > dgFloat32(1.0e-6f));
+				dgSwap(ret[i], ret[permute]);
+				dgSwap(tmp[i], tmp[permute]);
+			}
+		}
+
+		for (dgInt32 j = i + 1; j < 4; j++) {
+			dgVector scale(tmp[j][i] / tmp[i][i]);
+			tmp[j] -= tmp[i] * scale;
+			ret[j] -= ret[i] * scale.GetScalar();
+			tmp[j][i] = dgFloat32(0.0f);
+		}
+	}
+
+	for (dgInt32 i = 3; i >= 0; i--) {
+		dgVector pivot(tmp[i] * ret);
+		ret[i] = (ret[i] - pivot.AddHorizontal().GetScalar() + tmp[i][i] * ret[i]) / tmp[i][i];
+	}
+
+	return ret;
+}
+
+void dgMatrix::CalcPitchYawRoll (dgVector& euler0, dgVector& euler1) const
+{
+	const dgMatrix& matrix = *this;
+	dgAssert (matrix[2].DotProduct(matrix[0].CrossProduct(matrix[1])).GetScalar() > 0.0f);
+	dgAssert (dgAbs (matrix[2].DotProduct(matrix[0].CrossProduct(matrix[1])).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
+/*
+	// Assuming the angles are in radians.
+	if (matrix[0][2] > dgFloat32 (0.99995f)) {
+		dgFloat32 picth0 = dgFloat32 (0.0f);
+		dgFloat32 yaw0 = dgFloat32 (-dgPI * 0.5f);
+		dgFloat32 roll0 = - dgAtan2(matrix[2][1], matrix[1][1]);
+		euler0[0] = picth0;
+		euler0[1] = yaw0;
+		euler0[2] = roll0;
+
+		euler1[0] = picth0;
+		euler1[1] = yaw0;
+		euler1[2] = roll0;
+
+	} else if (matrix[0][2] < dgFloat32 (-0.99995f)) {
+		dgFloat32 picth0 = dgFloat32 (0.0f);
+		dgFloat32 yaw0 = dgFloat32 (dgPI * 0.5f);
+		dgFloat32 roll0 = dgAtan2(matrix[2][1], matrix[1][1]);
+		euler0[0] = picth0;
+		euler0[1] = yaw0;
+		euler0[2] = roll0;
+
+		euler1[0] = picth0;
+		euler1[1] = yaw0;
+		euler1[2] = roll0;
+	} else {
+		dgFloat32 yaw0 = -dgAsin ( matrix[0][2]);
+		dgFloat32 yaw1 = dgFloat32 (dgPI) - yaw0;
+		dgFloat32 sign0 = dgSign(dgCos (yaw0));
+		dgFloat32 sign1 = dgSign(dgCos (yaw1));
+
+		dgFloat32 picth0 = dgAtan2(matrix[1][2] * sign0, matrix[2][2] * sign0);
+		dgFloat32 picth1 = dgAtan2(matrix[1][2] * sign1, matrix[2][2] * sign1);
+
+		dgFloat32 roll0 = dgAtan2(matrix[0][1] * sign0, matrix[0][0] * sign0);
+		dgFloat32 roll1 = dgAtan2(matrix[0][1] * sign1, matrix[0][0] * sign1);
+
+		if (yaw1 > dgFloat32 (dgPI)) {
+			yaw1 -= dgFloat32 (2.0f * dgPI);
+		}
+
+		euler0[0] = picth0;
+		euler0[1] = yaw0;
+		euler0[2] = roll0;
+
+		euler1[0] = picth1;
+		euler1[1] = yaw1;
+		euler1[2] = roll1;
+	}
+*/
+
+	// Assuming the angles are in radians.
+	if (matrix[0][2] > dgFloat32 (0.99995f)) {
+		dgFloat32 picth0 = dgFloat32(0.0f);
+		dgFloat32 yaw0 = dgFloat32(-dgPi * 0.5f);
+		dgFloat32 roll0 = -dgAtan2(matrix[2][1], matrix[1][1]);
+		euler0[0] = picth0;
+		euler0[1] = yaw0;
+		euler0[2] = roll0;
+
+		euler1[0] = picth0;
+		euler1[1] = yaw0;
+		euler1[2] = roll0;
+
+	} else if (matrix[0][2] < dgFloat32 (-0.99995f)) {
+		dgFloat32 picth0 = dgFloat32 (0.0f);
+		dgFloat32 yaw0 = dgFloat32(dgPi * 0.5f);
+		dgFloat32 roll0 = dgAtan2(matrix[2][1], matrix[1][1]);
+		euler0[0] = picth0;
+		euler0[1] = yaw0;
+		euler0[2] = roll0;
+
+		euler1[0] = picth0;
+		euler1[1] = yaw0;
+		euler1[2] = roll0;
+
+	} else {
+		dgFloat32 yaw0 = -dgAsin(matrix[0][2]);
+		dgFloat32 yaw1 = dgFloat32(dgPi) - yaw0;
+
+		dgFloat32 picth0 = dgAtan2( matrix[1][2],  matrix[2][2]);
+		dgFloat32 picth1 = dgAtan2(-matrix[1][2], -matrix[2][2]);
+
+		dgFloat32 roll0 = dgAtan2( matrix[0][1],  matrix[0][0]);
+		dgFloat32 roll1 = dgAtan2(-matrix[0][1], -matrix[0][0]);
+
+		if (yaw1 > dgFloat32 (dgPi)) {
+			yaw1 -= dgFloat32 (2.0f * dgPi);
+		}
+
+		euler0[0] = picth0;
+		euler0[1] = yaw0;
+		euler0[2] = roll0;
+
+		euler1[0] = picth1;
+		euler1[1] = yaw1;
+		euler1[2] = roll1;
+	}
+
+	euler0[3] = dgFloat32(0.0f);
+	euler1[3] = dgFloat32(0.0f);
+
+#ifdef _DEBUG
+	dgMatrix m0 (dgPitchMatrix (euler0[0]) * dgYawMatrix(euler0[1]) * dgRollMatrix(euler0[2]));
+	dgMatrix m1 (dgPitchMatrix (euler1[0]) * dgYawMatrix(euler1[1]) * dgRollMatrix(euler1[2]));
+	for (dgInt32 i = 0; i < 3; i ++) {
+		for (dgInt32 j = 0; j < 3; j ++) {
+			dgFloat32 error = dgAbs (m0[i][j] - matrix[i][j]);
+			dgAssert (error < 5.0e-2f);
+			error = dgAbs (m1[i][j] - matrix[i][j]);
+			dgAssert (error < 5.0e-2f);
+		}
+	}
+#endif
+}
+
+
+void dgMatrix::PolarDecomposition (dgMatrix& transformMatrix, dgVector& scale, dgMatrix& stretchAxis) const
+{
+	// a polar decomposition decompose matrix A = O * S
+	// where S = sqrt (transpose (L) * L)
+
+	const dgMatrix& me = *this;
+	dgFloat32 sign = dgSign (me[2].DotProduct(me[0].CrossProduct(me[1])).GetScalar());
+	stretchAxis = me * Transpose();
+	scale = stretchAxis.EigenVectors();
+
+	// I need to deal with by seeing of some of the Scale are duplicated
+	// do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
+	// for now just us the matrix
+
+	scale[0] = sign * dgSqrt (scale[0]);
+	scale[1] = sign * dgSqrt (scale[1]);
+	scale[2] = sign * dgSqrt (scale[2]);
+	scale[3] = dgFloat32 (0.0f);
+
+	dgMatrix scaledAxis;
+	scaledAxis[0] = stretchAxis[0].Scale (dgFloat32 (1.0f) / scale[0]);
+	scaledAxis[1] = stretchAxis[1].Scale (dgFloat32 (1.0f) / scale[1]);
+	scaledAxis[2] = stretchAxis[2].Scale (dgFloat32 (1.0f) / scale[2]);
+	scaledAxis[3] = stretchAxis[3];
+	dgMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);
+
+	transformMatrix = symetricInv * (*this);
+	transformMatrix.m_posit = m_posit;
+}
+
+#if 0
+dgVector dgMatrix::EigenVectors()
+{
+	dgMatrix& mat = *this;
+	dgMatrix eigenVectors(dgGetIdentityMatrix());
+
+	dgVector d(mat[0][0], mat[1][1], mat[2][2], dgFloat32(0.0f));
+	dgVector b(d);
+	for (dgInt32 i = 0; i < 50; i++) {
+		dgFloat32 sm = mat[0][1] * mat[0][1] + mat[0][2] * mat[0][2] + mat[1][2] * mat[1][2];
+		if (sm < dgFloat32(1.0e-12f)) {
+			// check the eigenvalue vectors	
+			//dgVector tmp(eigenVectors.m_front.CrossProduct(eigenVectors.m_up));
+			//if (tmp.DotProduct(eigenVectors.m_right).GetScalar() < dgFloat32(0.0f)) {
+			//	eigenVectors.m_right = eigenVectors.m_right * dgVector::m_negOne;
+			//}
+			dgAssert (eigenVectors[0].DotProduct(eigenVectors[1].CrossProduct(eigenVectors[2])).GetScalar() > dgFloat32 (0.0f));
+			break;
+		}
+
+		dgFloat32 thresh = dgFloat32(0.0f);
+		if (i < 3) {
+			thresh = (dgFloat32)(0.2f / 9.0f) * sm;
+		}
+
+		dgVector z(dgVector::m_zero);
+		for (dgInt32 ip = 0; ip < 2; ip++) {
+			for (dgInt32 iq = ip + 1; iq < 3; iq++) {
+				dgFloat32 g = dgFloat32(100.0f) * dgAbs(mat[ip][iq]);
+				if ((i > 3) && ((dgAbs(d[ip]) + g) == dgAbs(d[ip])) && ((dgAbs(d[iq]) + g) == dgAbs(d[iq]))) {
+					mat[ip][iq] = dgFloat32(0.0f);
+				} else if (dgAbs(mat[ip][iq]) > thresh) {
+
+					dgFloat32 t;
+					dgFloat32 h = d[iq] - d[ip];
+					if (dgAbs(h) + g == dgAbs(h)) {
+						t = mat[ip][iq] / h;
+					} else {
+						dgFloat32 theta = dgFloat32(0.5f) * h / mat[ip][iq];
+						t = dgFloat32(1.0f) / (dgAbs(theta) + dgSqrt(dgFloat32(1.0f) + theta * theta));
+						if (theta < dgFloat32(0.0f)) {
+							t = -t;
+						}
+					}
+					dgFloat32 c = dgRsqrt(dgFloat32(1.0f) + t * t);
+					dgFloat32 s = t * c;
+					dgFloat32 tau = s / (dgFloat32(1.0f) + c);
+					h = t * mat[ip][iq];
+					z[ip] -= h;
+					z[iq] += h;
+					d[ip] -= h;
+					d[iq] += h;
+					mat[ip][iq] = dgFloat32(0.0f);
+
+					for (dgInt32 j = 0; j <= ip - 1; j++) {
+						dgFloat32 g0 = mat[j][ip];
+						dgFloat32 h0 = mat[j][iq];
+						mat[j][ip] = g0 - s * (h0 + g0 * tau);
+						mat[j][iq] = h0 + s * (g0 - h0 * tau);
+					}
+					for (dgInt32 j = ip + 1; j <= iq - 1; j++) {
+						dgFloat32 g0 = mat[ip][j];
+						dgFloat32 h0 = mat[j][iq];
+						mat[ip][j] = g0 - s * (h0 + g0 * tau);
+						mat[j][iq] = h0 + s * (g0 - h0 * tau);
+					}
+					for (dgInt32 j = iq + 1; j < 3; j++) {
+						dgFloat32 g0 = mat[ip][j];
+						dgFloat32 h0 = mat[iq][j];
+						mat[ip][j] = g0 - s * (h0 + g0 * tau);
+						mat[iq][j] = h0 + s * (g0 - h0 * tau);
+					}
+
+					dgVector sv(s);
+					dgVector tauv(tau);
+					dgVector gv(eigenVectors[ip]);
+					dgVector hv(eigenVectors[iq]);
+					eigenVectors[ip] -= sv * (hv + gv * tauv);
+					eigenVectors[iq] += sv * (gv - hv * tauv);
+				}
+			}
+		}
+
+		b += z;
+		d = b;
+	}
+
+	*this = eigenVectors;
+	return d;
+}
+
+#else
+dgVector dgMatrix::EigenVectors ()
+{
+	dgMatrix matrix (*this);
+	dgMatrix eigenVectors(dgGetIdentityMatrix());
+
+#if 0
+	if (dgAbs(m_front.m_z) > dgFloat32(1.0e-6f)) {
+		// calculate initial guess by convert to tridiagonal matrix using householder
+		// but this fail since it changes the oder of the Eigen values and Eigen vectors
+		dgVector u(m_front);
+		u.m_x = dgFloat32(0.0f);
+		dgVector v(dgVector::m_zero);
+		v.m_y = dgSqrt(u.DotProduct(u).GetScalar());
+		dgVector w(u - v);
+		w = w.Normalize();
+		eigenVectors = dgMatrix(w, w);
+		dgMatrix ident(dgGetIdentityMatrix());
+		eigenVectors[0] = ident[0] - eigenVectors[0] * dgVector::m_two;
+		eigenVectors[1] = ident[1] - eigenVectors[1] * dgVector::m_two;
+		eigenVectors[2] = ident[2] - eigenVectors[2] * dgVector::m_two;
+		matrix = eigenVectors * matrix * eigenVectors;
+	}
+	matrix[0][2] = dgFloat32(0.0f);
+	matrix[2][0] = dgFloat32(0.0f);
+#endif
+
+	// QR algorithm is really bad at converging matrices with very different eigenvalue. 
+	// the solution is to use RD with double shift which I do not feel like implementing. 
+	// using Jacobi diagonalize instead
+	dgVector d (matrix[0][0], matrix[1][1], matrix[2][2], dgFloat32 (0.0f)); 
+	dgVector b (d);
+	for (dgInt32 i = 0; i < 50; i++) {
+		dgFloat32 sm = matrix[0][1] * matrix[0][1] + matrix[0][2] * matrix[0][2] + matrix[1][2] * matrix[1][2];
+		if (sm < dgFloat32 (1.0e-12f)) {
+			// make sure the the Eigen vectors are orthonormal
+			//dgVector tmp (eigenVectors.m_front.CrossProduct(eigenVectors.m_up));
+			//if (tmp.DotProduct(eigenVectors.m_right).GetScalar() < dgFloat32(0.0f)) {
+			//	eigenVectors.m_right = eigenVectors.m_right * dgVector::m_negOne;
+			//}
+			dgAssert (eigenVectors[0].DotProduct(eigenVectors[1].CrossProduct(eigenVectors[2])).GetScalar() > dgFloat32 (0.0f));
+			break;
+		}
+
+		dgFloat32 thresh = dgFloat32 (0.0f);
+		if (i < 3) {
+			thresh = (dgFloat32)(0.2f / 9.0f) * sm;
+		}
+
+		dgVector z (dgVector::m_zero);
+		for (dgInt32 j = 0; j < 2; j ++) {
+			for (dgInt32 k = j + 1; k < 3; k ++) {
+				dgFloat32 g = dgFloat32 (100.0f) * dgAbs(matrix[j][k]);
+				if ((i > 3) && ((dgAbs(d[j]) + g) == dgAbs(d[j])) && ((dgAbs(d[k]) + g) == dgAbs(d[k]))) {
+					matrix[j][k] = dgFloat32 (0.0f);
+				} else if (dgAbs(matrix[j][k]) > thresh) {
+
+					dgFloat32 t;
+					dgFloat32 h = d[k] - d[j];
+					if (dgAbs(h) + g == dgAbs(h)) {
+						t = matrix[j][k] / h;
+					} else {
+						dgFloat32 theta = dgFloat32 (0.5f) * h / matrix[j][k];
+						t = dgFloat32(1.0f) / (dgAbs(theta) + dgSqrt(dgFloat32(1.0f) + theta * theta));
+						if (theta < dgFloat32 (0.0f)) {
+							t = -t;
+						}
+					}
+					dgFloat32 c = dgRsqrt (dgFloat32 (1.0f) + t * t); 
+					dgFloat32 s = t * c; 
+					dgFloat32 tau = s / (dgFloat32(1.0f) + c); 
+					h = t * matrix[j][k];
+					z[j] -= h; 
+					z[k] += h; 
+					d[j] -= h; 
+					d[k] += h;
+					matrix[j][k] = dgFloat32(0.0f);
+
+					for (dgInt32 n = 0; n <= j - 1; n ++) {
+						dgFloat32 g0 = matrix[n][j]; 
+						dgFloat32 h0 = matrix[n][k]; 
+						matrix[n][j] = g0 - s * (h0 + g0 * tau); 
+						matrix[n][k] = h0 + s * (g0 - h0 * tau);
+					}
+					for (dgInt32 n = j + 1; n <= k - 1; n ++) {
+						dgFloat32 g0 = matrix[j][n]; 
+						dgFloat32 h0 = matrix[n][k]; 
+						matrix[j][n] = g0 - s * (h0 + g0 * tau); 
+						matrix[n][k] = h0 + s * (g0 - h0 * tau);
+					}
+					for (dgInt32 n = k + 1; n < 3; n ++) {
+						dgFloat32 g0 = matrix[j][n]; 
+						dgFloat32 h0 = matrix[k][n]; 
+						matrix[j][n] = g0 - s * (h0 + g0 * tau); 
+						matrix[k][n] = h0 + s * (g0 - h0 * tau);
+					}
+
+					dgVector sv (s);
+					dgVector tauv (tau);
+					dgVector gv (eigenVectors[j]);
+					dgVector hv (eigenVectors[k]);
+					eigenVectors[j] -= sv * (hv + gv * tauv); 
+					eigenVectors[k] += sv * (gv - hv * tauv);
+				}
+			}
+		}
+
+		b += z; 
+		d = b; 
+	}
+
+	#ifdef _DEBUG
+		dgMatrix diag(dgGetIdentityMatrix());
+		diag[0][0] = d[0];
+		diag[1][1] = d[1];
+		diag[2][2] = d[2];
+		dgMatrix E(eigenVectors.Transpose());
+		dgMatrix originalMatrix(*this);
+		dgMatrix tempMatrix(E * diag * E.Transpose());
+		for (dgInt32 j = 0; j < 3; j++) {
+			for (dgInt32 k = 0; k < 3; k++) {
+				dgFloat32 error = originalMatrix[j][k] - tempMatrix[j][k];
+				dgAssert((error * error) < dgFloat32(1.0e-4f));
+			}
+		}
+	#endif
+
+	*this = eigenVectors;
+	return d;
+}
+#endif
+
+dgSpatialMatrix dgSpatialMatrix::Inverse(dgInt32 rows) const
+{
+	dgSpatialMatrix tmp(*this);
+	dgSpatialMatrix inv(dgFloat64(0.0f));
+	for (dgInt32 i = 0; i < rows; i++) {
+		inv[i][i] = dgFloat32(1.0f);
+	}
+
+	for (dgInt32 i = 0; i < rows; i++) {
+		dgFloat64 pivot = dgAbs(tmp[i][i]);
+		if (pivot < dgFloat64(0.01f)) {
+			int permute = i;
+			for (dgInt32 j = i + 1; j < rows; j++) {
+				dgFloat64 pivot1 = dgAbs(tmp[j][i]);
+				if (pivot1 > pivot) {
+					permute = j;
+					pivot = pivot1;
+				}
+			}
+			dgAssert(pivot > dgFloat32(0.0f));
+			dgAssert((pivot > dgFloat32(1.0e-6f)) || (dgConditionNumber(rows, 6, (dgFloat64*)&m_rows[0]) < dgFloat32(1.0e5f)));
+			//if (!((pivot > dgFloat32(1.0e-6f)) || (dgConditionNumber(rows, 6, (dgFloat64*)&m_rows[0]) < dgFloat32(1.0e5f))))
+			//{
+			//	for (dgInt32 m = 0; m < rows; m++) {
+			//		for (dgInt32 n = 0; n < rows; n++) {
+			//			dgTrace(("%f ", m_rows[m][n]));
+			//		}
+			//		dgTrace(("\n"));
+			//	}
+			//	dgAssert(0);
+			//}
+
+			if (permute != i) {
+				for (dgInt32 j = 0; j < rows; j++) {
+					dgSwap(tmp[i][j], tmp[permute][j]);
+					dgSwap(tmp[i][j], tmp[permute][j]);
+				}
+			}
+		}
+
+		for (dgInt32 j = i + 1; j < rows; j++) {
+			dgFloat64 scale = tmp[j][i] / tmp[i][i];
+			tmp[j][i] = dgFloat64(0.0f);
+			for (int k = i + 1; k < rows; k++) {
+				tmp[j][k] -= scale * tmp[i][k];
+			}
+			for (int k = 0; k <= i; k++) {
+				inv[j][k] -= scale * inv[i][k];
+			}
+		}
+	}
+
+	for (dgInt32 i = rows - 1; i >= 0; i--) {
+		dgSpatialVector acc(dgFloat64(0.0f));
+		for (dgInt32 j = i + 1; j < rows; j++) {
+			dgFloat64 pivot = tmp[i][j];
+			for (int k = 0; k < rows; k++) {
+				acc[k] += pivot * inv[j][k];
+			}
+		}
+		dgFloat64 den = dgFloat64(1.0f) / tmp[i][i];
+		for (dgInt32 k = 0; k < rows; k++) {
+			inv[i][k] = den * (inv[i][k] - acc[k]);
+		}
+	}
+
+
+#ifdef _DEBUG
+	for (dgInt32 i = 0; i < rows; i++) {
+		for (dgInt32 j = 0; j < rows; j++) {
+			tmp[i][j] = m_rows[j][i];
+		}
+	}
+	for (dgInt32 i = 0; i < rows; i++) {
+		dgSpatialVector v(inv.VectorTimeMatrix(tmp[i], rows));
+		dgAssert(dgAbs(v[i] - dgFloat64(1.0f)) < dgFloat64(1.0e-6f));
+		for (dgInt32 j = 0; j < rows; j++) {
+			if (j != i) {
+				dgAssert(dgAbs(v[j]) < dgFloat64(1.0e-6f));
+			}
+		}
+	}
+#endif
+
+	return inv;
+}

thirdparty/src/newton/dgCore/dgMatrix.h

@@ -0,0 +1,380 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgMatrix__
+#define __dgMatrix__
+
+
+#include "dgStdafx.h"
+#include "dgDebug.h"
+#include "dgVector.h"
+#include "dgPlane.h"
+#include <math.h>
+
+class dgMatrix;
+class dgQuaternion;
+
+const dgMatrix& dgGetZeroMatrix ();
+const dgMatrix& dgGetIdentityMatrix();
+
+
+DG_MSC_VECTOR_ALIGNMENT
+class dgMatrix
+{
+	public:
+	DG_CLASS_ALLOCATOR(allocator)
+
+	dgMatrix ();
+	dgMatrix (const dgFloat32* const array);
+	dgMatrix (const dgVector &front, const dgVector &up, const dgVector &right, const dgVector &posit);
+	dgMatrix (const dgQuaternion &rotation, const dgVector &position);
+
+	// create a orthonormal normal vector basis, front become m_front vector, and m_up and m_right are mutualiperpendicular to fron and to each other
+	dgMatrix (const dgVector &front);
+
+	// create a covariance Matrix = transpose(p) * q 
+	dgMatrix (const dgVector& p, const dgVector& q);
+
+	dgVector& operator[] (dgInt32 i);
+	const dgVector& operator[] (dgInt32 i) const;
+
+	dgMatrix Inverse () const;
+	dgMatrix Inverse4x4 () const;
+	dgMatrix Transpose () const;
+	dgMatrix Transpose4X4 () const;
+	dgVector RotateVector (const dgVector &v) const;
+	dgVector UnrotateVector (const dgVector &v) const;
+	dgVector TransformVector (const dgVector &v) const;
+	dgVector UntransformVector (const dgVector &v) const;
+	dgPlane TransformPlane (const dgPlane &localPlane) const;
+	dgPlane UntransformPlane (const dgPlane &globalPlane) const;
+	dgVector SolveByGaussianElimination(const dgVector &v) const;
+	void TransformBBox (const dgVector& p0local, const dgVector& p1local, dgVector& p0, dgVector& p1) const; 
+
+	void CalcPitchYawRoll (dgVector& euler0, dgVector& euler1) const;
+	void TransformTriplex (dgFloat32* const dst, dgInt32 dstStrideInBytes,
+						   const dgFloat32* const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
+
+#ifndef _NEWTON_USE_DOUBLE
+	void TransformTriplex (dgFloat64* const dst, dgInt32 dstStrideInBytes,
+						   const dgFloat64* const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
+
+	void TransformTriplex (dgFloat64* const dst, dgInt32 dstStrideInBytes,
+						   const dgFloat32* const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
+#endif
+
+	bool TestIdentity() const;
+	bool TestSymetric3x3() const;
+	bool TestOrthogonal(dgFloat32 tol = dgFloat32 (1.0e-4f)) const;
+
+	dgMatrix Multiply3X3 (const dgMatrix &B) const;
+	dgMatrix operator* (const dgMatrix &B) const;
+
+	// these function can only be called when dgMatrix is a PDS matrix
+	//void EigenVectors ();
+	dgVector EigenVectors ();
+	void PolarDecomposition (dgMatrix& transformMatrix, dgVector& scale, dgMatrix& stretchAxis) const;
+
+	// constructor for polar composition
+	dgMatrix (const dgMatrix& transformMatrix, const dgVector& scale, const dgMatrix& stretchAxis);
+
+	dgVector m_front;
+	dgVector m_up;
+	dgVector m_right;
+	dgVector m_posit;
+
+	static dgMatrix m_zeroMatrix;
+	static dgMatrix m_identityMatrix;
+} DG_GCC_VECTOR_ALIGNMENT;
+
+
+
+DG_INLINE dgMatrix::dgMatrix ()
+{
+}
+
+DG_INLINE dgMatrix::dgMatrix (const dgFloat32* const array)
+{
+	memcpy (&m_front.m_x, array, sizeof (dgMatrix)) ;
+}
+
+DG_INLINE dgMatrix::dgMatrix (const dgVector &front, const dgVector &up, const dgVector &right, const dgVector &posit)
+	:m_front (front), m_up(up), m_right(right), m_posit(posit)
+{
+}
+
+DG_INLINE dgMatrix::dgMatrix (const dgVector& p, const dgVector& q)
+	:m_front(q * p.BroadcastX())
+	,m_up   (q * p.BroadcastY())
+	,m_right(q * p.BroadcastZ())
+	,m_posit (dgVector::m_wOne)
+{
+}
+
+DG_INLINE dgMatrix::dgMatrix (const dgVector& front)
+{
+	m_front = front; 
+	if (dgAbs (front.m_z) > dgFloat32 (0.577f)) {
+		m_right = front.CrossProduct(dgVector (-front.m_y, front.m_z, dgFloat32(0.0f), dgFloat32(0.0f)));
+	} else {
+	  	m_right = front.CrossProduct(dgVector (-front.m_y, front.m_x, dgFloat32(0.0f), dgFloat32(0.0f)));
+	}
+  	//m_right = m_right.Scale (dgRsqrt (m_right.DotProduct(m_right).GetScalar()));
+	m_right = m_right.Normalize();
+  	m_up = m_right.CrossProduct(m_front);
+
+	m_front.m_w = dgFloat32(0.0f);
+	m_up.m_w = dgFloat32(0.0f);
+	m_right.m_w = dgFloat32(0.0f);
+	m_posit = dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f));
+
+	dgAssert ((dgAbs (m_front.DotProduct(m_front).GetScalar()) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f)); 
+	dgAssert ((dgAbs (m_up.DotProduct(m_up).GetScalar()) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f)); 
+	dgAssert ((dgAbs (m_right.DotProduct(m_right).GetScalar()) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f)); 
+	dgAssert ((dgAbs (m_right.DotProduct(m_front.CrossProduct(m_up)).GetScalar()) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f)); 
+}
+
+DG_INLINE dgVector& dgMatrix::operator[] (dgInt32  i)
+{
+	dgAssert (i < 4);
+	dgAssert (i >= 0);
+	return (&m_front)[i];
+}
+
+DG_INLINE const dgVector& dgMatrix::operator[] (dgInt32  i) const
+{
+	dgAssert (i < 4);
+	dgAssert (i >= 0);
+	return (&m_front)[i];
+}
+
+
+DG_INLINE dgMatrix dgMatrix::Transpose () const
+{
+	dgMatrix inv;
+	dgVector::Transpose4x4(inv[0], inv[1], inv[2], inv[3], m_front, m_up, m_right, dgVector::m_wOne);
+	return inv;
+}
+
+DG_INLINE dgMatrix dgMatrix::Transpose4X4 () const
+{
+	dgMatrix inv;
+	dgVector::Transpose4x4(inv[0], inv[1], inv[2], inv[3], m_front, m_up, m_right, m_posit);
+	return inv;
+}
+
+DG_INLINE dgVector dgMatrix::RotateVector (const dgVector &v) const
+{
+	return m_front * v.BroadcastX() + m_up * v.BroadcastY() + m_right * v.BroadcastZ();
+}
+
+DG_INLINE dgVector dgMatrix::UnrotateVector (const dgVector &v) const
+{
+	return dgVector ((m_front * v).AddHorizontal().GetScalar(), (m_up * v).AddHorizontal().GetScalar(), (m_right * v).AddHorizontal().GetScalar(), dgFloat32 (0.0f));
+}
+
+DG_INLINE dgVector dgMatrix::TransformVector (const dgVector &v) const
+{
+	return RotateVector(v) + m_posit;
+}
+
+DG_INLINE dgVector dgMatrix::UntransformVector (const dgVector &v) const
+{
+	return UnrotateVector(v - m_posit);
+}
+
+DG_INLINE dgPlane dgMatrix::TransformPlane (const dgPlane &localPlane) const
+{
+	return dgPlane (RotateVector (localPlane), localPlane.m_w - (localPlane.DotProduct(UnrotateVector (m_posit)).GetScalar()));  
+}
+
+DG_INLINE dgPlane dgMatrix::UntransformPlane (const dgPlane &globalPlane) const
+{
+	return dgPlane (UnrotateVector (globalPlane), globalPlane.Evalue(m_posit));
+}
+
+/*
+DG_INLINE void dgMatrix::EigenVectors ()
+{
+	dgVector eigenValues;
+	EigenVectors (eigenValues);
+}
+*/
+
+DG_INLINE dgMatrix dgMatrix::Inverse () const
+{
+	// much faster inverse
+	dgMatrix inv;
+	dgVector::Transpose4x4 (inv[0], inv[1], inv[2], inv[3], m_front, m_up, m_right, dgVector::m_wOne);
+	inv.m_posit -= inv[0] * m_posit.BroadcastX() + inv[1] * m_posit.BroadcastY() + inv[2] * m_posit.BroadcastZ();
+	return inv;
+}
+
+DG_INLINE bool dgMatrix::TestIdentity() const
+{
+	const dgMatrix& me = *this;
+	for (int i = 0; i < 4; i++) {
+		if (me[i][i] != dgFloat32 (1.0f)) {
+			return false;
+		}
+		for (int j = i + 1; j < 4; j++) {
+			if (me[i][j] != dgFloat32 (0.0f)) {
+				return false;
+			}
+			if (me[j][i] != dgFloat32(0.0f)) {
+				return false;
+			}
+		}
+	}
+	return true;
+}
+
+DG_INLINE bool dgMatrix::TestOrthogonal(dgFloat32 tol) const
+{
+	dgVector n (m_front.CrossProduct(m_up));
+	dgFloat32 a = m_right.DotProduct(m_right).GetScalar();
+	dgFloat32 b = m_up.DotProduct(m_up).GetScalar();
+	dgFloat32 c = m_front.DotProduct(m_front).GetScalar();
+	dgFloat32 d = n.DotProduct(m_right).GetScalar();
+
+#ifdef _DEBUG
+	const dgMatrix& me = *this;
+	for (int i = 0; i < 4; i++) {
+		for (int j = 0; j < 4; j++) {
+			dgAssert(dgCheckFloat(me[i][j]));
+		}
+	}
+#endif
+
+	return (m_front[3] == dgFloat32 (0.0f)) & 
+		   (m_up[3] == dgFloat32 (0.0f)) & 
+		   (m_right[3] == dgFloat32 (0.0f)) & 
+		   (m_posit[3] == dgFloat32 (1.0f)) &
+		   (dgAbs(a - dgFloat32 (1.0f)) < tol) & 
+		   (dgAbs(b - dgFloat32 (1.0f)) < tol) &
+		   (dgAbs(c - dgFloat32 (1.0f)) < tol) &
+		   (dgAbs(d - dgFloat32 (1.0f)) < tol); 
+}
+
+DG_INLINE bool dgMatrix::TestSymetric3x3() const
+{
+	const dgMatrix& me = *this;
+	return (dgAbs (me[0][1] - me[1][0]) < dgFloat32 (1.0e-5f)) && 
+		   (dgAbs (me[0][2] - me[2][0]) < dgFloat32 (1.0e-5f)) &&
+		   (dgAbs (me[1][2] - me[2][1]) < dgFloat32 (1.0e-5f)) &&
+		   (me[0][3] == dgFloat32 (0.0f)) &&
+		   (me[1][3] == dgFloat32 (0.0f)) &&
+		   (me[2][3] == dgFloat32 (0.0f)) &&
+		   (me[3][0] == dgFloat32 (0.0f)) &&
+		   (me[3][1] == dgFloat32 (0.0f)) &&
+		   (me[3][2] == dgFloat32 (0.0f)) &&
+		   (me[3][3] == dgFloat32 (1.0f));
+}
+
+DG_INLINE dgMatrix dgPitchMatrix(dgFloat32 ang)
+{
+	dgFloat32 sinAng = dgSin (ang);
+	dgFloat32 cosAng = dgCos (ang);
+	return dgMatrix (dgVector (dgFloat32(1.0f),  dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)), 
+		dgVector (dgFloat32(0.0f),  cosAng,          sinAng,          dgFloat32(0.0f)),
+		dgVector (dgFloat32(0.0f), -sinAng,          cosAng,          dgFloat32(0.0f)), 
+		dgVector (dgFloat32(0.0f),  dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f))); 
+
+}
+
+DG_INLINE dgMatrix dgYawMatrix(dgFloat32 ang)
+{
+	dgFloat32 sinAng = dgSin (ang);
+	dgFloat32 cosAng = dgCos (ang);
+	return dgMatrix (dgVector (cosAng,          dgFloat32(0.0f), -sinAng,          dgFloat32(0.0f)), 
+		dgVector (dgFloat32(0.0f), dgFloat32(1.0f),  dgFloat32(0.0f), dgFloat32(0.0f)), 
+		dgVector (sinAng,          dgFloat32(0.0f),  cosAng,          dgFloat32(0.0f)), 
+		dgVector (dgFloat32(0.0f), dgFloat32(0.0f),  dgFloat32(0.0f), dgFloat32(1.0f))); 
+}
+
+DG_INLINE dgMatrix dgRollMatrix(dgFloat32 ang)
+{
+	dgFloat32 sinAng = dgSin (ang);
+	dgFloat32 cosAng = dgCos (ang);
+	return dgMatrix (dgVector ( cosAng,          sinAng,          dgFloat32(0.0f), dgFloat32(0.0f)), 
+					 dgVector (-sinAng,          cosAng,          dgFloat32(0.0f), dgFloat32(0.0f)),
+					 dgVector ( dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f)), 
+					 dgVector ( dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f))); 
+}																		 
+
+
+
+DG_MSC_VECTOR_ALIGNMENT
+class dgSpatialMatrix
+{
+	public:
+	DG_INLINE dgSpatialMatrix()
+	{
+	}
+
+	DG_INLINE dgSpatialMatrix(dgFloat32 val)
+	{
+		const dgSpatialVector row (val);
+		for (dgInt32 i = 0; i < 6; i++) {
+			m_rows[i] = row;
+		}
+	}
+
+	DG_INLINE dgSpatialVector& operator[] (dgInt32 i)
+	{
+		dgAssert(i < 6);
+		dgAssert(i >= 0);
+		return m_rows[i];
+	}
+
+	DG_INLINE const dgSpatialVector& operator[] (dgInt32 i) const
+	{
+		dgAssert(i < 6);
+		dgAssert(i >= 0);
+		return m_rows[i];
+	}
+
+	dgSpatialMatrix Inverse(dgInt32 rows) const;
+
+	DG_INLINE dgSpatialVector VectorTimeMatrix(const dgSpatialVector& jacobian) const
+	{
+		dgSpatialVector tmp(m_rows[0].Scale (jacobian[0]));
+		for (dgInt32 i = 1; i < 6; i++) {
+			tmp = tmp + m_rows[i].Scale(jacobian[i]);
+		}
+		return tmp;
+	}
+
+	DG_INLINE dgSpatialVector VectorTimeMatrix(const dgSpatialVector& jacobian, dgInt32 dof) const
+	{
+		dgSpatialVector tmp(dgFloat32 (0.0f));
+		for (dgInt32 i = 0; i < dof; i++) {
+			tmp = tmp + m_rows[i].Scale(jacobian[i]);
+		}
+		return tmp;
+	}
+
+	dgSpatialVector m_rows[6];
+} DG_GCC_VECTOR_ALIGNMENT;
+
+
+#endif
+

thirdparty/src/newton/dgCore/dgMemory.cpp

@@ -0,0 +1,781 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgList.h"
+#include "dgDebug.h"
+#include "dgMemory.h"
+
+
+#ifdef DG_OLD_ALLOCATOR
+dgInt32 dgMemoryAllocator::m_lock0 = 0;
+dgInt32 dgMemoryAllocator::m_lock1 = 0;
+#define DG_MEMORY_LOCK() dgScopeSpinPause lock (&dgMemoryAllocator::m_lock0);
+#define DG_MEMORY_LOCK_LOW() dgScopeSpinPause lock (&dgMemoryAllocator::m_lock1);
+
+class dgMemoryAllocator::dgMemoryBin
+{
+	public:
+	class dgMemoryBinInfo
+	{
+		public:
+		dgInt32 m_count;
+		dgInt32 m_totalCount;
+		dgInt32 m_stepInBytes;
+		dgMemoryBin* m_next;
+		dgMemoryBin* m_prev;
+	};
+	char m_pool[DG_MEMORY_BIN_SIZE - sizeof (dgMemoryBinInfo)-DG_MEMORY_GRANULARITY * 2];
+	dgMemoryBinInfo m_info;
+};
+
+class dgMemoryAllocator::dgMemoryCacheEntry
+{
+	public:
+	dgMemoryCacheEntry* m_next;
+	dgMemoryCacheEntry* m_prev;
+};
+
+class dgMemoryAllocator::dgMemoryInfo
+{
+	public:
+	void *m_ptr;
+	dgMemoryAllocator* m_allocator;
+	dgInt32 m_size;
+	dgInt32 m_enum;
+
+	#ifdef _DEBUG
+	dgInt32 m_workingSize;
+	#endif
+
+	DG_INLINE void SaveInfo(dgMemoryAllocator* const allocator, void* const ptr, dgInt32 size, dgInt32& enumerator, dgInt32 workingSize = 0)
+	{
+		m_ptr = ptr;
+		m_size = size;
+		m_enum = enumerator;
+		enumerator++;
+		m_allocator = allocator;
+#ifdef _DEBUG
+		m_workingSize = workingSize;
+#endif
+	}
+};
+
+class dgGlobalAllocator: public dgMemoryAllocator, public dgList<dgMemoryAllocator*>
+{
+	public:
+	dgGlobalAllocator ()
+		:dgMemoryAllocator (__malloc__, __free__), dgList<dgMemoryAllocator*> (NULL)
+	{
+		SetAllocator (this);
+	}
+
+	~dgGlobalAllocator ()
+	{
+		dgAssert (GetCount() == 0);
+	}
+
+	static void* dgApi __malloc__ (dgUnsigned32 size) 
+	{
+		return malloc (size);
+	}
+
+	static void dgApi __free__ (void* const ptr, dgUnsigned32 size)
+	{
+		free (ptr);
+	}
+
+	void operator delete (void* const ptr)
+	{
+		dgAssert (0);
+		free (ptr);
+	}
+
+	dgInt32 GetMemoryUsed () const
+	{
+		dgInt32 mem = m_memoryUsed;
+		for (dgList<dgMemoryAllocator*>::dgListNode* node = GetFirst(); node; node = node->GetNext()) {
+			mem += node->GetInfo()->GetMemoryUsed();
+		}
+		return mem;
+	}
+
+	static dgGlobalAllocator& GetGlobalAllocator()
+	{
+		static dgGlobalAllocator m_globalAllocator;
+		return m_globalAllocator;
+	}
+};
+
+dgMemoryAllocator::dgMemoryAllocator ()
+	:m_free(NULL)
+	,m_malloc(NULL)
+	,m_enumerator(0)
+	,m_memoryUsed(0)
+	,m_isInList(1)
+{
+	SetAllocatorsCallback (dgGlobalAllocator::GetGlobalAllocator().m_malloc, dgGlobalAllocator::GetGlobalAllocator().m_free);
+	memset (m_memoryDirectory, 0, sizeof (m_memoryDirectory));
+	dgGlobalAllocator::GetGlobalAllocator().Append(this);
+}
+
+dgMemoryAllocator::dgMemoryAllocator (dgMemAlloc memAlloc, dgMemFree memFree)
+	:m_free(NULL)
+	,m_malloc(NULL)
+	,m_enumerator(0)
+	,m_memoryUsed(0)
+	,m_isInList(0)
+{
+	SetAllocatorsCallback (memAlloc, memFree);
+	memset (m_memoryDirectory, 0, sizeof (m_memoryDirectory));
+}
+
+dgMemoryAllocator::~dgMemoryAllocator  ()
+{
+	if (m_isInList) {
+		dgGlobalAllocator::GetGlobalAllocator().Remove(this);
+	}
+	dgAssert (m_memoryUsed == 0);
+}
+
+
+void *dgMemoryAllocator::operator new (size_t size) 
+{ 
+	return dgMallocStack(size);
+}
+
+void dgMemoryAllocator::operator delete (void* const ptr) 
+{ 
+	dgFreeStack(ptr); 
+}
+
+dgInt32 dgMemoryAllocator::GetMemoryUsed() const
+{
+	return m_memoryUsed;
+}
+
+void dgMemoryAllocator::SetAllocatorsCallback (dgMemAlloc memAlloc, dgMemFree memFree)
+{
+	m_free = memFree;
+	m_malloc = memAlloc;
+}
+
+void *dgMemoryAllocator::MallocLow (dgInt32 workingSize, dgInt32 alignment)
+{
+	DG_MEMORY_LOCK_LOW();
+	alignment = dgMax (alignment, DG_MEMORY_GRANULARITY);
+	dgAssert (((-alignment) & (alignment - 1)) == 0);
+	dgInt32 size = workingSize + alignment * 2;
+	void* const ptr = m_malloc(dgUnsigned32 (size));
+	dgAssert (ptr);
+#ifdef _DEBUG
+	memset(ptr, 99, size_t(size));
+#endif
+
+	dgUnsigned64 val = dgUnsigned64 (PointerToInt(ptr));
+	val = (val & dgUnsigned64(-alignment)) + alignment * 2;
+	void* const retPtr = IntToPointer (val);
+
+	dgMemoryInfo* const info = ((dgMemoryInfo*) (retPtr)) - 1;
+	info->SaveInfo(this, ptr, size, m_enumerator, workingSize);
+
+	dgAtomicExchangeAndAdd (&m_memoryUsed, size);
+	return retPtr;
+}
+
+void dgMemoryAllocator::FreeLow (void* const retPtr)
+{
+	DG_MEMORY_LOCK_LOW();
+	dgMemoryInfo* const info = ((dgMemoryInfo*) (retPtr)) - 1;
+	dgAssert (info->m_allocator == this);
+
+	dgAtomicExchangeAndAdd (&m_memoryUsed, -info->m_size);
+
+#ifdef _DEBUG
+	memset (retPtr, 0, size_t(info->m_workingSize));
+#endif
+
+	m_free (info->m_ptr, dgUnsigned32 (info->m_size));
+}
+
+void *dgMemoryAllocator::Malloc (dgInt32 memsize)
+{
+	dgAssert (dgInt32 (sizeof (dgMemoryCacheEntry) + sizeof (dgInt32) + sizeof(dgInt32)) <= DG_MEMORY_GRANULARITY);
+
+	dgInt32 size = memsize + DG_MEMORY_GRANULARITY - 1;
+	size &= (-DG_MEMORY_GRANULARITY);
+
+	dgInt32 paddedSize = size + DG_MEMORY_GRANULARITY; 
+	dgInt32 entry = paddedSize >> DG_MEMORY_GRANULARITY_BITS;	
+
+	void* ptr;
+	if (entry >= DG_MEMORY_BIN_ENTRIES) {
+		ptr = MallocLow (size);
+	} else {
+		DG_MEMORY_LOCK();
+		if (!m_memoryDirectory[entry].m_cache) {
+			dgMemoryBin* const bin = (dgMemoryBin*) MallocLow (sizeof (dgMemoryBin));
+
+			dgInt32 count = dgInt32 (sizeof (bin->m_pool) / paddedSize);
+			bin->m_info.m_count = 0;
+			bin->m_info.m_totalCount = count;
+			bin->m_info.m_stepInBytes = paddedSize;
+			bin->m_info.m_next = m_memoryDirectory[entry].m_first;
+			bin->m_info.m_prev = NULL;
+			if (bin->m_info.m_next) {
+				bin->m_info.m_next->m_info.m_prev = bin;
+			}
+
+			m_memoryDirectory[entry].m_first = bin;
+
+			dgInt8* charPtr = reinterpret_cast<dgInt8*>(bin->m_pool);
+			m_memoryDirectory[entry].m_cache = (dgMemoryCacheEntry*)charPtr;
+
+			for (dgInt32 i = 0; i < count; i ++) {
+				dgMemoryCacheEntry* const cashe = (dgMemoryCacheEntry*) charPtr;
+				cashe->m_next = (dgMemoryCacheEntry*) (charPtr + paddedSize);
+				cashe->m_prev = (dgMemoryCacheEntry*) (charPtr - paddedSize);
+				dgMemoryInfo* const info = ((dgMemoryInfo*) (charPtr + DG_MEMORY_GRANULARITY)) - 1;						
+				info->SaveInfo(this, bin, entry, m_enumerator, memsize);
+				charPtr += paddedSize;
+			}
+			dgMemoryCacheEntry* const cashe = (dgMemoryCacheEntry*) (charPtr - paddedSize);
+			cashe->m_next = NULL;
+			m_memoryDirectory[entry].m_cache->m_prev = NULL;
+		}
+
+
+		dgAssert (m_memoryDirectory[entry].m_cache);
+
+		dgMemoryCacheEntry* const cashe = m_memoryDirectory[entry].m_cache;
+		m_memoryDirectory[entry].m_cache = cashe->m_next;
+		if (cashe->m_next) {
+			cashe->m_next->m_prev = NULL;
+		}
+
+		ptr = ((dgInt8*)cashe) + DG_MEMORY_GRANULARITY;
+
+		dgMemoryInfo* info;
+		info = ((dgMemoryInfo*) (ptr)) - 1;
+		dgAssert (info->m_allocator == this);
+
+		dgMemoryBin* const bin = (dgMemoryBin*) info->m_ptr;
+		bin->m_info.m_count ++;
+	}
+	return ptr;
+}
+
+void dgMemoryAllocator::Free (void* const retPtr)
+{
+	dgMemoryInfo* const info = ((dgMemoryInfo*) (retPtr)) - 1;
+	dgAssert (info->m_allocator == this);
+
+	dgInt32 entry = info->m_size;
+
+	if (entry >= DG_MEMORY_BIN_ENTRIES) {
+		FreeLow (retPtr);
+	} else {
+		DG_MEMORY_LOCK();
+		dgMemoryCacheEntry* const cashe = (dgMemoryCacheEntry*) (((char*)retPtr) - DG_MEMORY_GRANULARITY) ;
+		
+		dgMemoryCacheEntry* const tmpCashe = m_memoryDirectory[entry].m_cache;
+		if (tmpCashe) {
+			dgAssert (!tmpCashe->m_prev);
+			tmpCashe->m_prev = cashe;
+		}
+		cashe->m_next = tmpCashe;
+		cashe->m_prev = NULL;
+
+		m_memoryDirectory[entry].m_cache = cashe;
+
+		dgMemoryBin* const bin = (dgMemoryBin *) info->m_ptr;
+
+		dgAssert (bin);
+#ifdef _DEBUG
+		dgAssert ((bin->m_info.m_stepInBytes - DG_MEMORY_GRANULARITY) > 0);
+		memset (retPtr, 0, size_t(bin->m_info.m_stepInBytes - DG_MEMORY_GRANULARITY));
+#endif
+
+		bin->m_info.m_count --;
+		if (bin->m_info.m_count == 0) {
+
+			dgInt32 count = bin->m_info.m_totalCount;
+			dgInt32 sizeInBytes = bin->m_info.m_stepInBytes;
+			char* charPtr = bin->m_pool;
+			for (dgInt32 i = 0; i < count; i ++) {
+				dgMemoryCacheEntry* const tmpCashe1 = (dgMemoryCacheEntry*)charPtr;
+				charPtr += sizeInBytes;
+
+				if (tmpCashe1 == m_memoryDirectory[entry].m_cache) {
+					m_memoryDirectory[entry].m_cache = tmpCashe1->m_next;
+				}
+
+				if (tmpCashe1->m_prev) {
+					tmpCashe1->m_prev->m_next = tmpCashe1->m_next;
+				}
+
+				if (tmpCashe1->m_next) {
+					tmpCashe1->m_next->m_prev = tmpCashe1->m_prev;
+				}
+			}
+
+			if (m_memoryDirectory[entry].m_first == bin) {
+				m_memoryDirectory[entry].m_first = bin->m_info.m_next;
+			}
+
+			if (bin->m_info.m_next) {
+				bin->m_info.m_next->m_info.m_prev = bin->m_info.m_prev;
+			}
+			if (bin->m_info.m_prev) {
+				bin->m_info.m_prev->m_info.m_next = bin->m_info.m_next;
+			}
+
+			FreeLow (bin);
+		}
+	}
+}
+
+dgInt32 dgMemoryAllocator::GetSize (void* const retPtr)
+{
+	dgMemoryInfo* const info = ((dgMemoryInfo*)(retPtr)) - 1;
+	return info->m_size;
+}
+
+void dgMemoryAllocator::SetGlobalAllocators (dgMemAlloc malloc, dgMemFree free)
+{
+	dgGlobalAllocator::GetGlobalAllocator().SetAllocatorsCallback (malloc, free);
+}
+
+dgInt32 dgMemoryAllocator::GetGlobalMemoryUsed ()
+{
+	return dgGlobalAllocator::GetGlobalAllocator().GetMemoryUsed();
+}
+
+// this can be used by function that allocates large memory pools memory locally on the stack
+// this by pases the pool allocation because this should only be used for very large memory blocks.
+// this was using virtual memory on windows but 
+// but because of many complaint I changed it to use malloc and free
+void* dgApi dgMallocStack (size_t size)
+{
+	void * const ptr = dgGlobalAllocator::GetGlobalAllocator().MallocLow (dgInt32 (size));
+	return ptr;
+}
+
+void* dgApi dgMallocAligned (size_t size, dgInt32 align)
+{
+	void * const ptr = dgGlobalAllocator::GetGlobalAllocator().MallocLow (dgInt32 (size), align);
+	return ptr;
+}
+
+// this can be used by function that allocates large memory pools memory locally on the stack
+// this by pases the pool allocation because this should only be used for very large memory blocks.
+// this was using virtual memory on windows but 
+// but because of many complaint I changed it to use malloc and free
+void  dgApi dgFreeStack (void* const ptr)
+{
+	dgGlobalAllocator::GetGlobalAllocator().FreeLow (ptr);
+}
+
+// general memory allocation for all data in the library
+void* dgApi dgMalloc (size_t size, dgMemoryAllocator* const allocator) 
+{
+	void* ptr = NULL;
+	dgAssert (allocator);
+
+	if (size) {
+		ptr = allocator->Malloc (dgInt32 (size));
+	}
+	return ptr;
+}
+
+// general deletion allocation for all data in the library
+void dgApi dgFree (void* const ptr)
+{
+	if (ptr) {
+		dgMemoryAllocator::dgMemoryInfo* const info = ((dgMemoryAllocator::dgMemoryInfo*) ptr) - 1;
+		dgAssert (info->m_allocator);
+		info->m_allocator->Free (ptr);
+	}
+}
+
+#else
+
+
+void* dgGlobalAllocator::__malloc__(dgUnsigned32 size)
+{
+	return malloc(size);
+}
+
+void dgGlobalAllocator::__free__(void* const ptr, dgUnsigned32 size)
+{
+	free(ptr);
+}
+
+void dgGlobalAllocator::SetAllocatorsCallback (dgMemAlloc malloc, dgMemFree free)
+{
+	m_free = free;
+	m_malloc = malloc;
+}
+
+
+void* dgGlobalAllocator::Malloc(dgInt32 size)
+{
+	dgInt32 paddedSize = size + sizeof (dgMemoryAllocator::dgMemoryGranularity) + sizeof (dgMemoryAllocator::dgMemoryHeader);
+	char* const ptr = (char*)m_malloc(paddedSize);
+	
+	dgUnsigned64 address = dgUnsigned64(ptr + sizeof (dgMemoryAllocator::dgMemoryHeader) + sizeof (dgMemoryAllocator::dgMemoryGranularity)-1) & ~(sizeof (dgMemoryAllocator::dgMemoryGranularity)-1);
+	
+	dgMemoryAllocator::dgMemoryHeader* const info = ((dgMemoryAllocator::dgMemoryHeader*)address) - 1;
+	info->m_ptr = ptr;
+	info->m_allocator = this;
+	info->m_size = size;
+	info->m_paddedSize = paddedSize;
+	dgAtomicExchangeAndAdd(&m_memoryUsed, paddedSize);
+	return &info[1];
+}
+
+void dgGlobalAllocator::Free(void* const ptr)
+{
+	dgMemoryAllocator::dgMemoryHeader* const info = ((dgMemoryAllocator::dgMemoryHeader*)ptr) - 1;
+	dgAssert(info->m_allocator == this);
+
+	dgAtomicExchangeAndAdd(&m_memoryUsed, -info->m_paddedSize);
+	m_free(info->m_ptr, info->m_paddedSize);
+}
+
+dgMemoryAllocatorBase& dgGlobalAllocator::GetGlobalAllocator()
+{
+	static dgGlobalAllocator m_globalAllocator;
+	return m_globalAllocator;
+}
+
+
+dgMemoryAllocator::dgMemoryPage::dgMemoryPage(dgInt32 size, dgMemoryPage* const root, dgMemoryAllocator* const allocator)
+	:m_next(root)
+	,m_prev(NULL)
+	,m_fullPageNext(NULL)
+	,m_fullPagePrev(NULL)
+	,m_freeList (NULL)
+	,m_count(0)
+	,m_capacity(0)
+{
+	if (root) {
+		dgAssert (!root->m_prev);
+		root->m_prev = this;
+	}
+	dgInt32 paddSize = size + sizeof (dgMemoryHeader);
+	dgAssert ((paddSize & (sizeof (dgMemoryGranularity) - 1)) == 0);
+	
+	const char* const ptr1 = &m_buffer[sizeof (m_buffer) - paddSize];
+	for (char* ptr = &m_buffer[sizeof (dgMemoryGranularity)]; ptr <= ptr1; ptr += paddSize) {
+		dgAssert ((dgUnsigned64(ptr) & (sizeof (dgMemoryGranularity) - 1)) == 0);
+
+		dgMemoryGranularity* const freeList = (dgMemoryGranularity*) ptr;
+		dgMemoryHeader* const header = ((dgMemoryHeader*)freeList) - 1;
+		header->m_page = this;
+		header->m_allocator = allocator;
+		header->m_size = 0;
+		header->m_paddedSize = size;
+
+		freeList->m_next = m_freeList;
+		m_freeList = freeList;
+		m_count ++;
+	}
+
+	m_capacity = m_count;
+}
+
+dgMemoryAllocator::dgMemoryPage::~dgMemoryPage()
+{
+	dgAssert(m_count == m_capacity);
+}
+
+void *dgMemoryAllocator::dgMemoryPage::operator new (size_t size)
+{
+	dgMemoryAllocatorBase& globalAllocator = dgGlobalAllocator::GetGlobalAllocator();
+	return globalAllocator.Malloc(dgInt32 (size));
+}
+
+void dgMemoryAllocator::dgMemoryPage::operator delete (void* const ptr)
+{
+	dgMemoryAllocatorBase& globalAllocator = dgGlobalAllocator::GetGlobalAllocator();
+	globalAllocator.Free(ptr);
+}
+
+void* dgMemoryAllocator::dgMemoryPage::Malloc(dgInt32 size)
+{
+	m_count --;
+	dgAssert (m_count >= 0);
+	dgMemoryGranularity* const freeList = m_freeList;
+	m_freeList = m_freeList->m_next;
+
+	dgMemoryHeader* const header = ((dgMemoryHeader*)freeList) - 1;
+	header->m_size = size;
+	return freeList;
+}
+
+void dgMemoryAllocator::dgMemoryPage::Free(void* const ptr)
+{
+	m_count++;
+	dgAssert(m_count <= m_capacity);
+	dgMemoryHeader* const info = ((dgMemoryHeader*)ptr) - 1;
+	info->m_size = 0;
+
+	dgMemoryGranularity* const freeList = (dgMemoryGranularity*) ptr;
+	freeList->m_next = m_freeList;
+	m_freeList = freeList;
+}
+
+dgMemoryAllocator::dgMemoryBeam::dgMemoryBeam()
+	:m_firstPage(NULL)
+	,m_fullPage(NULL)
+	,m_beamSize(0)
+	,m_inUsedCount(0)
+	,m_fullPageCount(0)
+{
+}
+
+dgMemoryAllocator::dgMemoryBeam::~dgMemoryBeam()
+{
+	while (m_firstPage) {
+		if (m_firstPage->m_next) {
+			m_firstPage->m_next->m_prev = NULL;
+		}
+		dgMemoryPage* const firstPage = m_firstPage;
+		m_firstPage = m_firstPage->m_next;
+		delete firstPage;
+	}
+
+	while (m_fullPage) {
+		dgAssert(0);
+		if (m_fullPage->m_fullPageNext) {
+			m_fullPage->m_fullPageNext->m_fullPagePrev = NULL;
+		}
+		dgMemoryPage* const fullPage = m_fullPage;
+		m_fullPage = m_fullPage->m_fullPageNext;
+		delete fullPage;
+	}
+
+	m_fullPage = NULL;
+	m_firstPage = NULL;
+}
+
+void* dgMemoryAllocator::dgMemoryBeam::Malloc(dgInt32 size)
+{
+	dgAssert (size <= m_beamSize);
+	if (!m_firstPage) {
+		m_firstPage = new dgMemoryPage (m_beamSize, m_firstPage, m_allocator);
+		m_inUsedCount ++;
+	}
+
+	dgAssert (m_firstPage->m_count);
+	//if (m_firstPage->m_count == 0) {
+	//	m_firstPage = new dgMemoryPage (m_beamSize, m_firstPage, m_allocator);
+	//}
+
+	void* ptr = m_firstPage->Malloc(size);
+	if (m_firstPage->m_count == 0) {
+		dgMemoryPage* const page = m_firstPage;
+
+		m_inUsedCount --;
+		m_fullPageCount ++;
+
+		m_firstPage = page->m_next;
+		if (m_firstPage) {
+			m_firstPage->m_prev = NULL;
+		}
+		page->m_next = NULL;
+		page->m_prev = NULL;
+
+		dgAssert(!page->m_fullPageNext);
+		dgAssert(!page->m_fullPagePrev);
+		page->m_fullPageNext = m_fullPage;
+		if (m_fullPage) {
+			m_fullPage->m_fullPagePrev = page;
+		}
+		m_fullPage = page;
+	}
+	return ptr;
+}
+
+void dgMemoryAllocator::dgMemoryBeam::Free(void* const ptr)
+{
+	dgMemoryHeader* const info = ((dgMemoryHeader*)ptr) - 1;
+	dgMemoryPage* const page = info->m_page;
+
+	page->Free(ptr);
+	dgAssert (page->m_count <= page->m_capacity);
+	if (page->m_count == page->m_capacity) {
+		m_inUsedCount --;
+		if (page == m_firstPage) {
+			m_firstPage = m_firstPage->m_next;
+		}
+		if (page->m_next) {
+			page->m_next->m_prev = page->m_prev;
+		}
+		if (page->m_prev) {
+			page->m_prev->m_next = page->m_next;
+		}
+		page->m_next = NULL;
+		page->m_prev = NULL;
+		delete page;
+	} else if (page->m_count == 1) {
+
+		m_inUsedCount++;
+		m_fullPageCount--;
+
+		if (page == m_fullPage) {
+			m_fullPage = m_fullPage->m_fullPageNext;
+		}
+		if (page->m_fullPageNext) {
+			page->m_fullPageNext->m_fullPagePrev = page->m_fullPagePrev;
+		}
+		if (page->m_fullPagePrev) {
+			page->m_fullPagePrev->m_fullPageNext = page->m_fullPageNext;
+		}
+		page->m_fullPagePrev = NULL;
+		page->m_fullPageNext = NULL;
+
+		dgAssert(!page->m_next);
+		dgAssert(!page->m_prev);
+		page->m_next = m_firstPage;
+		if (m_firstPage) {
+			m_firstPage->m_prev = page;
+		}
+		m_firstPage = page;
+
+//	} else if (page->m_prev) {
+//		dgInt32 key = page->GetSortKey();
+//		dgMemoryPage* prevPage = page->m_prev;
+//		while (prevPage && prevPage->GetSortKey() < key)
+//		{
+//			prevPage = prevPage->m_prev;
+//		}
+//
+//		if (prevPage) {
+//			dgAssert(0);
+//		} else {
+//			dgAssert(0);
+//		}
+	}
+}
+
+void dgMemoryAllocator::dgMemoryBeam::Init(dgInt32 size, dgMemoryAllocator* const allocator)
+{
+	m_beamSize = size;
+	m_allocator = allocator;
+}
+
+dgMemoryAllocator::dgMemoryAllocator()
+{
+//	for (dgInt32 i = 0; i < DG_MEMORY_BEAMS_COUNT; i++) {
+//		dgInt32 size = ((dgInt32 (sizeof (dgMemoryGranularity) * (dgPow(dgFloat32(1.6f), i + 2) - dgPow(dgFloat32(1.6f), i + 1))) + sizeof(dgMemoryGranularity) - 1) & -dgInt32 (sizeof(dgMemoryGranularity))) - sizeof (dgMemoryHeader);
+//		m_beams[i].Init (size, this);
+//	}
+
+	dgInt32 index = 0;
+	dgInt32 size0 = 0;
+	dgFloat32 base = dgFloat32(1.3f);
+	dgFloat32 exp = dgFloat32 (0.0f);
+	while (index < DG_MEMORY_BEAMS_COUNT) {
+		dgFloat32 x0 = dgPow(base, exp);
+		dgFloat32 x1 = dgPow(base, exp + dgFloat32(1.0f));
+		dgInt32 size = ((dgInt32(sizeof(dgMemoryGranularity) * (x1 - x0) + sizeof(dgMemoryGranularity) - 1)) & -dgInt32(sizeof(dgMemoryGranularity))) - sizeof(dgMemoryHeader);
+		exp += dgFloat32(1.0f);
+		if (size > size0) {
+			size0 = size;
+			m_beams[index].Init(size, this);
+			index++;
+		}
+	}
+}
+
+dgMemoryAllocator::~dgMemoryAllocator()
+{
+}
+
+void *dgMemoryAllocator::operator new (size_t size)
+{
+	dgMemoryAllocatorBase& globalAllocator = dgGlobalAllocator::GetGlobalAllocator();
+	return globalAllocator.Malloc(dgInt32 (size));
+}
+
+void dgMemoryAllocator::operator delete (void* const ptr)
+{
+	dgMemoryAllocatorBase& globalAllocator = dgGlobalAllocator::GetGlobalAllocator();
+	globalAllocator.Free (ptr);
+}
+
+void dgMemoryAllocator::SetGlobalAllocators(dgMemAlloc malloc, dgMemFree free)
+{
+	dgGlobalAllocator* const globalAllocator = (dgGlobalAllocator*)&dgGlobalAllocator::GetGlobalAllocator();
+	globalAllocator->SetAllocatorsCallback(malloc, free);
+}
+
+dgInt32 dgMemoryAllocator::GetGlobalMemoryUsed()
+{
+	dgGlobalAllocator* const globalAllocator = (dgGlobalAllocator*)&dgGlobalAllocator::GetGlobalAllocator();
+	return globalAllocator->GetMemoryUsed();
+}
+
+dgInt32 dgMemoryAllocator::GetSize (void* const ptr)
+{
+	dgMemoryHeader* const info = ((dgMemoryHeader*)ptr) - 1;
+	return info->m_size;
+}
+
+void* dgMemoryAllocator::Malloc(dgInt32 size)
+{
+	dgMemoryAllocatorBase& globalAllocator = dgGlobalAllocator::GetGlobalAllocator();
+	if (size > m_beams[DG_MEMORY_BEAMS_COUNT-1].m_beamSize) {
+		return globalAllocator.Malloc(size);
+	} else {
+		dgMemoryBeam* const beam = FindBeam(size);
+		return beam->Malloc(size);
+	}
+}
+
+void dgMemoryAllocator::Free(void* const ptr)
+{
+	dgMemoryHeader* const info = ((dgMemoryHeader*)ptr) - 1;
+	dgMemoryAllocatorBase& globalAllocator = dgGlobalAllocator::GetGlobalAllocator();
+	if (info->m_size > m_beams[DG_MEMORY_BEAMS_COUNT - 1].m_beamSize) {
+		globalAllocator.Free (ptr);
+	} else {
+		dgMemoryBeam* const beam = FindBeam(info->m_size);
+		beam->Free(ptr);
+	}
+}
+
+dgMemoryAllocator::dgMemoryBeam* dgMemoryAllocator::FindBeam(dgInt32 size)
+{
+	dgInt32 i = m_beams[DG_MEMORY_BEAMS_COUNT / 2].m_beamSize >= size ? 0 : DG_MEMORY_BEAMS_COUNT / 2;
+	for (; i < DG_MEMORY_BEAMS_COUNT; i++) {
+		if (m_beams[i].m_beamSize >= size) {
+			return &m_beams[i];
+		}
+	}
+	dgAssert(0);
+	return NULL;
+}
+
+
+#endif

thirdparty/src/newton/dgCore/dgMemory.h

@@ -0,0 +1,352 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgMemory__
+#define __dgMemory__
+
+#include "dgStdafx.h"
+
+class dgMemoryAllocator;
+
+#define DG_CLASS_ALLOCATOR_NEW(allocator)			DG_INLINE void* operator new (size_t size, dgMemoryAllocator* const allocator) { return dgMalloc(size, allocator);}
+#define DG_CLASS_ALLOCATOR_NEW_ARRAY(allocator)		DG_INLINE void* operator new[] (size_t size, dgMemoryAllocator* const allocator) { return dgMalloc(size, allocator);}
+#define DG_CLASS_ALLOCATOR_DELETE(allocator)		DG_INLINE void  operator delete (void* const ptr, dgMemoryAllocator* const allocator) { dgFree(ptr); }
+#define DG_CLASS_ALLOCATOR_DELETE_ARRAY(allocator)	DG_INLINE void  operator delete[] (void* const ptr, dgMemoryAllocator* const allocator) { dgFree(ptr); }
+#define DG_CLASS_ALLOCATOR_NEW_DUMMY				DG_INLINE void* operator new (size_t size) { dgAssert (0); return dgMalloc(size, NULL);}
+#define DG_CLASS_ALLOCATOR_NEW_ARRAY_DUMMY			DG_INLINE void* operator new[] (size_t size) { dgAssert (0); return dgMalloc(size, NULL);}
+#define DG_CLASS_ALLOCATOR_DELETE_DUMMY				DG_INLINE void  operator delete (void* const ptr) { dgFree(ptr); }
+#define DG_CLASS_ALLOCATOR_DELETE_ARRAY_DUMMY		DG_INLINE void  operator delete[] (void* const ptr) { dgFree(ptr); }
+
+
+#define DG_CLASS_ALLOCATOR(allocator)				\
+	DG_CLASS_ALLOCATOR_DELETE(allocator)			\
+	DG_CLASS_ALLOCATOR_DELETE_ARRAY(allocator)		\
+	DG_CLASS_ALLOCATOR_NEW(allocator)				\
+	DG_CLASS_ALLOCATOR_NEW_ARRAY(allocator)			\
+	DG_CLASS_ALLOCATOR_NEW_DUMMY					\
+	DG_CLASS_ALLOCATOR_NEW_ARRAY_DUMMY				\
+	DG_CLASS_ALLOCATOR_DELETE_DUMMY					\
+	DG_CLASS_ALLOCATOR_DELETE_ARRAY_DUMMY
+
+typedef void* (dgApi *dgMemAlloc) (dgUnsigned32 size);
+typedef void (dgApi *dgMemFree) (void* const ptr, dgUnsigned32 size);
+
+#define DG_OLD_ALLOCATOR
+
+#ifdef DG_OLD_ALLOCATOR
+
+void* dgApi dgMalloc (size_t size, dgMemoryAllocator* const allocator);
+void  dgApi dgFree (void* const ptr);
+
+void* dgApi dgMallocStack (size_t size);
+void* dgApi dgMallocAligned (size_t size, dgInt32 alignmentInBytes);
+void  dgApi dgFreeStack (void* const ptr);
+
+
+
+class dgMemoryAllocator
+{
+	#if (defined (__LP64__) || defined (_WIN_64_VER) || defined (__MINGW64__) || defined (_POSIX_VER_64) || defined (_MACOSX_VER))
+		#define DG_MEMORY_GRANULARITY_BITS		6	
+	#else
+		#define DG_MEMORY_GRANULARITY_BITS		5	
+	#endif
+	#define DG_MEMORY_GRANULARITY				(1 << DG_MEMORY_GRANULARITY_BITS)	
+	#define DG_MEMORY_SIZE						(1024 - 64)
+	#define DG_MEMORY_BIN_SIZE					(1024 * 16)
+	#define DG_MEMORY_BIN_ENTRIES				(DG_MEMORY_SIZE / DG_MEMORY_GRANULARITY)
+
+	public: 
+	class dgMemoryBin;
+	class dgMemoryInfo;
+	class dgMemoryCacheEntry;
+
+	class dgMemDirectory
+	{
+		public: 
+		dgMemoryBin* m_first;
+		dgMemoryCacheEntry* m_cache;
+	};
+
+	dgMemoryAllocator ();
+	virtual ~dgMemoryAllocator ();
+
+	void *operator new (size_t size);
+	void operator delete (void* const ptr);
+	dgInt32 GetMemoryUsed() const;
+
+	void SetAllocatorsCallback (dgMemAlloc memAlloc, dgMemFree memFree);
+	virtual void *MallocLow (dgInt32 size, dgInt32 alignment = DG_MEMORY_GRANULARITY);
+	virtual void FreeLow (void* const retPtr);
+	virtual void *Malloc (dgInt32 memsize);
+	virtual void Free (void* const retPtr);
+	virtual int GetSize (void* const retPtr);
+
+	static dgInt32 GetGlobalMemoryUsed ();
+	static void SetGlobalAllocators (dgMemAlloc alloc, dgMemFree free);
+
+	protected:
+	dgMemoryAllocator (bool init)
+		:m_free(NULL)
+		,m_malloc(NULL)
+		,m_enumerator(0)
+		,m_memoryUsed(0)
+		,m_isInList(0)
+	{	
+	}
+
+	dgMemoryAllocator (dgMemAlloc memAlloc, dgMemFree memFree);
+
+	dgMemFree m_free;
+	dgMemAlloc m_malloc;
+	dgMemDirectory m_memoryDirectory[DG_MEMORY_BIN_ENTRIES + 1]; 
+	dgInt32 m_enumerator;
+	dgInt32 m_memoryUsed;
+	dgInt32 m_isInList;
+
+	public:
+	static dgInt32 m_lock0;
+	static dgInt32 m_lock1;
+};
+
+class dgStackMemoryAllocator: public dgMemoryAllocator 
+{
+	public:
+	DG_INLINE dgStackMemoryAllocator(void* const pool, dgInt32 size)
+		:dgMemoryAllocator (false)
+		,m_pool((dgInt8*) pool)
+		,m_index(0)
+		,m_size(size)
+	{
+	}
+
+	DG_INLINE ~dgStackMemoryAllocator()
+	{
+	}
+
+	DG_INLINE void* Alloc(dgInt32 size)
+	{
+		dgInt8* const ptr = (dgInt8*) (reinterpret_cast<uintptr_t>(m_pool + m_index + 15) & -0x10);
+		m_index = dgInt32 (ptr - m_pool) + size;
+		dgAssert (m_index < m_size);
+		return ptr;
+	}
+
+	DG_INLINE void* Malloc(dgInt32 size)
+	{
+		return Alloc(size);
+	}
+
+	DG_INLINE void* MallocLow(dgInt32 size, dgInt32 alignment)
+	{
+		return Alloc(size);
+	}
+
+	DG_INLINE void Free(void* const retPtr)
+	{
+	}
+
+	DG_INLINE void FreeLow(void* const retPtr)
+	{
+	}
+
+	dgInt8* m_pool;
+	dgInt32 m_index;
+	dgInt32 m_size;
+};
+
+#else
+
+
+class dgMemoryAllocatorBase
+{
+	public:
+	dgMemoryAllocatorBase() {}
+	virtual ~dgMemoryAllocatorBase() {}
+
+	virtual void* Malloc(dgInt32 size) = 0;
+	virtual void Free(void* const ptr) = 0;
+};
+
+class dgGlobalAllocator: public dgMemoryAllocatorBase
+{
+	public:
+	dgGlobalAllocator()
+		:dgMemoryAllocatorBase()
+		,m_free(__free__)
+		,m_malloc(__malloc__)
+		,m_memoryUsed(0)
+	{
+	}
+
+	~dgGlobalAllocator() 
+	{
+	}
+
+	void SetAllocatorsCallback(dgMemAlloc malloc, dgMemFree free);
+
+	static dgMemoryAllocatorBase& GetGlobalAllocator();
+	dgInt32 GetMemoryUsed() const { return m_memoryUsed; }
+
+	private:
+	static void* dgApi __malloc__(dgUnsigned32 size);
+	static void dgApi __free__(void* const ptr, dgUnsigned32 size);
+	
+	void* Malloc(dgInt32 size);
+	void Free(void* const ptr);
+
+	dgMemFree m_free;
+	dgMemAlloc m_malloc;
+	dgInt32 m_memoryUsed;
+};
+
+
+class dgMemoryAllocator: public dgMemoryAllocatorBase
+{
+	public:
+	#define DG_MEMORY_GRANULARITY_BITS	6	
+	#define DG_MEMORY_GRANULARITY		(1 << DG_MEMORY_GRANULARITY_BITS)	
+	#define DG_MEMORY_BEAMS_COUNT		16
+//	#define DG_MEMORY_BEAMS_COUNT		1
+	#define DG_MEMORY_BEAMS_BUFFER_SIZE	(1024 * 32)
+
+	class dgMemoryPage;
+	class dgMemoryHeader
+	{
+		public:
+		dgMemoryAllocatorBase* m_allocator;
+		union {
+			void* m_ptr;
+			dgMemoryPage* m_page;
+		};
+		dgInt32 m_size;
+		dgInt32 m_paddedSize;
+	};
+
+	class dgMemoryGranularity
+	{
+		public:
+		union
+		{
+			dgMemoryGranularity* m_next;
+			char m_padd1[DG_MEMORY_GRANULARITY];
+		};
+	};
+
+	class dgMemoryPage
+	{
+		public:
+		dgMemoryPage(dgInt32 size, dgMemoryPage* const root, dgMemoryAllocator* const allocator);
+		~dgMemoryPage();
+		void *operator new (size_t size);
+		void operator delete (void* const ptr);
+
+		void* Malloc(dgInt32 size);
+		void Free(void* const ptr);
+
+		char m_buffer[DG_MEMORY_BEAMS_BUFFER_SIZE];
+
+		dgMemoryPage* m_next;
+		dgMemoryPage* m_prev;
+		dgMemoryPage* m_fullPageNext;
+		dgMemoryPage* m_fullPagePrev;
+		dgMemoryGranularity* m_freeList;
+		dgInt32 m_count;
+		dgInt32 m_capacity;
+	};
+
+	class dgMemoryBeam
+	{
+		public:
+		dgMemoryBeam();
+		~dgMemoryBeam();
+		void Init(dgInt32 size, dgMemoryAllocator* const allocator);
+
+		void* Malloc(dgInt32 size);
+		void Free(void* const ptr);
+
+		dgMemoryPage* m_firstPage;
+		dgMemoryPage* m_fullPage;
+		dgMemoryAllocator* m_allocator;
+		dgInt32 m_beamSize;
+
+		dgInt32 m_inUsedCount;
+		dgInt32 m_fullPageCount;
+	};
+
+	dgMemoryAllocator();
+	virtual ~dgMemoryAllocator();
+
+	virtual void* Malloc(dgInt32 size);
+	virtual void Free(void* const ptr);
+	virtual dgInt32 GetSize (void* const ptr);
+
+	void* MallocLow(dgInt32 size, dgInt32 aligment=DG_MEMORY_GRANULARITY)
+	{
+		return Malloc(size);
+	}
+
+	void FreeLow(void* const ptr)
+	{
+		Free (ptr);
+	}
+
+	static dgInt32 GetGlobalMemoryUsed();
+	static void SetGlobalAllocators(dgMemAlloc alloc, dgMemFree free);
+
+	void *operator new (size_t size);
+	void operator delete (void* const ptr);
+
+	private:
+	dgMemoryBeam* FindBeam(dgInt32 size);
+
+	dgMemoryBeam m_beams[DG_MEMORY_BEAMS_COUNT];
+};
+
+
+DG_INLINE void* dgMalloc(size_t size, dgMemoryAllocatorBase* const allocator)
+{
+	void* const ptr = allocator->Malloc(dgInt32(size));
+	return ptr;
+}
+
+DG_INLINE void dgFree(void* const ptr)
+{
+	dgMemoryAllocator::dgMemoryHeader* const info = ((dgMemoryAllocator::dgMemoryHeader*)ptr) - 1;
+	dgAssert(info->m_allocator);
+	info->m_allocator->Free(ptr);
+}
+
+DG_INLINE void* dgMallocStack(size_t size)
+{
+	return dgMalloc(size, (dgGlobalAllocator*) &dgGlobalAllocator::GetGlobalAllocator());
+}
+
+DG_INLINE void dgFreeStack(void* const ptr)
+{
+	dgFree(ptr);
+}
+
+
+#endif
+
+#endif
+

thirdparty/src/newton/dgCore/dgMutexThread.cpp

@@ -0,0 +1,124 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgThread.h"
+#include "dgMutexThread.h"
+
+
+dgMutexThread::dgMutexThread(const char* const name, dgInt32 id)
+	:dgThread(name, id)
+	,m_mutex()
+	,m_parentMutex()
+{
+	Init ();
+}
+
+dgMutexThread::~dgMutexThread(void)
+{
+	Terminate();
+}
+
+void dgMutexThread::Terminate()
+{
+	if (IsThreadActive()) {
+		dgInterlockedExchange(&m_terminate, 1);
+		m_mutex.Release();
+		Close();
+	}
+} 
+
+void dgMutexThread::Execute (dgInt32 threadID)
+{
+	// suspend this tread until the call thread decide to 
+	dgAssert (threadID == m_id);
+	while (!m_terminate) {
+		// wait for the main thread to signal an update
+		m_mutex.Wait();
+		if (!m_terminate) {
+			TickCallback(threadID);
+		}
+		m_parentMutex.Release();
+	}
+}
+
+void dgMutexThread::Tick()
+{
+	// let the thread run until the update function return  
+	m_mutex.Release();
+	m_parentMutex.Wait();
+}
+
+dgAsyncThread::dgAsyncThread(const char* const name, dgInt32 id)
+	:dgThread(name, id)
+	,m_mutex()
+	,m_inUpdate(0)
+	,m_beginUpdate(0)
+{
+	Init();
+}
+
+dgAsyncThread::~dgAsyncThread(void)
+{
+	Terminate();
+}
+
+void dgAsyncThread::Terminate()
+{
+	if (IsThreadActive()) {
+		dgInterlockedExchange(&m_terminate, 1);
+		m_mutex.Release();
+		Close();
+	}
+}
+
+void dgAsyncThread::Sync()
+{
+	while (m_inUpdate) {
+		dgThreadYield();
+	}
+}
+
+void dgAsyncThread::Tick()
+{
+	// let the thread run until the update function return  
+	Sync();
+	m_beginUpdate = 0;
+	m_mutex.Release();
+	while (!m_beginUpdate) {
+		dgThreadPause();
+	}
+}
+
+
+void dgAsyncThread::Execute(dgInt32 threadID)
+{
+	dgAssert(threadID == m_id);
+	while (!m_terminate) {
+		m_mutex.Wait();
+		if (!m_terminate) {
+			dgInterlockedExchange(&m_inUpdate, 1);
+			dgInterlockedExchange(&m_beginUpdate, 1);
+			TickCallback(threadID);
+			dgInterlockedExchange(&m_inUpdate, 0);
+		}
+	}
+}

thirdparty/src/newton/dgCore/dgMutexThread.h

@@ -0,0 +1,66 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DG_MUTEX_THREAD_H__
+#define __DG_MUTEX_THREAD_H__
+
+#include "dgTypes.h"
+#include "dgThread.h"
+
+class dgMutexThread: public dgThread
+{
+	public:
+	dgMutexThread(const char* const name, dgInt32 id);
+	virtual ~dgMutexThread(void);
+
+	void Tick(); 
+	void Terminate(); 
+
+	protected:
+	virtual void Execute (dgInt32 threadID);
+	virtual void TickCallback (dgInt32 threadID) = 0;
+
+	private:
+	dgSemaphore m_mutex;
+	dgSemaphore m_parentMutex;
+};
+
+
+class dgAsyncThread: public dgThread
+{
+	public:
+	dgAsyncThread(const char* const name, dgInt32 id);
+	virtual ~dgAsyncThread(void);
+
+	void Tick();
+	void Sync();
+	void Terminate();
+
+	protected:
+	virtual void Execute(dgInt32 threadID);
+	virtual void TickCallback(dgInt32 threadID) = 0;
+
+	private:
+	dgSemaphore m_mutex;
+	dgInt32 m_inUpdate;
+	dgInt32 m_beginUpdate;
+};
+#endif

thirdparty/src/newton/dgCore/dgNode.cpp

@@ -0,0 +1,231 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgNode.h"
+
+
+dgInitRtti(dgBaseNode);
+
+dgBaseNode::dgBaseNode (const dgBaseNode &clone)
+	:dgRef (clone)
+{
+	Clear ();
+
+	for (dgBaseNode* obj = clone.child; obj; obj = obj->sibling) {
+		dgBaseNode* newObj = (dgBaseNode *)obj->CreateClone ();
+		newObj->Attach (this);
+		newObj->Release();
+	}
+}
+
+
+dgBaseNode::~dgBaseNode () 
+{
+	if (child) {
+		dgBaseNode* tmp;
+		for (dgBaseNode* ptr = child; ptr && ptr->Release(); ptr = tmp) {
+			ptr->Kill();
+			tmp = ptr->sibling;
+			ptr->parent = NULL;
+			ptr->sibling = NULL;
+		}
+	}
+	
+	dgBaseNode* tmp;
+	for (dgBaseNode* ptr = sibling; ptr && ptr->Release(); ptr = tmp) {
+		ptr->Kill();
+		tmp = ptr->sibling;
+		ptr->parent = NULL;
+		ptr->sibling = NULL;
+	}
+}
+
+void dgBaseNode::CloneFixUp (const dgBaseNode &clone)
+{
+	dgAssert (GetNameID() == clone.GetNameID());
+
+	dgBaseNode* cloneChild = clone.GetChild();
+	for (dgBaseNode* obj = child; obj; obj = obj->sibling) {
+		obj->CloneFixUp (*cloneChild);
+		cloneChild = cloneChild->GetSibling();
+	}
+}
+
+
+void dgBaseNode::Attach (dgBaseNode *parentArg, bool addFirst)
+{
+	dgAssert (!parent);
+	dgAssert (!sibling);
+	dgAssert (parentArg);
+
+	
+	parent = parentArg;
+	if (parent->child) {
+		if (addFirst) {
+			sibling = parent->child;
+			parent->child = this;
+		} else {
+			dgBaseNode* obj = parent->child;
+			for (; obj->sibling; obj = obj->sibling)
+			{
+				
+			}
+			obj->sibling = this;
+		}
+	} else {
+		parent->child = this;
+	}
+
+	AddRef();
+}
+
+
+void dgBaseNode::Detach ()
+{
+ 	if (parent) {
+		if (parent->child == this) {
+			parent->child = sibling;
+		} else {
+			dgBaseNode* ptr = parent->child;
+			for (; ptr->sibling != this; ptr = ptr->sibling)
+			{
+				
+			}
+			ptr->sibling = sibling;
+		}
+		parent = NULL;
+		sibling = NULL;
+		Release();
+	}
+}
+	
+
+dgBaseNode* dgBaseNode::GetRoot() const
+{
+	const dgBaseNode* root = this;
+	for (; root->parent; root = root->parent)
+	{
+		
+	}
+	return (dgBaseNode*)root;
+}
+
+
+dgBaseNode* dgBaseNode::GetFirst() const
+{
+	dgBaseNode* ptr = (dgBaseNode *)this;
+	for (; ptr->child; ptr = ptr->child)
+	{
+		
+	}
+	return ptr;
+}
+
+dgBaseNode* dgBaseNode::GetNext() const
+{
+	if (sibling) {
+		return sibling->GetFirst();
+	}
+
+	dgBaseNode* ptr = parent;
+	dgBaseNode* x = (dgBaseNode *)this;
+	for (; ptr && (x == ptr->sibling); ptr = ptr->parent) {
+		x = ptr;
+	}
+	return ptr;
+}
+
+
+
+dgBaseNode* dgBaseNode::GetLast() const
+{
+
+	dgBaseNode* ptr = (dgBaseNode *)this;	
+	for (ptr = (dgBaseNode *)this; ptr->sibling; ptr = ptr->sibling)
+	{
+		
+	}
+	return ptr;
+}
+
+
+dgBaseNode* dgBaseNode::GetPrev() const
+{
+	if (child) {
+		return child->GetNext();
+	}
+
+	dgBaseNode* ptr = parent;
+	dgBaseNode* x = (dgBaseNode *)this;
+	for (; ptr && (x == ptr->child); ptr = ptr->child) {
+		x = ptr;
+	}
+	return ptr;
+}
+
+
+
+dgBaseNode* dgBaseNode::Find (dgUnsigned32 nameCRC) const 
+{
+	dgBaseNode *ptr = GetFirst();
+	for (; ptr; ptr = ptr->GetNext()) {
+		if (nameCRC == ptr->GetNameID()) {
+			break;
+		}
+	}
+	return ptr;
+}
+
+bool dgBaseNode::SanityCheck()
+{
+	return true;
+}
+
+void dgBaseNode::PrintHierarchy (
+	dgFile &file, 
+	char *indent) const 
+{
+	char newIndent[1024];
+
+	sprintf (newIndent, "%s   ", indent);
+	for (dgBaseNode *node = child; node; node = node->sibling) {
+		node->PrintHierarchy (file, newIndent);
+	} 
+}
+
+
+void dgBaseNode::DebugPrint (const char *fileName)
+{
+/*
+	char indent[512];
+
+	indent[0] = '\0';
+	dgFile file (fileName, "w");
+	PrintHierarchy (file, indent); 
+*/
+}
+
+
+
+
+
+

thirdparty/src/newton/dgCore/dgNode.h

@@ -0,0 +1,262 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgNode__
+#define __dgNode__
+
+#include "dgStdafx.h"
+#include "dgCRC.h"
+#include "dgRef.h"
+#include "dgRtti.h"
+
+
+class dgFile;
+//enum dgSaveType;
+
+
+class dgBaseNode: public dgRef
+{
+	public:
+	dgBaseNode *GetChild () const;
+	dgBaseNode *GetParent () const;
+	dgBaseNode *GetSibling () const;
+
+	void Detach ();
+	void Attach (dgBaseNode* const parent, bool addFirst = false);
+	
+	dgBaseNode *GetRoot () const;
+	dgBaseNode *GetFirst() const;
+	dgBaseNode *GetLast() const;
+	dgBaseNode *GetNext() const;
+	dgBaseNode *GetPrev() const;
+
+	dgBaseNode *Find (dgUnsigned32 nameCRC) const; 
+	dgBaseNode *Find (const char* const name) const;
+	void DebugPrint (const char* const fileName); 
+	bool SanityCheck();
+
+
+	protected:
+	dgBaseNode ();
+	dgBaseNode (const char* const name);
+	dgBaseNode (const dgBaseNode &clone);
+	~dgBaseNode ();
+
+//	virtual void Save (dgFile &file, dgSaveType saveType, void* const context) const; 
+	virtual void CloneFixUp (const dgBaseNode &clone);
+	virtual void PrintHierarchy (dgFile &file, char* indentation) const; 
+
+	private:
+	inline void Clear();
+
+	dgAddRtti(dgRef);
+	dgBaseNode* parent;
+	dgBaseNode* child;
+	dgBaseNode* sibling;
+
+};
+
+template<class T>
+class dgNode: public dgBaseNode
+{
+	public:
+	dgNode ();
+	dgNode (const char* const name);
+	void Attach (T* parent, bool addFirst = false);
+	void Detach ();
+	T* GetChild () const;
+	T* GetSibling () const;
+	T* GetParent () const;
+	T* GetRoot () const;
+	T* GetFirst() const;
+	T* GetLast() const;
+	T* GetNext() const;
+	T* GetPrev() const;
+	T* Find (dgUnsigned32 nameCRC) const;
+	T* Find (const char* const name) const;
+
+	protected:
+	dgNode (const T &clone);
+	virtual ~dgNode ();
+	dgRef *CreateClone () const;
+
+};
+
+
+
+
+
+inline dgBaseNode::dgBaseNode ()
+	:dgRef ()
+{
+	Clear ();
+}
+
+inline dgBaseNode::dgBaseNode (const char* const name)
+	:dgRef (name)
+{
+	Clear ();
+}
+
+
+inline void dgBaseNode::Clear()
+{
+	child = NULL;
+	parent = NULL;
+	sibling = NULL;
+}
+
+
+inline dgBaseNode *dgBaseNode::GetChild () const
+{
+	return child;
+}
+
+inline dgBaseNode *dgBaseNode::GetSibling () const
+{
+	return sibling;
+}
+
+inline dgBaseNode *dgBaseNode::GetParent () const
+{
+	return parent;
+}
+
+
+inline dgBaseNode *dgBaseNode::Find (const char* const name) const
+{
+	return Find (dgCRC (name)); 
+} 
+
+
+
+
+template<class T>
+dgNode<T>::dgNode ()
+	:dgBaseNode ()
+{
+}
+
+template<class T>
+dgNode<T>::dgNode (const T &clone)
+	:dgBaseNode (clone)
+{
+}
+
+template<class T>
+dgNode<T>::dgNode (const char* const name)
+	:dgBaseNode (name)
+{
+}
+
+template<class T>
+dgNode<T>::~dgNode () 
+{
+}
+
+
+template<class T>
+dgRef *dgNode<T>::CreateClone () const
+{
+	return new T (*(T*)this);
+}
+
+template<class T>
+void dgNode<T>::Attach (T* const parent, bool addFirst)
+{
+	dgBaseNode::Attach(parent, addFirst);
+}
+
+template<class T>
+void dgNode<T>::Detach ()
+{
+	dgBaseNode::Detach ();
+}
+
+template<class T>
+T* dgNode<T>::GetChild () const
+{
+	return (T*) dgBaseNode::GetChild();
+}
+
+template<class T>
+T* dgNode<T>::GetSibling () const
+{
+	return (T*) dgBaseNode::GetSibling ();
+}
+
+template<class T>
+T* dgNode<T>::GetParent () const
+{
+	return (T*) dgBaseNode::GetParent ();
+}
+
+
+template<class T>
+T* dgNode<T>::GetRoot () const
+{
+	return (T*) dgBaseNode::GetRoot ();
+}
+
+
+template<class T>
+T* dgNode<T>::GetFirst() const
+{
+	return (T*) dgBaseNode::GetFirst ();
+}
+
+template<class T>
+T* dgNode<T>::GetLast() const
+{
+	return (T*) dgBaseNode::GetLast ();
+}
+
+
+template<class T>
+T* dgNode<T>::GetNext() const
+{
+	return (T*) dgBaseNode::GetNext ();
+}
+
+template<class T>
+T* dgNode<T>::GetPrev() const
+{
+	return (T*) dgBaseNode::GetPrev ();
+}
+
+
+template<class T>
+T* dgNode<T>::Find (dgUnsigned32 nameCRC) const 
+{
+	return (T*) dgBaseNode::Find (nameCRC);
+}
+
+template<class T>
+T* dgNode<T>::Find (const char* const name) const
+{
+	return (T*) dgBaseNode::Find (name);
+} 
+
+
+
+
+#endif
+

thirdparty/src/newton/dgCore/dgObb.cpp

@@ -0,0 +1,868 @@
+/* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgObb.h"
+#include "dgDebug.h"
+#include "dgPlane.h"
+#include "dgMatrix.h"
+
+
+namespace InternalSphere
+{
+	const dgFloat32 SPHERE_TOL = 0.002f;	
+
+	static dgFloat32 AspectRatio (dgFloat32 x, dgFloat32 y)
+	{
+		dgFloat32 tmp;
+		x = dgAbs (x);
+		y = dgAbs (y);
+
+		if (y < x) {
+			tmp = y;
+			y = x;
+			x = tmp;
+		}
+		if (y < 1.0e-12)	{
+			y = 1.0e-12f;
+		}
+		return x / y;
+	}
+	
+
+	static void BoundingBox (const dgMatrix& matrix, const dgFloat32 vertex[], dgInt32 stride, const dgInt32 index[], dgInt32 indexCount, dgVector &min, dgVector &max)
+	{
+	   dgFloat32 xmin = dgFloat32 (1.0e10f);
+	   dgFloat32 ymin = dgFloat32 (1.0e10f);
+	   dgFloat32 zmin = dgFloat32 (1.0e10f);
+
+	   dgFloat32 xmax = dgFloat32 (-1.0e10f);
+	   dgFloat32 ymax = dgFloat32 (-1.0e10f);
+	   dgFloat32 zmax = dgFloat32 (-1.0e10f);
+
+	   const dgFloat32*	const ptr = vertex;
+	   for (dgInt32 j = 0 ; j < indexCount; j ++ ) {
+			dgInt32 i = index[j] * stride;
+			dgVector tmp (ptr[i + 0], ptr[i + 1], ptr[i + 2], dgFloat32 (0.0f));
+			tmp = matrix.UnrotateVector (tmp);
+			if (tmp.m_x < xmin) xmin = tmp.m_x;
+			if (tmp.m_y < ymin) ymin = tmp.m_y;
+			if (tmp.m_z < zmin) zmin = tmp.m_z;
+			if (tmp.m_x > xmax) xmax = tmp.m_x;
+			if (tmp.m_y > ymax) ymax = tmp.m_y;
+			if (tmp.m_z > zmax) zmax = tmp.m_z;
+	   }
+		
+	   min = dgVector (xmin, ymin, zmin, dgFloat32 (0.0f));
+	   max = dgVector (xmax, ymax, zmax, dgFloat32 (0.0f));
+	}
+
+	// Compute axis aligned box
+	static void BoundingBox (const dgMatrix &Mat, const dgFloat32 vertex[], dgInt32 vertexCount, dgInt32 stride, dgVector &min, dgVector &max)
+	{
+		dgFloat32 xmin = dgFloat32 (1.0e10f);
+		dgFloat32 ymin = dgFloat32 (1.0e10f);
+		dgFloat32 zmin = dgFloat32 (1.0e10f);
+
+		dgFloat32 xmax = dgFloat32 (-1.0e10f);
+		dgFloat32 ymax = dgFloat32 (-1.0e10f);
+		dgFloat32 zmax = dgFloat32 (-1.0e10f);
+
+		const dgFloat32* ptr = vertex;
+		for (dgInt32 i = 0 ; i < vertexCount; i ++ ) {
+			dgVector tmp (ptr[0], ptr[1], ptr[2], dgFloat32 (0.0f));
+			ptr += stride;
+			tmp = Mat.UnrotateVector (tmp);
+			if (tmp.m_x < xmin) xmin = tmp.m_x;
+			if (tmp.m_y < ymin) ymin = tmp.m_y;
+			if (tmp.m_z < zmin) zmin = tmp.m_z;
+			if (tmp.m_x > xmax) xmax = tmp.m_x;
+			if (tmp.m_y > ymax) ymax = tmp.m_y;
+			if (tmp.m_z > zmax) zmax = tmp.m_z;
+		}
+
+		min = dgVector (xmin, ymin, zmin, dgFloat32 (0.0f));
+		max = dgVector (xmax, ymax, zmax, dgFloat32 (0.0f));
+	}
+
+
+	static void Statistics (dgObb &sphere, dgVector &eigenValues, const dgVector &scale, const dgFloat32 vertex[], const dgInt32 faceIndex[], dgInt32 indexCount, dgInt32 stride)
+	{
+		dgVector var (dgFloat32 (0.0f));
+		dgVector cov (dgFloat32 (0.0f));
+		dgVector centre (dgFloat32 (0.0f));
+		dgVector massCenter (dgFloat32 (0.0f));
+
+		dgVector scaleVector (scale & dgVector::m_triplexMask);
+
+		dgFloat64 totalArea = dgFloat32 (0.0f);
+		const dgFloat32* const ptr = vertex;
+		for (dgInt32 i = 0; i < indexCount; i += 3) {
+			dgInt32 index = faceIndex[i] * stride;
+			dgVector p0 (&ptr[index]);
+			p0 = p0 & dgVector::m_triplexMask;
+			p0 = p0 * scaleVector;
+
+			index = faceIndex[i + 1] * stride;;
+			dgVector p1 (&ptr[index]);
+			p1 = p1 & dgVector::m_triplexMask;
+			p1 = p1 * scaleVector;
+
+			index = faceIndex[i + 2] * stride;;
+			dgVector p2 (&ptr[index]);
+			p2 = p2 & dgVector::m_triplexMask;
+			p2 = p2 * scaleVector;
+
+			dgVector normal ((p1 - p0).CrossProduct(p2 - p0));
+			dgAssert(normal.m_w == dgFloat32(0.0f));
+			dgFloat64 area = dgFloat32 (0.5f) * sqrt (normal.DotProduct(normal).GetScalar());
+
+			centre = p0 + p1 + p2;
+			centre = centre.Scale (dgFloat32  (1.0f / 3.0f));
+
+			// Inertia of each point in the triangle
+			dgFloat64 Ixx = p0.m_x * p0.m_x + p1.m_x * p1.m_x + p2.m_x * p2.m_x;	
+			dgFloat64 Iyy = p0.m_y * p0.m_y + p1.m_y * p1.m_y + p2.m_y * p2.m_y;	
+			dgFloat64 Izz = p0.m_z * p0.m_z + p1.m_z * p1.m_z + p2.m_z * p2.m_z;	
+
+			dgFloat64 Ixy = p0.m_x * p0.m_y + p1.m_x * p1.m_y + p2.m_x * p2.m_y;	
+			dgFloat64 Iyz = p0.m_y * p0.m_z + p1.m_y * p1.m_z + p2.m_y * p2.m_z;	
+			dgFloat64 Ixz = p0.m_x * p0.m_z + p1.m_x * p1.m_z + p2.m_x * p2.m_z;	
+
+			if (area > dgEpsilon * 10.0) {
+				dgFloat64 K = area / dgFloat64 (12.0);
+				//Coriolis theorem for Inertia of a triangle in an arbitrary orientation
+				Ixx = K * (Ixx + 9.0 * centre.m_x * centre.m_x);
+				Iyy = K * (Iyy + 9.0 * centre.m_y * centre.m_y);
+				Izz = K * (Izz + 9.0 * centre.m_z * centre.m_z);
+
+				Ixy = K * (Ixy + 9.0 * centre.m_x * centre.m_y);
+				Ixz = K * (Ixz + 9.0 * centre.m_x * centre.m_z);
+				Iyz = K * (Iyz + 9.0 * centre.m_y * centre.m_z);
+				centre = centre.Scale ((dgFloat32)area);
+			} 
+
+			totalArea += area;
+			massCenter += centre;
+			var += dgVector ((dgFloat32)Ixx, (dgFloat32)Iyy, (dgFloat32)Izz, dgFloat32 (0.0f));
+			cov += dgVector ((dgFloat32)Ixy, (dgFloat32)Ixz, (dgFloat32)Iyz, dgFloat32 (0.0f));
+		}
+
+		if (totalArea > dgEpsilon * 10.0) {
+			dgFloat64 K = dgFloat64 (1.0) / totalArea; 
+			var = var.Scale ((dgFloat32)K);
+			cov = cov.Scale ((dgFloat32)K);
+			massCenter = massCenter.Scale ((dgFloat32)K);
+		}
+
+		dgFloat64 Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
+		dgFloat64 Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
+		dgFloat64 Izz = var.m_z - massCenter.m_z * massCenter.m_z;
+
+		dgFloat64 Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
+		dgFloat64 Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
+		dgFloat64 Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
+
+		sphere.m_front = dgVector ((dgFloat32)Ixx, (dgFloat32)Ixy, (dgFloat32)Ixz, dgFloat32 (0.0f));
+		sphere.m_up    = dgVector ((dgFloat32)Ixy, (dgFloat32)Iyy, (dgFloat32)Iyz, dgFloat32 (0.0f));
+		sphere.m_right = dgVector ((dgFloat32)Ixz, (dgFloat32)Iyz, (dgFloat32)Izz, dgFloat32 (0.0f));
+		eigenValues = sphere.EigenVectors();
+	}
+
+	
+	static void Statistics (dgObb& sphere, dgVector &eigenValues, dgVector &scaleVector, const dgFloat32 vertex[], dgInt32 vertexCount, dgInt32 stride)
+	{
+		dgBigVector var (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgBigVector cov (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgBigVector massCenter (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+	
+		const dgFloat32* ptr = vertex;
+		for (dgInt32 i = 0; i < vertexCount; i ++) {
+			dgFloat32 x = ptr[0] * scaleVector.m_x;
+			dgFloat32 y = ptr[1] * scaleVector.m_y;
+			dgFloat32 z = ptr[2] * scaleVector.m_z;
+			ptr += stride;
+			massCenter += dgBigVector (x, y, z, dgFloat32 (0.0f));
+			var += dgBigVector (x * x, y * y, z * z, dgFloat32 (0.0f));
+			cov += dgBigVector (x * y, x * z, y * z, dgFloat32 (0.0f));
+		}
+	
+		dgFloat64 k = dgFloat64 (1.0) / vertexCount;
+		var = var.Scale (k);
+		cov = cov.Scale (k);
+		massCenter = massCenter.Scale (k);
+	
+		dgFloat64 Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
+		dgFloat64 Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
+		dgFloat64 Izz = var.m_z - massCenter.m_z * massCenter.m_z;
+	
+		dgFloat64 Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
+		dgFloat64 Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
+		dgFloat64 Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
+	
+		sphere.m_front = dgVector (dgFloat32(Ixx), dgFloat32(Ixy), dgFloat32(Ixz), dgFloat32 (0.0f));
+		sphere.m_up    = dgVector (dgFloat32(Ixy), dgFloat32(Iyy), dgFloat32(Iyz), dgFloat32 (0.0f));
+		sphere.m_right = dgVector (dgFloat32(Ixz), dgFloat32(Iyz), dgFloat32(Izz), dgFloat32 (0.0f));
+		eigenValues = sphere.EigenVectors ();
+	}
+
+/*
+	static void Statistics (
+		dgObb &sphere,
+		dgVector &eigenValues,
+		const dgVector &scaleVector,
+		const dgFloat32 vertex[],
+		dgInt32 stride,
+		const dgFace face[],
+		dgInt32 faceCount)
+	{
+		dgAssert (0);
+
+		dgInt32 i;
+		dgInt32 index;
+		const dgFloat32 *ptr;
+		dgFloat64 K;
+		dgFloat64 Ixx;
+		dgFloat64 Iyy;
+		dgFloat64 Izz;
+		dgFloat64 Ixy;
+		dgFloat64 Ixz;
+		dgFloat64 Iyz;
+		dgFloat64 area;
+		dgFloat64 totalArea;
+		const dgFace *Face;
+	
+		dgVector var (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgVector cov (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgVector centre (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgVector massCenter (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		
+		totalArea = 0.0;
+		ptr = vertex;
+		for (i = 0; i < faceCount; i ++) {
+			 Face = &face[i];
+		
+			 index = Face->m_wireFrame[0] * stride;
+			 dgVector p0 (&ptr[index]);
+			 p0 = p0 * scaleVector;
+	
+			 index = Face->m_wireFrame[1] * stride;
+			 dgVector p1 (&ptr[index]);
+			 p1 = p1 * scaleVector;
+		
+			 index = Face->m_wireFrame[2] * stride;
+			 dgVector p2 (&ptr[index]);
+			 p2 = p2 * scaleVector;
+		
+			 dgVector normal ((p1 - p0) * (p2 - p0));
+		
+			 area = 0.5 * sqrt (normal % normal);
+		
+			 centre = p0 + p1 + p2;
+			 centre = centre.Scale (1.0f / 3.0f);
+		
+			 // Inercia of each point in the triangle
+			 Ixx = p0.m_x * p0.m_x + p1.m_x * p1.m_x + p2.m_x * p2.m_x;	
+			 Iyy = p0.m_y * p0.m_y + p1.m_y * p1.m_y + p2.m_y * p2.m_y;	
+			 Izz = p0.m_z * p0.m_z + p1.m_z * p1.m_z + p2.m_z * p2.m_z;	
+		
+			 Ixy = p0.m_x * p0.m_y + p1.m_x * p1.m_y + p2.m_x * p2.m_y;	
+			 Iyz = p0.m_y * p0.m_z + p1.m_y * p1.m_z + p2.m_y * p2.m_z;	
+			 Ixz = p0.m_x * p0.m_z + p1.m_x * p1.m_z + p2.m_x * p2.m_z;	
+		
+			 if (area > dgEPSILON * 10.0) {
+			     K = area / 12.0;
+			     //Coriolis theorem for Inertia of a triangle in an arbitrary orientation
+			     Ixx = K * (Ixx + 9.0 * centre.m_x * centre.m_x);
+			     Iyy = K * (Iyy + 9.0 * centre.m_y * centre.m_y);
+			     Izz = K * (Izz + 9.0 * centre.m_z * centre.m_z);
+		
+			     Ixy = K * (Ixy + 9.0 * centre.m_x * centre.m_y);
+			     Ixz = K * (Ixz + 9.0 * centre.m_x * centre.m_z);
+			     Iyz = K * (Iyz + 9.0 * centre.m_y * centre.m_z);
+			     centre = centre.Scale ((dgFloat32)area);
+			 } 
+		
+			 totalArea += area;
+			 massCenter += centre;
+			 var += dgVector ((dgFloat32)Ixx, (dgFloat32)Iyy, (dgFloat32)Izz);
+			 cov += dgVector ((dgFloat32)Ixy, (dgFloat32)Ixz, (dgFloat32)Iyz);
+		}
+		
+		if (totalArea > dgEPSILON * 10.0) {
+			K = 1.0 / totalArea; 
+			var = var.Scale ((dgFloat32)K);
+			cov = cov.Scale ((dgFloat32)K);
+			massCenter = massCenter.Scale ((dgFloat32)K);
+		}
+	
+		Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
+		Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
+		Izz = var.m_z - massCenter.m_z * massCenter.m_z;
+		
+		Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
+		Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
+		Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
+	
+		sphere.m_front = dgVector ((dgFloat32)Ixx, (dgFloat32)Ixy, (dgFloat32)Ixz);
+		sphere.m_up    = dgVector ((dgFloat32)Ixy, (dgFloat32)Iyy, (dgFloat32)Iyz);
+		sphere.m_right = dgVector ((dgFloat32)Ixz, (dgFloat32)Iyz, (dgFloat32)Izz);
+		sphere.EigenVectors(eigenValues);
+	}
+*/
+}
+
+
+
+void dgObb::SetDimensions (const dgFloat32 vertex[], dgInt32 strideInBytes, const dgInt32 triangles[], dgInt32 indexCount, const dgMatrix *basis)
+{
+	dgVector eigen;
+	dgVector scaleVector (dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
+
+	if (indexCount < 3) {
+		return;
+	}
+
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
+	if (!basis)	{
+
+		InternalSphere::Statistics (*this, eigen, scaleVector, vertex, triangles, indexCount, stride);
+
+		dgInt32 k = 0;
+		for (dgInt32 i = 0; i < 3; i ++) {
+			if (k >= 6) {
+				 break;
+			}
+			for (dgInt32 j = i + 1; j < 3; j ++) {
+				dgFloat32 aspect = InternalSphere::AspectRatio (eigen[i], eigen[j]);
+				if (aspect > dgFloat32 (0.9f)) {
+					scaleVector[i] *= dgFloat32 (2.0f); 
+					InternalSphere::Statistics (*this, eigen, scaleVector, vertex, triangles, indexCount, stride);
+					k ++;
+					i = -1;
+					break;
+				}
+			}
+		}
+	} else {
+		*this = *basis;
+	}
+
+	dgVector min; 
+	dgVector max;
+	InternalSphere::BoundingBox (*this, vertex, stride, triangles, indexCount, min, max);
+
+	dgVector massCenter	((max + min) * dgVector::m_half);
+	//massCenter = massCenter.Scale (dgFloat32 (0.5f));
+	m_posit = TransformVector (massCenter);
+
+	dgVector dim ((max - min) * dgVector::m_half);
+	//dim = dim.Scale (dgFloat32(0.5f));
+	SetDimensions (dim.m_x, dim.m_y, dim.m_z);
+}
+
+
+void dgObb::SetDimensions (const dgFloat32 vertex[], dgInt32 strideInBytes, dgInt32 count, const dgMatrix *basis) 
+{
+	dgVector eigen;
+	dgVector scaleVector (dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32 (0.0f));
+
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
+	if (!basis)	{
+		InternalSphere::Statistics (*this, eigen, scaleVector, vertex, count, stride);
+
+		dgInt32 k = 0;
+		for (dgInt32 i = 0; i < 3; i ++) {
+			if (k >= 6) {
+				 break;
+			}
+			for (dgInt32 j = i + 1; j < 3; j ++) {
+				dgFloat32 aspect = InternalSphere::AspectRatio (eigen[i], eigen[j]);
+				if (aspect > dgFloat32 (0.9f)) {
+					scaleVector[i] *= dgFloat32 (2.0f); 
+					InternalSphere::Statistics (*this, eigen, scaleVector, vertex, count, stride);
+					k ++;
+					i = -1;
+					break;
+				}
+			}
+		}
+	} else {
+		*this = *basis;
+	}
+		
+   dgVector min; 
+   dgVector max;
+   InternalSphere::BoundingBox (*this, vertex, count, stride, min, max);
+
+	dgVector massCenter ((max + min) * dgVector::m_half);
+	//massCenter = massCenter.Scale (0.5);
+	m_posit = TransformVector (massCenter);
+
+	dgVector dim ((max - min) * dgVector::m_half);
+	//dim = dim.Scale (dgFloat32(0.5f));
+	SetDimensions (dim.m_x + InternalSphere::SPHERE_TOL, 
+				   dim.m_y + InternalSphere::SPHERE_TOL, 
+				   dim.m_z + InternalSphere::SPHERE_TOL);
+}
+
+/*
+void dgObb::SetDimensions (
+	const dgFloat32 vertex[],  
+	dgInt32 strideInBytes,
+	const dgInt32 index[],
+	dgInt32 indexCount,
+	const dgMatrix *basis)
+{
+	dgInt32 i;
+	dgInt32 j;
+	dgInt32 k;
+	dgInt32 stride;
+	dgFloat32 aspect;
+	dgVector eigen;
+	dgVector scaleVector (dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32 (0.0f));
+
+	stride = strideInBytes / sizeof (dgFloat32);
+	if (!basis)	{
+		InternalSphere::Statistics (*this, eigen, scaleVector, vertex, index, indexCount, stride);
+
+		k = 0;
+		for (i = 0; i < 3; i ++) {
+			if (k >= 6) {
+				 break;
+			}
+			for (j = i + 1; j < 3; j ++) {
+				aspect = InternalSphere::AspectRatio (eigen[i], eigen[j]);
+				if (aspect > dgFloat32 (0.9f)) {
+					scaleVector[i] *= dgFloat32 (2.0f); 
+					InternalSphere::Statistics (*this, eigen, scaleVector, vertex, index, indexCount, stride);
+					i = -1;
+					k ++;
+					break;
+				}
+			}
+		}
+	} else {
+		*this = *basis;
+	}
+		
+   dgVector min; 
+   dgVector max;
+   InternalSphere::BoundingBox (*this, vertex, stride, index, indexCount, min, max);
+
+	dgVector massCenter (max + min);
+	massCenter = massCenter.Scale (dgFloat32(0.5f));
+	m_posit = TransformVector (massCenter);
+
+	dgVector dim (max - min);
+	dim = dim.Scale (dgFloat32(0.5f));
+	SetDimensions (dim.m_x + InternalSphere::SPHERE_TOL, 
+				   dim.m_y + InternalSphere::SPHERE_TOL, 
+				   dim.m_z + InternalSphere::SPHERE_TOL);
+}
+*/
+
+
+
+/*
+dgObb::dgObb (
+	const dgObb &dgObb, 
+	const dgVector &Dir)
+{
+	if ((Dir % Dir) < EPSILON * 0.01f) {
+		*this = dgObb;
+		return;
+	}
+
+	front = Dir;
+	front.Fast_Normalize(); 
+
+	if (dgAbs (front % dgObb.right) < 0.995) {
+		up = front * dgObb.right;
+		up.Fast_Normalize(); 
+	} else {
+		up	= dgObb.up;
+	}
+	right = up * front;
+
+	dgVector Step (Dir.Scale(0.5));
+	size.m_x = (dgFloat32)(dgObb.size.m_x * dgAbs (right % dgObb.right) +
+						 dgObb.size.m_y * dgAbs (right % dgObb.up) +
+						 dgObb.size.m_z * dgAbs (right % dgObb.front));
+
+	size.m_y = (dgFloat32)(dgObb.size.m_x * dgAbs (up % dgObb.right) +
+						  dgObb.size.m_y * dgAbs (up % dgObb.up) +
+						  dgObb.size.m_z * dgAbs (up % dgObb.front));
+
+	size.m_z = (dgFloat32)(sqrt (Step % Step) +
+							dgObb.size.m_x * dgAbs (front % dgObb.right) +
+							dgObb.size.m_y * dgAbs (front % dgObb.up) +
+							dgObb.size.m_z * dgAbs (front % dgObb.front));
+	posit = dgObb.posit + Step;
+
+}
+
+
+
+bool dgObb::dgObb_Overlap_Test (const dgObb &dgObb)
+{
+	dgFloat64 R;
+	dgVector Dir (dgObb.posit - posit);
+
+  	R = size.m_x * dgAbs (right % Dir) + dgObb.size.m_x * dgAbs (dgObb.right % Dir) + 
+	    size.m_y * dgAbs (up % Dir) + dgObb.size.m_y * dgAbs (dgObb.up % Dir) + 
+		 size.m_z * dgAbs (front %Dir) + dgObb.size.m_z * dgAbs (dgObb.front % Dir);
+	if (R	< (Dir % Dir)) {
+		return false;
+	}
+
+	R = size.m_x * dgAbs (right % dgObb.right) + 
+	    size.m_y * dgAbs (up % dgObb.right) + 
+		 size.m_z * dgAbs (front % dgObb.right) + dgObb.size.m_x;
+	if (R	< dgAbs (Dir % dgObb.right)) {
+		return false;
+	}
+
+	R = size.m_x * dgAbs (right % dgObb.up) + 
+	    size.m_y * dgAbs (up % dgObb.up) + 
+		 size.m_z * dgAbs (front % dgObb.up) + dgObb.size.m_y;
+	if (R	< dgAbs (Dir % dgObb.up)) {
+		return false;
+	}
+
+	R = size.m_x * dgAbs (right % dgObb.front) + 
+	    size.m_y * dgAbs (up % dgObb.front) + 
+		 size.m_z * dgAbs (front % dgObb.front) + dgObb.size.m_z; 
+	if (R	< dgAbs (Dir % dgObb.front)) {
+		return false;
+	}
+
+	R = dgObb.size.m_x * dgAbs (dgObb.right % right) + 
+	    dgObb.size.m_y * dgAbs (dgObb.up % right) + 
+		 dgObb.size.m_z * dgAbs (dgObb.front % right) + size.m_x;
+	if (R	< dgAbs (Dir % right)) {
+		return false;
+	}
+
+	R = dgObb.size.m_x * dgAbs (dgObb.right % up) + 
+	    dgObb.size.m_y * dgAbs (dgObb.up % up) + 
+		 dgObb.size.m_z * dgAbs (dgObb.front % up) + size.m_y;
+	if (R	< dgAbs (Dir % up)) {
+		return false;
+	}
+
+	R = dgObb.size.m_x * dgAbs (dgObb.right % front) + 
+	    dgObb.size.m_y * dgAbs (dgObb.up % front) + 
+		 dgObb.size.m_z * dgAbs (dgObb.front % front) + size.m_z;
+	if (R	< dgAbs (Dir % front)) {
+		return false;
+	}
+
+	return true;
+}
+
+
+void dgObb::Swept_Volume (
+	dgVector &min, 
+	dgVector &max)
+{
+	dgFloat32 w;
+	dgFloat32 h;
+	dgFloat32 b;
+
+	w = (dgFloat32)(size.m_x * dgAbs(right.m_x) + size.m_y * dgAbs(up.m_x) + size.m_z * dgAbs(front.m_x));  
+	h = (dgFloat32)(size.m_x * dgAbs(right.m_y) + size.m_y * dgAbs(up.m_y) + size.m_z * dgAbs(front.m_y));  
+	b = (dgFloat32)(size.m_x * dgAbs(right.m_z) + size.m_y * dgAbs(up.m_z) + size.m_z * dgAbs(front.m_z));  
+
+	min.m_x = posit.m_x - w;
+	min.m_y = posit.m_y - h;
+	min.m_z = posit.m_z - b;
+
+	max.m_x = posit.m_x + w;
+	max.m_y = posit.m_y + h;
+	max.m_z = posit.m_z + b;
+}
+*/
+
+
+/*
+dgInt32 dgObb::FrontTest (
+	const dgMatrix& matrix, 
+	const dgPlane* plane) const
+{
+	dgFloat32 R;
+	dgFloat32 dR;
+	InternalSphere::dgFloatSign flag0;
+	InternalSphere::dgFloatSign flag1;
+
+	dR = m_size.m_x * dgAbs (matrix.m_front.m_x) + m_size.m_y * dgAbs (matrix.m_up.m_x) + m_size.m_z * dgAbs (matrix.m_right.m_x);
+	R = plane[5].m_x * matrix.m_posit.m_x + plane[5].m_w; 
+
+	flag0.f = R + dR;
+	flag1.f = R - dR;
+	flag0.i = flag0.i >> 30 & 2;
+	flag1.i = flag1.i >> 31 & 1;
+	return InternalSphere::CodeTbl[flag0.i | flag1.i];
+}
+
+dgInt32 dgObb::RearTest (const dgMatrix& matrix, const dgPlane* plane) const
+{
+	dgFloat32 R;
+	dgFloat32 dR;
+	InternalSphere::dgFloatSign flag0;
+	InternalSphere::dgFloatSign flag1;
+
+	dR = m_size.m_x * dgAbs (matrix.m_front.m_x) + m_size.m_y * dgAbs (matrix.m_up.m_x) + m_size.m_z * dgAbs (matrix.m_right.m_x);
+	R = plane[4].m_x * matrix.m_posit.m_x + plane[4].m_w; 
+
+	flag0.f = R + dR;
+	flag1.f = R - dR;
+	flag0.i = flag0.i >> 30 & 2;
+	flag1.i = flag1.i >> 31 & 1;
+	return InternalSphere::CodeTbl[flag0.i | flag1.i];
+}
+
+
+dgInt32 dgObb::LeftTest (const dgMatrix& matrix, const dgPlane* plane) const
+{
+	dgFloat32 R;
+	dgFloat32 dR;
+	InternalSphere::dgFloatSign flag0;
+	InternalSphere::dgFloatSign flag1;
+
+	dR = m_size.m_x * dgAbs (matrix.m_front.m_x * plane[0].m_x + matrix.m_front.m_z * plane[0].m_z) + 
+		 m_size.m_y * dgAbs (matrix.m_up.m_x    * plane[0].m_x + matrix.m_up.m_z    * plane[0].m_z) +
+		 m_size.m_z * dgAbs (matrix.m_right.m_x * plane[0].m_x + matrix.m_right.m_z * plane[0].m_z);
+	R = plane[0].m_x * matrix.m_posit.m_x + plane[0].m_z * matrix.m_posit.m_z; 
+
+	flag0.f = R + dR;
+	flag1.f = R - dR;
+	flag0.i = (flag0.i >> 30) & 2;
+	flag1.i = (flag1.i >> 31) & 1;
+	return InternalSphere::CodeTbl[flag0.i | flag1.i];
+}
+
+dgInt32 dgObb::RightTest (const dgMatrix& matrix, const dgPlane* plane) const
+{
+	dgFloat32 R;
+	dgFloat32 dR;
+	InternalSphere::dgFloatSign flag0;
+	InternalSphere::dgFloatSign flag1;
+
+	dR = m_size.m_x * dgAbs (matrix.m_front.m_x * plane[1].m_x + matrix.m_front.m_z * plane[1].m_z) + 
+		 m_size.m_y * dgAbs (matrix.m_up.m_x    * plane[1].m_x + matrix.m_up.m_z    * plane[1].m_z) +
+		 m_size.m_z * dgAbs (matrix.m_right.m_x * plane[1].m_x + matrix.m_right.m_z * plane[1].m_z);
+	R = plane[1].m_x * matrix.m_posit.m_x + plane[1].m_z * matrix.m_posit.m_z; 
+
+	flag0.f = R + dR;
+	flag1.f = R - dR;
+	flag0.i = (flag0.i >> 30) & 2;
+	flag1.i = (flag1.i >> 31) & 1;
+	return InternalSphere::CodeTbl[flag0.i | flag1.i];
+}
+
+dgInt32 dgObb::BottomTest (const dgMatrix& matrix, const dgPlane* plane) const
+{
+	dgFloat32 R;
+	dgFloat32 dR;
+	InternalSphere::dgFloatSign flag0;
+	InternalSphere::dgFloatSign flag1;
+
+	dR = m_size.m_x * dgAbs (matrix.m_front.m_x * plane[2].m_x + matrix.m_front.m_y * plane[2].m_y) + 
+		 m_size.m_y * dgAbs (matrix.m_up.m_x    * plane[2].m_x + matrix.m_up.m_y    * plane[2].m_y) +
+		 m_size.m_z * dgAbs (matrix.m_right.m_x * plane[2].m_x + matrix.m_right.m_y * plane[2].m_y);
+
+	R = plane[2].m_x * matrix.m_posit.m_x + plane[2].m_y * matrix.m_posit.m_y; 
+
+	flag0.f = R + dR;
+	flag1.f = R - dR;
+	flag0.i = (flag0.i >> 30) & 2;
+	flag1.i = (flag1.i >> 31) & 1;
+
+	return InternalSphere::CodeTbl[flag0.i | flag1.i];
+}
+
+dgInt32 dgObb::TopTest (const dgMatrix& matrix, const dgPlane* plane) const
+{
+	dgFloat32 R;
+	dgFloat32 dR;
+	InternalSphere::dgFloatSign flag0;
+	InternalSphere::dgFloatSign flag1;
+
+	dR = m_size.m_x * dgAbs (matrix.m_front.m_x * plane[3].m_x + matrix.m_front.m_y * plane[3].m_y) + 
+		 m_size.m_y * dgAbs (matrix.m_up.m_x    * plane[3].m_x + matrix.m_up.m_y    * plane[3].m_y) +
+		 m_size.m_z * dgAbs (matrix.m_right.m_x * plane[3].m_x + matrix.m_right.m_y * plane[3].m_y);
+
+	R = plane[3].m_x * matrix.m_posit.m_x + plane[3].m_y * matrix.m_posit.m_y; 
+
+	flag0.f = R + dR;
+	flag1.f = R - dR;
+	flag0.i = (flag0.i >> 30) & 2;
+	flag1.i = (flag1.i >> 31) & 1;
+	return InternalSphere::CodeTbl[flag0.i | flag1.i];
+}
+
+
+
+dgInt32 dgObb::VisibilityTestLow (
+	const dgCamera* camera,
+	const dgMatrix& matrix) const
+{
+	dgInt32 i;
+	dgInt32 code;
+	const dgPlane* planes;
+	const dgPlane* guardPlanes;
+
+	planes = camera->GetViewVolume();
+
+	code = (this->*planeTest) (matrix, planes);
+	if (code != -1) {
+		for (i = 0; i < 6; i ++) {
+			code |= (this->*planeTestArray[i]) (matrix, planes);
+			if (code == -1) {
+				planeTest = planeTestArray[i];
+				return -1;
+			}
+		}
+
+		if (code) {
+			guardPlanes = camera->GetGuardViewVolume();
+			if (guardPlanes) {
+				code = 0;
+				for (i = 0; i < 6; i ++) {
+					code |= (this->*planeTestArray[i]) (matrix, guardPlanes);
+					dgAssert (code >= 0);
+					if (code) {
+						return code;
+					}
+				}
+			}
+		}
+	}
+
+   return code;
+}
+
+
+dgInt32 dgObb::VisibilityTest (const dgCamera* camera) const
+{
+	dgMatrix viewMatrix (*this * camera->GetViewMatrix());
+	return VisibilityTestLow (camera, viewMatrix);
+}
+
+dgInt32 dgObb::VisibilityTest (const dgCamera* camera, const dgMatrix &worldMatrix) const 
+{
+	dgMatrix viewMatrix (*this * worldMatrix * camera->GetViewMatrix());
+	return VisibilityTestLow (camera, viewMatrix);
+}
+
+void dgObb::Render (
+	const dgCamera* camera, 
+	const dgMatrix &worldMatrix, 
+	dgUnsigned32 rgb) const
+{
+	dgInt32 i;
+	struct ColorVertex
+	{
+		dgFloat32 m_x;
+		dgFloat32 m_y;
+		dgFloat32 m_z;
+		dgColor m_color;
+	};
+
+	dgUnsigned32 index [][2] = {
+		{0, 4}, {1, 5}, {2, 6}, {3, 7},
+		{0, 1}, {4, 5}, {7, 6}, {3, 2},
+		{1, 2}, {5, 6}, {4, 7}, {0, 3},
+	};
+
+	ColorVertex* ptr;
+	ColorVertex box[8];
+
+	box[0].m_x = -size.m_x;
+	box[0].m_y = -size.m_y;
+	box[0].m_z = -size.m_z;
+	box[0].m_color.m_val = rgb;
+
+	box[1].m_x =  size.m_x;
+	box[1].m_y = -size.m_y;
+	box[1].m_z = -size.m_z;
+	box[1].m_color.m_val = rgb;
+
+	box[2].m_x =  size.m_x;
+	box[2].m_y = -size.m_y;
+	box[2].m_z =  size.m_z;
+	box[2].m_color.m_val = rgb;
+
+	box[3].m_x = -size.m_x;
+	box[3].m_y = -size.m_y;
+	box[3].m_z =  size.m_z;
+	box[3].m_color.m_val = rgb;
+
+	box[4].m_x = -size.m_x;
+	box[4].m_y =  size.m_y;
+	box[4].m_z = -size.m_z;
+	box[4].m_color.m_val = rgb;
+
+	box[5].m_x =  size.m_x;
+	box[5].m_y =  size.m_y;
+	box[5].m_z = -size.m_z;
+	box[5].m_color.m_val = rgb;
+
+	box[6].m_x =  size.m_x;
+	box[6].m_y =  size.m_y;
+	box[6].m_z =  size.m_z;
+	box[6].m_color.m_val = rgb;
+
+	box[7].m_x = -size.m_x;
+	box[7].m_y =  size.m_y;
+	box[7].m_z =  size.m_z;
+	box[7].m_color.m_val = rgb;
+
+	dgRenderDescriptorParams param;
+	param.m_indexCount = 0;
+	param.m_vertexCount = sizeof (index) / sizeof (dgInt32); 
+	param.m_descType = dgDynamicVertex;
+	param.m_primitiveType = RENDER_LINELIST;
+	param.m_vertexFlags = VERTEX_ENABLE_XYZ | COLOR_ENABLE;
+
+	dgRenderDescriptor desc (param);
+
+	dgMatrix tmpMat (*this * worldMatrix);
+	camera->SetWorldMatrix (&tmpMat);
+
+	desc.m_material = dgMaterial::UseDebugMaterial();
+
+  	dgVertexRecord vertexRecord (desc.LockVertex());
+	ptr = (ColorVertex*) vertexRecord.vertex.ptr;
+	for (i = 0; i < (sizeof (index) / (2 * sizeof (dgUnsigned32))); i ++) {
+		 ptr[0] = box[index[i][0]]; 
+		 ptr[1] = box[index[i][1]]; 
+		 ptr += 2;
+	}
+	desc.UnlockVertex();
+
+	camera->Render (desc);
+
+	desc.m_material->Release();
+}
+*/
+