Merge branch 'master' into nazara-next

LICENSE-Kiwi

@@ -0,0 +1,71 @@
+=========================
+ The Kiwi licensing terms
+=========================
+Kiwi is licensed under the terms of the Modified BSD License (also known as
+New or Revised BSD), as follows:
+
+Copyright (c) 2013, Nucleic Development Team
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this
+list of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+Neither the name of the Nucleic Development Team nor the names of its
+contributors may be used to endorse or promote products derived from this
+software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+About Kiwi
+----------
+Chris Colbert began the Kiwi project in December 2013 in an effort to
+create a blisteringly fast UI constraint solver. Chris is still the
+project lead.
+
+The Nucleic Development Team is the set of all contributors to the Nucleic
+project and its subprojects.
+
+The core team that coordinates development on GitHub can be found here:
+http://github.com/nucleic. The current team consists of:
+
+* Chris Colbert
+
+Our Copyright Policy
+--------------------
+Nucleic uses a shared copyright model. Each contributor maintains copyright
+over their contributions to Nucleic. But, it is important to note that these
+contributions are typically only changes to the repositories. Thus, the Nucleic
+source code, in its entirety is not the copyright of any single person or
+institution. Instead, it is the collective copyright of the entire Nucleic
+Development Team. If individual contributors want to maintain a record of what
+changes/contributions they have specific copyright on, they should indicate
+their copyright in the commit message of the change, when they commit the
+change to one of the Nucleic repositories.
+
+With this in mind, the following banner should be used in any source code file
+to indicate the copyright and license terms:
+
+#------------------------------------------------------------------------------
+# Copyright (c) 2013, Nucleic Development Team.
+#
+# Distributed under the terms of the Modified BSD License.
+#
+# The full license is in the file LICENSE, distributed with this software.
+#------------------------------------------------------------------------------

appveyor.yml

@@ -19,11 +19,11 @@ skip_commits:
   message: /\[Posix\]/
 
 os:
-  - Visual Studio 2017
+  - Visual Studio 2019
 
 environment:
   matrix:
-  - TOOLSET: vs2017
+  - TOOLSET: vs2019
 
 install:
   - cd build && "./premake5.exe" %TOOLSET% && cd ..

build/scripts/common.lua

@@ -127,6 +127,10 @@ function NazaraBuild:Execute()
 
 				filter({})
 				
+				if (libTable.Custom) then
+					libTable.Custom()
+				end
+
 				self:PostconfigGenericProject()
 			end
 		end
@@ -187,6 +191,10 @@ function NazaraBuild:Execute()
 
 			filter({})
 				
+			if (moduleTable.Custom) then
+				moduleTable.Custom()
+			end
+
 			if (not _OPTIONS["united"]) then
 				self:PostconfigNazaraProject()
 			end
@@ -267,6 +275,10 @@ function NazaraBuild:Execute()
 			filter({})
 
 			self:PostconfigNazaraProject()
+				
+			if (toolTable.Custom) then
+				toolTable.Custom()
+			end
 		end
 
 		group("Examples")
@@ -321,6 +333,10 @@ function NazaraBuild:Execute()
 
 			filter({})
 
+			if (exampleTable.Custom) then
+				exampleTable.Custom()
+			end
+
 			self:PostconfigNazaraProject()
 		end
 	end

build/scripts/modules/physics3d.lua

@@ -1,10 +1,24 @@
 MODULE.Name = "Physics3D"
 
+MODULE.Defines = {
+	"_NEWTON_STATIC_LIB"
+}
+
+MODULE.OsDefines.Windows = {
+	"_WINDOWS"
+}
+
 MODULE.Libraries = {
 	"NazaraCore",
-	"Newton" -- Newton Game Dynamics
+	"newton" -- Newton Game Dynamics
 }
 
-MODULE.DynLib = {
-	"Newton"
-}
+MODULE.Custom = function()
+	vectorextensions("SSE3")
+
+	filter({"architecture:x86_64", "system:linux"})
+		defines("_POSIX_VER_64")
+
+	filter({"architecture:x86", "system:linux"})
+		defines("_POSIX_VER")
+end

include/Nazara/Math/Angle.hpp

@@ -27,8 +27,9 @@ namespace Nz
 		public:
 			Angle() = default;
 			Angle(T angle);
+			Angle(const Angle<AngleUnit::Degree, T>& angle);
+			Angle(const Angle<AngleUnit::Radian, T>& angle);
 			template<typename U> explicit Angle(const Angle<Unit, U>& Angle);
-			Angle(const Angle&) = default;
 			~Angle() = default;
 
 			T GetCos() const;
@@ -51,9 +52,6 @@ namespace Nz
 			Angle<AngleUnit::Radian, T> ToRadianAngle() const;
 			String ToString() const;
 
-			template<AngleUnit U = Unit, typename = std::enable_if_t<U != AngleUnit::Degree>> operator Angle<AngleUnit::Degree, T>() const { return ToDegreeAngle(); } // GCC < 8 bug
-			template<AngleUnit U = Unit, typename = std::enable_if_t<U != AngleUnit::Radian>> operator Angle<AngleUnit::Radian, T>() const { return ToRadianAngle(); } // GCC < 8 bug
-
 			Angle& operator=(const Angle&) = default;
 
 			Angle operator+(const Angle& other) const;

include/Nazara/Math/Angle.inl

@@ -155,6 +155,28 @@ namespace Nz
 	{
 	}
 
+	/*!
+	* \brief Constructs an Angle object from a angle in degrees, converting if required
+	*
+	* \param value Angle object to copy
+	*/
+	template<AngleUnit Unit, typename T>
+	Angle<Unit, T>::Angle(const Angle<AngleUnit::Degree, T>& angle) :
+	value(Detail::AngleUtils<Unit>::FromDegrees(angle.value))
+	{
+	}
+
+	/*!
+	* \brief Constructs an Angle object from a angle in radians, converting if required
+	*
+	* \param value Angle object to copy
+	*/
+	template<AngleUnit Unit, typename T>
+	Angle<Unit, T>::Angle(const Angle<AngleUnit::Radian, T>& angle) :
+	value(Detail::AngleUtils<Unit>::FromRadians(angle.value))
+	{
+	}
+
 	/*!
 	* \brief Computes the cosine of the angle
 	* \return Cosine of angle

include/Nazara/Network/AbstractSocket.hpp

@@ -22,7 +22,7 @@ namespace Nz
 			AbstractSocket(AbstractSocket&& abstractSocket) noexcept;
 			virtual ~AbstractSocket();
 
-			void Close();
+			void Close() noexcept;
 
 			void EnableBlocking(bool blocking);
 
@@ -41,7 +41,7 @@ namespace Nz
 			void SetSendBufferSize(std::size_t size);
 
 			AbstractSocket& operator=(const AbstractSocket&) = delete;
-			AbstractSocket& operator=(AbstractSocket&& abstractSocket);
+			AbstractSocket& operator=(AbstractSocket&& abstractSocket) noexcept;
 
 			// Signals:
 			NazaraSignal(OnStateChanged, const AbstractSocket* /*socket*/, SocketState /*oldState*/, SocketState /*newState*/);

include/Nazara/Network/UdpSocket.hpp

@@ -46,6 +46,9 @@ namespace Nz
 			bool SendMultiple(const IpAddress& to, const NetBuffer* buffers, std::size_t bufferCount, std::size_t* sent);
 			bool SendPacket(const IpAddress& to, const NetPacket& packet);
 
+			UdpSocket& operator=(const UdpSocket& udpSocket) = delete;
+			UdpSocket& operator=(UdpSocket && udpSocket) noexcept = default;
+
 		private:
 			void OnClose() override;
 			void OnOpened() override;

include/Nazara/Physics3D/PhysWorld3D.hpp

@@ -49,7 +49,6 @@ namespace Nz
 
 			void SetGravity(const Vector3f& gravity);
 			void SetMaxStepCount(std::size_t maxStepCount);
-			void SetSolverModel(unsigned int model);
 			void SetStepSize(float stepSize);
 			void SetThreadCount(unsigned int threadCount);
 
@@ -72,7 +71,7 @@ namespace Nz
 				CollisionCallback collisionCallback;
 			};
 
-			static int OnAABBOverlap(const NewtonMaterial* const material, const NewtonBody* const body0, const NewtonBody* const body1, int threadIndex);
+			static int OnAABBOverlap(const NewtonJoint* const contact, float timestep, int threadIndex);
 			static void ProcessContact(const NewtonJoint* const contact, float timestep, int threadIndex);
 
 			std::unordered_map<Nz::UInt64, std::unique_ptr<Callback>> m_callbacks;

include/NazaraSDK/Components/CollisionComponent2D.hpp

@@ -35,11 +35,11 @@ namespace Ndk
 
 			void Recenter(const Nz::Vector2f& origin);
 
-			void SetGeom(Nz::Collider2DRef geom);
+			void SetGeom(Nz::Collider2DRef geom, bool recomputeMoment = true, bool recomputeMassCenter = true);
 			void SetGeomOffset(const Nz::Vector2f& geomOffset);
 
 			CollisionComponent2D& operator=(Nz::Collider2DRef geom);
-			CollisionComponent2D& operator=(CollisionComponent2D&& collision) = default;
+			CollisionComponent2D& operator=(CollisionComponent2D&& collision) = delete;
 
 			static ComponentIndex componentIndex;
 

include/NazaraSDK/Components/CollisionComponent3D.hpp

@@ -32,7 +32,7 @@ namespace Ndk
 			void SetGeom(Nz::Collider3DRef geom);
 
 			CollisionComponent3D& operator=(Nz::Collider3DRef geom);
-			CollisionComponent3D& operator=(CollisionComponent3D&& collision) = default;
+			CollisionComponent3D& operator=(CollisionComponent3D&& collision) = delete;
 
 			static ComponentIndex componentIndex;
 

src/Nazara/Network/AbstractSocket.cpp

@@ -71,7 +71,7 @@ namespace Nz
 	* \brief Closes the socket
 	*/
 
-	void AbstractSocket::Close()
+	void AbstractSocket::Close() noexcept
 	{
 		if (m_handle != SocketImpl::InvalidHandle)
 		{
@@ -237,7 +237,7 @@ namespace Nz
 	* \param abstractSocket AbstractSocket to move in this
 	*/
 
-	AbstractSocket& AbstractSocket::operator=(AbstractSocket&& abstractSocket)
+	AbstractSocket& AbstractSocket::operator=(AbstractSocket&& abstractSocket) noexcept
 	{
 		Close();
 

src/Nazara/Physics2D/RigidBody2D.cpp

@@ -695,6 +695,8 @@ namespace Nz
 		cpBodySetVelocity(to, cpBodyGetVelocity(from));
 
 		cpBodySetType(to, cpBodyGetType(from));
+
+		to->velocity_func = from->velocity_func;
 	}
 
 	void RigidBody2D::CopyShapeData(cpShape* from, cpShape* to)

src/Nazara/Physics3D/Collider3D.cpp

@@ -259,7 +259,7 @@ namespace Nz
 
 	NewtonCollision* CapsuleCollider3D::CreateHandle(PhysWorld3D* world) const
 	{
-		return NewtonCreateCapsule(world->GetHandle(), m_radius, m_length, 0, m_matrix);
+		return NewtonCreateCapsule(world->GetHandle(), m_radius, m_radius, m_length, 0, m_matrix);
 	}
 
 	/******************************* CompoundCollider3D ********************************/
@@ -396,7 +396,7 @@ namespace Nz
 
 	NewtonCollision* CylinderCollider3D::CreateHandle(PhysWorld3D* world) const
 	{
-		return NewtonCreateCylinder(world->GetHandle(), m_radius, m_length, 0, m_matrix);
+		return NewtonCreateCylinder(world->GetHandle(), m_radius, m_radius, m_length, 0, m_matrix);
 	}
 
 	/********************************* NullCollider3D **********************************/

src/Nazara/Physics3D/PhysWorld3D.cpp

@@ -93,11 +93,6 @@ namespace Nz
 		m_maxStepCount = maxStepCount;
 	}
 
-	void PhysWorld3D::SetSolverModel(unsigned int model)
-	{
-		NewtonSetSolverModel(m_world, model);
-	}
-
 	void PhysWorld3D::SetStepSize(float stepSize)
 	{
 		m_stepSize = stepSize;
@@ -116,7 +111,8 @@ namespace Nz
 		callbackPtr->aabbOverlapCallback = std::move(aabbOverlapCallback);
 		callbackPtr->collisionCallback = std::move(collisionCallback);
 
-		NewtonMaterialSetCollisionCallback(m_world, firstMaterial, secondMaterial, callbackPtr.get(), (callbackPtr->aabbOverlapCallback) ? OnAABBOverlap : nullptr, (callbackPtr->collisionCallback) ? ProcessContact : nullptr);
+		NewtonMaterialSetCollisionCallback(m_world, firstMaterial, secondMaterial, (callbackPtr->aabbOverlapCallback) ? OnAABBOverlap : nullptr, (callbackPtr->collisionCallback) ? ProcessContact : nullptr);
+		NewtonMaterialSetCallbackUserData(m_world, firstMaterial, secondMaterial, callbackPtr.get());
 
 		UInt64 firstMaterialId(firstMaterial);
 		UInt64 secondMaterialId(secondMaterial);
@@ -163,17 +159,31 @@ namespace Nz
 		}
 	}
 
-	int PhysWorld3D::OnAABBOverlap(const NewtonMaterial* const material, const NewtonBody* const body0, const NewtonBody* const body1, int threadIndex)
+	int PhysWorld3D::OnAABBOverlap(const NewtonJoint* const contactJoint, dFloat timestep, int threadIndex)
 	{
-		RigidBody3D* bodyA = static_cast<RigidBody3D*>(NewtonBodyGetUserData(body0));
-		RigidBody3D* bodyB = static_cast<RigidBody3D*>(NewtonBodyGetUserData(body1));
+		RigidBody3D* bodyA = static_cast<RigidBody3D*>(NewtonBodyGetUserData(NewtonJointGetBody0(contactJoint)));
+		RigidBody3D* bodyB = static_cast<RigidBody3D*>(NewtonBodyGetUserData(NewtonJointGetBody1(contactJoint)));
 		assert(bodyA && bodyB);
 
-		Callback* callbackData = static_cast<Callback*>(NewtonMaterialGetMaterialPairUserData(material));
-		assert(callbackData);
-		assert(callbackData->aabbOverlapCallback);
+		using ContactJoint = void*;
 
-		return callbackData->aabbOverlapCallback(*bodyA, *bodyB);
+		// Query all joints first, to prevent removing a joint from the list while iterating on it
+		StackVector<ContactJoint> contacts = NazaraStackVector(ContactJoint, NewtonContactJointGetContactCount(contactJoint));
+		for (ContactJoint contact = NewtonContactJointGetFirstContact(contactJoint); contact; contact = NewtonContactJointGetNextContact(contactJoint, contact))
+			contacts.push_back(contact);
+
+		for (ContactJoint contact : contacts)
+		{
+			NewtonMaterial* material = NewtonContactGetMaterial(contact);
+			Callback* callbackData = static_cast<Callback*>(NewtonMaterialGetMaterialPairUserData(material));
+			assert(callbackData);
+			assert(callbackData->collisionCallback);
+
+			if (!callbackData->collisionCallback(*bodyA, *bodyB))
+				return 0;
+		}
+
+		return 1;
 	}
 
 	void PhysWorld3D::ProcessContact(const NewtonJoint* const contactJoint, float timestep, int threadIndex)

src/Nazara/Physics3D/RigidBody3D.cpp

@@ -315,7 +315,7 @@ namespace Nz
 			{
 				// If we already have a mass, we already have an inertial matrix as well, just rescale it
 				float Ix, Iy, Iz;
-				NewtonBodyGetMassMatrix(m_body, &m_mass, &Ix, &Iy, &Iz);
+				NewtonBodyGetMass(m_body, &m_mass, &Ix, &Iy, &Iz);
 
 				float scale = mass / m_mass;
 				NewtonBodySetMassMatrix(m_body, mass, Ix*scale, Iy*scale, Iz*scale);

src/NazaraSDK/Components/CollisionComponent2D.cpp

@@ -53,11 +53,11 @@ namespace Ndk
 	*
 	* \param geom Geometry used for collisions
 	*/
-	void CollisionComponent2D::SetGeom(Nz::Collider2DRef geom)
+	void CollisionComponent2D::SetGeom(Nz::Collider2DRef geom, bool recomputeMoment, bool recomputeMassCenter)
 	{
 		m_geom = std::move(geom);
 
-		GetRigidBody()->SetGeom(m_geom);
+		GetRigidBody()->SetGeom(m_geom, recomputeMoment, recomputeMassCenter);
 	}
 
 	/*!

src/NazaraSDK/Components/PhysicsComponent2D.cpp

@@ -77,7 +77,7 @@ namespace Ndk
 		if (IsComponent<CollisionComponent2D>(component))
 		{
 			NazaraAssert(m_object, "Invalid object");
-			m_object->SetGeom(static_cast<CollisionComponent2D&>(component).GetGeom());
+			m_object->SetGeom(static_cast<CollisionComponent2D&>(component).GetGeom(), false, false);
 		}
 	}
 
@@ -94,7 +94,7 @@ namespace Ndk
 		if (IsComponent<CollisionComponent2D>(component))
 		{
 			NazaraAssert(m_object, "Invalid object");
-			m_object->SetGeom(Nz::NullCollider2D::New());
+			m_object->SetGeom(Nz::NullCollider2D::New(), false, false);
 		}
 	}
 

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;
+}
+*/