* Adjustment: Update Bullet version to 3.24.

Engine/lib/bullet/examples/Experiments/ImplicitCloth/ImplicitClothExample.cpp

@@ -0,0 +1,113 @@
+#include "ImplicitClothExample.h"
+
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonCameraInterface.h"
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "../CommonInterfaces/CommonWindowInterface.h"
+#include "stan/vecmath.h"
+#include "stan/Cloth.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+#ifdef _DEBUG
+int numX = 20, numY = 20;
+#else
+int numX = 60, numY = 60;
+#endif
+const size_t total_points = (numX) * (numY);
+
+struct ImplicitClothExample : public CommonExampleInterface
+{
+	struct GUIHelperInterface* m_guiHelper;
+	int m_option;
+
+	Cloth* m_cloth;
+
+public:
+	ImplicitClothExample(struct GUIHelperInterface* helper, int option)
+		: m_guiHelper(helper),
+		  m_option(option),
+		  m_cloth(0)
+	{
+	}
+	virtual void initPhysics();
+	virtual void exitPhysics();
+	virtual void stepSimulation(float deltaTime);
+	virtual void renderScene();
+	virtual void physicsDebugDraw(int debugFlags);  //for now we reuse the flags in Bullet/src/LinearMath/btIDebugDraw.h
+	virtual bool mouseMoveCallback(float x, float y)
+	{
+		return false;
+	}
+	virtual bool mouseButtonCallback(int button, int state, float x, float y)
+	{
+		return false;
+	}
+	virtual bool keyboardCallback(int key, int state)
+	{
+		return false;
+	}
+
+	virtual void resetCamera()
+	{
+		float dist = 10;
+		float pitch = 62;
+		float yaw = 33;
+		float targetPos[3] = {-3, 2.4, -3.6};
+		m_guiHelper->resetCamera(dist, pitch, yaw, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+void ImplicitClothExample::initPhysics()
+{
+	float size = 10;
+	m_guiHelper->setUpAxis(1);
+	m_cloth = ClothCreate(numX, numY, size);
+}
+void ImplicitClothExample::exitPhysics()
+{
+	delete m_cloth;
+	m_cloth = 0;
+}
+void ImplicitClothExample::stepSimulation(float deltaTime)
+{
+	m_cloth->Simulate(deltaTime);
+	m_cloth->cloth_gravity.y = -9.8;  //-9.8;//-9.8;//-9.8;//0;//-9.8;//0;//-9.8;//0;//-9.8;
+	m_cloth->cloth_gravity.z = -9.8;  //0;//-9.8;//0;//-9.8;
+
+	m_cloth->spring_struct = 10000000.0f;
+	m_cloth->spring_shear = 10000000.0f;
+
+	//m_cloth->spring_struct=1000000.0f;
+	//m_cloth->spring_shear=1000000.0f;
+
+	m_cloth->spring_damp = 0;  //100;
+}
+void ImplicitClothExample::renderScene()
+{
+}
+void ImplicitClothExample::physicsDebugDraw(int debugFlags)
+{
+	CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
+
+	b3AlignedObjectArray<unsigned int> indices;
+
+	for (int i = 0; i < m_cloth->springs.count; i++)
+	{
+		indices.push_back(m_cloth->springs[i].a);
+		indices.push_back(m_cloth->springs[i].b);
+	}
+	float lineColor[4] = {0.4, 0.4, 1.0, 1};
+	renderer->drawLines(&m_cloth->X[0].x, lineColor, total_points, sizeof(float3), &indices[0], indices.size(), 1);
+
+	float pointColor[4] = {1, 0.4, 0.4, 1};
+
+	//		renderer->drawPoints(&m_cloth->X[0].x,pointColor,total_points,sizeof(float3),3);
+}
+
+class CommonExampleInterface* ImplicitClothCreateFunc(struct CommonExampleOptions& options)
+{
+	return new ImplicitClothExample(options.m_guiHelper, options.m_option);
+}

Engine/lib/bullet/examples/Experiments/ImplicitCloth/ImplicitClothExample.h

@@ -0,0 +1,6 @@
+#ifndef IMPLICIT_CLOTH_EXAMPLE_H
+#define IMPLICIT_CLOTH_EXAMPLE_H
+
+class CommonExampleInterface* ImplicitClothCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //IMPLICIT_CLOTH_EXAMPLE_H

Engine/lib/bullet/examples/Experiments/ImplicitCloth/stan/Cloth.cpp

@@ -0,0 +1,87 @@
+
+//-----------------------------------------------------------------------------------------------
+//
+// The remainder of this file shows how to use the spring network class with backward integration
+// in order to implement a cloth system within a 3D game environment.
+// The cloth class extends the springnetwork class in order to provide
+// import/export, rendering support, and hooks into the game.
+//
+
+#include "Cloth.h"
+
+Array<Cloth *> cloths;
+
+Cloth::Cloth(const char *_name, int _n) : SpringNetwork(_n),
+										  color(0, 0.5f, 1.0f)
+{
+	cloths.Add(this);
+}
+Cloth::~Cloth()
+{
+	cloths.Remove(this);
+}
+
+//
+// I/O support for serialization of our springnetwork and cloth objects.
+//
+
+int cloth_showbbox = 0;         // for debug visualization  shows bounding box.
+float cloth_showvert = 0.025f;  // size of box to put around current vert selected, 0 turns off
+
+Cloth *ClothCreate(int w, int h, float size)
+{
+	// simple cloth generation routine that creates a typical square cloth.
+	// better to use a real pipeline to generate these, this is just for testing.
+	int i, j;
+	Cloth *cloth = new Cloth("cloth", w * h);
+	cloth->w = w;
+	cloth->h = h;  // later for rendering.
+	for (i = 0; i < h; i++)
+		for (j = 0; j < w; j++)
+		{
+			cloth->X[i * w + j] = (float3(-0.5f, -0.5f, 0) + float3((float)j / (w - 1.0f), 1.0f - (float)i / (h - 1.0f), 0)) * size;
+		}
+	for (i = 0; i < h; i++)
+		for (j = 0; j < w; j++)
+		{
+			if (i < h - 1) cloth->CreateSpring(SPRING_STRUCT, i * w + j, (i + 1) * w + j);                    // structural
+			if (j < w - 1) cloth->CreateSpring(SPRING_STRUCT, i * w + j, i * w + (j + 1));                    // structural
+			if (j < w - 1 && i < h - 1) cloth->CreateSpring(SPRING_SHEAR, i * w + j, (i + 1) * w + (j + 1));  // shear
+			if (j > 0 && i < h - 1) cloth->CreateSpring(SPRING_SHEAR, i * w + j, (i + 1) * w + (j - 1));      // shear
+			if (i < h - 2) cloth->CreateSpring(SPRING_BEND, i * w + j, (i + 2) * w + j);                      // benders
+			if (j < w - 2) cloth->CreateSpring(SPRING_BEND, i * w + j, i * w + (j + 2));                      // benders
+		}
+	cloth->UpdateLimits();
+	return cloth;
+}
+
+int cloth_tess = 20;
+float3 cloth_spawnpoint(0, 3, 5.0f);
+
+/*
+static void ClothDrawSprings(Cloth *cloth)
+{
+       static const float3 color[3]={float3(1,1,0),float3(1,0,1),float3(0,1,1)};
+       float3N &X = cloth->X;
+       for(int i=0;i<cloth->springs.count;i++)
+       {
+               SpringNetwork::Spring &s = cloth->springs[i];
+               extern void Line(const float3 &,const float3 &,const float3 &color_rgb);
+               Line(X[s.a],X[s.b],color[s.type]);
+       }
+}
+*/
+int cloth_showsprings = 0;
+
+void DoCloths()
+{
+	int i;
+	for (i = 0; i < cloths.count; i++)
+	{
+		Cloth *cloth = cloths[i];
+
+		//  cloth->Simulate((cloth->cloth_step<0)?DeltaT:cloth->cloth_step);
+		//if(cloth_showsprings)
+		//        ClothDrawSprings(cloth); // debug visualization
+	}
+}

Engine/lib/bullet/examples/Experiments/ImplicitCloth/stan/Cloth.h

@@ -0,0 +1,18 @@
+#ifndef STAN_CLOTH_H
+#define STAN_CLOTH_H
+
+#include "SpringNetwork.h"
+
+class Cloth : public SpringNetwork
+{
+public:
+	int w, h;
+
+	float3 color;  // for debug rendering
+	Cloth(const char* _name, int _n);
+	~Cloth();
+};
+
+Cloth* ClothCreate(int w, int h, float size);
+
+#endif  //STAN_CLOTH_H

Engine/lib/bullet/examples/Experiments/ImplicitCloth/stan/SpringNetwork.cpp

@@ -0,0 +1,183 @@
+#include "vec3n.h"
+//#include "console.h"
+
+extern int numX;
+
+//
+// Cloth - Backward Integrated Spring Network
+//
+// (c) Stan Melax 2006
+// http://www.melax.com/cloth
+// freeware demo and source
+// Although its free software, I'll gaurantee and support this software as much as is reasonable.
+// However, if you choose to use any of this code, then you agree that
+// I assume no financial liability should the software not meet your expectations.
+// But do feel free to send any feedback.
+//
+// The core backward integration functionality has all been extracted into the SpringNetwork class.
+// This makes it easy for you if you just want to look at or use the math and the algorithms.
+// The remainder of the code builds a cloth system with basic render support, I/O, and manipulators,
+// so its possible to make use of the technology within a 3D application.
+// This code is separated from the SpringNetwork class in order to avoid pushing a particular style
+// and prevent any dependancies of the algorithms onto unrelated systems.
+// Feel free to adapt any of this into your own 3D engine/environment.
+//
+// Instead of having unique Hooke force and damping coefficients on each spring, the SpringNetwork
+// code uses a spring 'type' that indexes a short list of shared named coefficients.
+// This was just more practical for the typical application of this technology.
+// Over-designed systems that are too general can be slower for
+// the next guy to understand and more painful to use.
+// Editing/creation is easier when only 1 number needs to be changed.
+// Nonetheless, feel free to adapt to your own needs.
+//
+
+#include <stdio.h>
+#include <float.h>
+
+#include "vec3n.h"
+//#include "console.h"
+//#include "manipulatori.h"
+//#include "object.h"
+//#include "xmlparse.h"
+
+static const float3x3 I(1, 0, 0, 0, 1, 0, 0, 0, 1);
+
+inline float3x3 dfdx_spring(const float3 &dir, float length, float rest, float k)
+{
+	// dir is unit length direction, rest is spring's restlength, k is spring constant.
+	return ((I - outerprod(dir, dir)) * Min(1.0f, rest / length) - I) * -k;
+}
+inline float3x3 dfdx_damp(const float3 &dir, float length, const float3 &vel, float rest, float damping)
+{
+	// inner spring damping   vel is the relative velocity  of the endpoints.
+	return (I - outerprod(dir, dir)) * (-damping * -(dot(dir, vel) / Max(length, rest)));
+}
+inline float3x3 dfdv_damp(const float3 &dir, float damping)
+{
+	// derivative of force wrt velocity.
+	return outerprod(dir, dir) * damping;
+}
+
+#include "SpringNetwork.h"
+
+SpringNetwork::SpringNetwork(int _n) : X(_n), V(_n), F(_n), dV(_n), A(_n), dFdX(_n), dFdV(_n)
+{
+	assert(SPRING_STRUCT == 0);
+	assert(&spring_shear == &spring_struct + SPRING_SHEAR);
+	assert(&spring_bend == &spring_struct + SPRING_BEND);
+	assert(&spring_struct == &spring_k[SPRING_STRUCT]);
+	assert(&spring_shear == &spring_k[SPRING_SHEAR]);
+	assert(&spring_bend == &spring_k[SPRING_BEND]);
+	//   spring_struct=1000000.0f;
+	//  spring_shear=1000000.0f;
+
+	spring_struct = 1000.0f;
+	spring_shear = 100.0f;
+
+	spring_bend = 25.0f;
+	spring_damp = 5.0f;
+	spring_air = 1.0f;
+	spring_air = 1.0f;
+	cloth_step = 0.25f;  // delta time for cloth
+	cloth_gravity = float3(0, -10, 0);
+	cloth_sleepthreshold = 0.001f;
+	cloth_sleepcount = 100;
+	awake = cloth_sleepcount;
+
+	//fix/pin two points in worldspace
+	float3Nx3N::Block zero;
+	zero.m = float3x3(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	zero.c = 0;
+	zero.r = 0;
+	S.blocks.Add(zero);
+	zero.r = numX - 1;
+	S.blocks.Add(zero);
+}
+
+SpringNetwork::Spring &SpringNetwork::AddBlocks(Spring &s)
+{
+	// Called during initial creation of springs in our spring network.
+	// Sets up the sparse matrices corresponding to connections.
+	// Note the indices (s.iab,s.iba) are also stored with spring to avoid looking them up each time a spring is applied
+	// All 3 matrices A,dFdX, and dFdV are contstructed identically so the block array layout will be the same for each.
+	s.iab = A.blocks.count;  // added 'ab' blocks will have this index.
+	A.blocks.Add(float3Nx3N::Block(s.a, s.b));
+	dFdX.blocks.Add(float3Nx3N::Block(s.a, s.b));
+	dFdV.blocks.Add(float3Nx3N::Block(s.a, s.b));
+	s.iba = A.blocks.count;  // added 'ba' blocks will have this index.
+	A.blocks.Add(float3Nx3N::Block(s.b, s.a));
+	dFdX.blocks.Add(float3Nx3N::Block(s.b, s.a));
+	dFdV.blocks.Add(float3Nx3N::Block(s.b, s.a));
+	return s;
+}
+
+void SpringNetwork::PreSolveSpring(const SpringNetwork::Spring &s)
+{
+	// Adds this spring's contribution into force vector F and force derivitves dFdX and dFdV
+	// One optimization would be premultiply dfdx by dt*dt and F and dFdV by dt right here in this function.
+	// However, for educational purposes we wont do that now and intead just follow the paper directly.
+	//assert(dFdX.blocks[s.a].c==s.a);  // delete this assert, no bugs here
+	//assert(dFdX.blocks[s.a].r==s.a);
+	float3 extent = X[s.b] - X[s.a];
+	float length = magnitude(extent);
+	float3 dir = (length == 0) ? float3(0, 0, 0) : extent * 1.0f / length;
+	float3 vel = V[s.b] - V[s.a];
+	float k = spring_k[s.type];
+	float3 f = dir * ((k * (length - s.restlen)) + spring_damp * dot(vel, dir));  // spring force + damping force
+	F[s.a] += f;
+	F[s.b] -= f;
+	float3x3 dfdx = dfdx_spring(dir, length, s.restlen, k) + dfdx_damp(dir, length, vel, s.restlen, spring_damp);
+	dFdX.blocks[s.a].m -= dfdx;    // diagonal chunk dFdX[a,a]
+	dFdX.blocks[s.b].m -= dfdx;    // diagonal chunk dFdX[b,b]
+	dFdX.blocks[s.iab].m += dfdx;  // off-diag chunk dFdX[a,b]
+	dFdX.blocks[s.iba].m += dfdx;  // off-diag chunk dFdX[b,a]
+	float3x3 dfdv = dfdv_damp(dir, spring_damp);
+	dFdV.blocks[s.a].m -= dfdv;    // diagonal chunk dFdV[a,a]
+	dFdV.blocks[s.b].m -= dfdv;    // diagonal chunk dFdV[b,b]
+	dFdV.blocks[s.iab].m += dfdv;  // off-diag chunk dFdV[a,b]
+	dFdV.blocks[s.iba].m += dfdv;  // off-diag chunk dFdV[b,a]
+}
+
+void SpringNetwork::CalcForces()
+{
+	// Collect forces and derivatives:  F,dFdX,dFdV
+	dFdX.Zero();
+	dFdV.InitDiagonal(-spring_air);
+	F.Init(cloth_gravity);
+
+	F.element[0] = float3(0, 0, 0);
+	F.element[numX - 1] = float3(0, 0, 0);
+
+	F -= V * spring_air;
+	for (int i = 0; i < springs.count; i++)
+	{
+		PreSolveSpring(springs[i]);  // will add to F,dFdX,dFdV
+	}
+}
+
+void SpringNetwork::Simulate(float dt)
+{
+	// Get ready for conjugate gradient iterative solver step.
+	// Initialize operands.
+	if (!awake) return;
+	CalcForces();
+	int n = X.count;    // all our big vectors are of this size
+	float3N dFdXmV(n);  // temp to store result of matrix multiply
+	float3N B(n);
+	dV.Zero();
+	A.Identity();  // build up the big matrix we feed to solver
+	A -= dFdV * dt + dFdX * (dt * dt);
+
+	dFdXmV = dFdX * V;
+	B = F * dt + dFdXmV * (dt * dt);
+
+	ConjGradientFiltered(dV, A, B, S);
+	V = V + dV;
+	//	   V.element[0] = float3(0,0,0);
+	//	   V.element[numX-1] = float3(0,0,0);
+
+	X = X + V * dt;
+
+	UpdateLimits();
+	awake = (dot(V, V) < cloth_sleepthreshold) ? awake - 1 : awake = cloth_sleepcount;
+}

Engine/lib/bullet/examples/Experiments/ImplicitCloth/stan/SpringNetwork.h

@@ -0,0 +1,61 @@
+#ifndef STAN_SPRING_NETWORK_H
+#define STAN_SPRING_NETWORK_H
+
+#include "vec3n.h"
+
+#define SPRING_STRUCT (0)
+#define SPRING_SHEAR (1)
+#define SPRING_BEND (2)
+
+class SpringNetwork
+{
+public:
+	class Spring
+	{
+	public:
+		int type;  // index into coefficients spring_k[]
+		float restlen;
+		int a, b;      // spring endpoints vector indices
+		int iab, iba;  // indices into off-diagonal blocks of sparse matrix
+		Spring() {}
+		Spring(int _type, int _a, int _b, float _restlen) : type(_type), a(_a), b(_b), restlen(_restlen) { iab = iba = -1; }
+	};
+	Array<Spring> springs;
+	float3N X;        // positions of all points
+	float3N V;        // velocities
+	float3N F;        // force on each point
+	float3N dV;       // change in velocity
+	float3Nx3N A;     // big matrix we solve system with
+	float3Nx3N dFdX;  // big matrix of derivative of force wrt position
+	float3Nx3N dFdV;  // big matrix of derivative of force wrt velocity
+	float3Nx3N S;     // used for our constraints - contains only some diagonal blocks as needed S[i,i]
+	int awake;
+	float3 bmin, bmax;
+	union {
+		struct
+		{
+			float spring_struct;
+			float spring_shear;
+			float spring_bend;
+		};
+		float spring_k[3];
+	};
+	float spring_damp;
+	float spring_air;
+	float cloth_step;  // delta time for cloth
+	float3 cloth_gravity;
+	float cloth_sleepthreshold;
+	int cloth_sleepcount;
+
+	SpringNetwork(int _n);
+	Spring &AddBlocks(Spring &s);
+	Spring &CreateSpring(int type, int a, int b, float restlen) { return AddBlocks(springs.Add(Spring(type, a, b, restlen))); }
+	Spring &CreateSpring(int type, int a, int b) { return CreateSpring(type, a, b, magnitude(X[b] - X[a])); }
+	void UpdateLimits() { BoxLimits(X.element, X.count, bmin, bmax); }
+	void Wake() { awake = cloth_sleepcount; }
+	void Simulate(float dt);
+	void PreSolveSpring(const Spring &s);
+	void CalcForces();
+};
+
+#endif  //STAN_SPRING_NETWORK_H

Engine/lib/bullet/examples/Experiments/ImplicitCloth/stan/array.h

@@ -0,0 +1,307 @@
+//
+//  Typical template dynamic array container class.
+//  By S Melax 1998
+//
+// anyone is free to use, inspect, learn from, or ignore
+// the code here as they see fit.
+//
+// A very simple template array class.
+// Its easiest to understand this array
+// class by seeing how it is used in code.
+//
+// For example:
+//     for(i=0;i<myarray.count;i++)
+//         myarray[i] = somefunction(i);
+//
+// When the array runs out of room, it
+// reallocates memory and doubles the size of its
+// storage buffer.  The reason for *doubleing* the amount of
+// memory is so the order of any algorithm using this class
+// is the same as it would be had you used a regular C array.
+// The penalty for reallocating and copying
+// For example consider adding n elements to a list.
+// Lets sum the number of times elements are "copied".
+// The worst case occurs when n=2^k+1 where k is integer.
+// In this case we do a big reallocation when we add the last element.
+// n elements are copied once, n/2 elements are copied twice,
+// n/4 elements are copied 3 times, and so on ...
+//    total == n* (1+1/2 + 1/4 + 1/8 + ...) == n * 2
+// So we do n*2 copies.  Therefore adding n
+// elements to an Array is still O(n).
+// The memory usage is also of the same order as if a C array was used.
+// An Array uses less than double the minimum needed space.  Again, we
+// see that we are within a small constant multiple.
+//
+// Why no "realloc" to avoid the copy when reallocating memory?
+// You have a choice to either use malloc/free and friends
+// or to use new/delete.  Its bad mojo to mix these.  new/delete was
+// chosen to be C++ish and have the array elements constructors/destructors
+// invoked as expected.
+//
+//
+
+#ifndef SM_ARRAY_H
+#define SM_ARRAY_H
+
+#include <assert.h>
+#include <stdio.h>
+
+template <class Type>
+class Array
+{
+public:
+	Array(int s = 0);
+	Array(Array<Type> &array);
+	~Array();
+	void allocate(int s);
+	void SetSize(int s);
+	void Pack();
+	Type &Add(Type);
+	void AddUnique(Type);
+	int Contains(Type);
+	void Insert(Type, int);
+	int IndexOf(Type);
+	void Remove(Type);
+	void DelIndex(int i);
+	Type &DelIndexWithLast(int i);
+	Type *element;
+	int count;
+	int array_size;
+	const Type &operator[](int i) const
+	{
+		assert(i >= 0 && i < count);
+		return element[i];
+	}
+	Type &operator[](int i)
+	{
+		assert(i >= 0 && i < count);
+		return element[i];
+	}
+	Type &Pop()
+	{
+		assert(count);
+		count--;
+		return element[count];
+	}
+	Array<Type> &copy(const Array<Type> &array);
+	Array<Type> &operator=(Array<Type> &array);
+};
+
+template <class Type>
+Array<Type>::Array(int s)
+{
+	if (s == -1) return;
+	count = 0;
+	array_size = 0;
+	element = NULL;
+	if (s)
+	{
+		allocate(s);
+	}
+}
+
+template <class Type>
+Array<Type>::Array(Array<Type> &array)
+{
+	count = 0;
+	array_size = 0;
+	element = NULL;
+	*this = array;
+}
+
+template <class Type>
+Array<Type> &Array<Type>::copy(const Array<Type> &array)
+{
+	assert(array.array_size >= 0);
+	count = 0;
+	for (int i = 0; i < array.count; i++)
+	{
+		Add(array[i]);
+	}
+	return *this;
+}
+template <class Type>
+Array<Type> &Array<Type>::operator=(Array<Type> &array)
+{
+	if (array.array_size < 0)  //  negative number means steal the data buffer instead of copying
+	{
+		delete[] element;
+		element = array.element;
+		array_size = -array.array_size;
+		count = array.count;
+		array.count = array.array_size = 0;
+		array.element = NULL;
+		return *this;
+	}
+	count = 0;
+	for (int i = 0; i < array.count; i++)
+	{
+		Add(array[i]);
+	}
+	return *this;
+}
+
+template <class Type>
+Array<Type>::~Array()
+{
+	if (element != NULL && array_size != 0)
+	{
+		delete[] element;
+	}
+	count = 0;
+	array_size = 0;
+	element = NULL;
+}
+
+template <class Type>
+void Array<Type>::allocate(int s)
+{
+	assert(s > 0);
+	assert(s >= count);
+	if (s == array_size) return;
+	Type *old = element;
+	array_size = s;
+	element = new Type[array_size];
+	assert(element);
+	for (int i = 0; i < count; i++)
+	{
+		element[i] = old[i];
+	}
+	if (old) delete[] old;
+}
+
+template <class Type>
+void Array<Type>::SetSize(int s)
+{
+	if (s == 0)
+	{
+		if (element)
+		{
+			delete[] element;
+			element = NULL;
+		}
+		array_size = s;
+	}
+	else
+	{
+		allocate(s);
+	}
+	count = s;
+}
+
+template <class Type>
+void Array<Type>::Pack()
+{
+	allocate(count);
+}
+
+template <class Type>
+Type &Array<Type>::Add(Type t)
+{
+	assert(count <= array_size);
+	if (count == array_size)
+	{
+		allocate((array_size) ? array_size * 2 : 16);
+	}
+	//int i;
+	//for(i=0;i<count;i++) {
+	// dissallow duplicates
+	//	assert(element[i] != t);
+	//}
+	element[count++] = t;
+	return element[count - 1];
+}
+
+template <class Type>
+int Array<Type>::Contains(Type t)
+{
+	int i;
+	int found = 0;
+	for (i = 0; i < count; i++)
+	{
+		if (element[i] == t) found++;
+	}
+	return found;
+}
+
+template <class Type>
+void Array<Type>::AddUnique(Type t)
+{
+	if (!Contains(t)) Add(t);
+}
+
+template <class Type>
+void Array<Type>::DelIndex(int i)
+{
+	assert(i < count);
+	count--;
+	while (i < count)
+	{
+		element[i] = element[i + 1];
+		i++;
+	}
+}
+
+template <class Type>
+Type &Array<Type>::DelIndexWithLast(int i)
+{
+	assert(i < count);
+	count--;
+	if (i < count)
+	{
+		Type r = element[i];
+		element[i] = element[count];
+		element[count] = r;
+	}
+	return element[count];
+}
+
+template <class Type>
+void Array<Type>::Remove(Type t)
+{
+	int i;
+	for (i = 0; i < count; i++)
+	{
+		if (element[i] == t)
+		{
+			break;
+		}
+	}
+	assert(i < count);  // assert object t is in the array.
+	DelIndex(i);
+	for (i = 0; i < count; i++)
+	{
+		assert(element[i] != t);
+	}
+}
+
+template <class Type>
+void Array<Type>::Insert(Type t, int k)
+{
+	int i = count;
+	Add(t);  // to allocate space
+	while (i > k)
+	{
+		element[i] = element[i - 1];
+		i--;
+	}
+	assert(i == k);
+	element[k] = t;
+}
+
+template <class Type>
+int Array<Type>::IndexOf(Type t)
+{
+	int i;
+	for (i = 0; i < count; i++)
+	{
+		if (element[i] == t)
+		{
+			return i;
+		}
+	}
+	assert(0);
+	return -1;
+}
+
+#endif

Engine/lib/bullet/examples/Experiments/ImplicitCloth/stan/vec3n.cpp

@@ -0,0 +1,147 @@
+//
+// Big Vector and Sparse Matrix Classes
+//
+
+#include <float.h>
+
+#include "vec3n.h"
+
+float conjgrad_lasterror;
+float conjgrad_epsilon = 0.1f;
+int conjgrad_loopcount;
+int conjgrad_looplimit = 100;
+
+/*EXPORTVAR(conjgrad_lasterror);
+EXPORTVAR(conjgrad_epsilon  );
+EXPORTVAR(conjgrad_loopcount);
+EXPORTVAR(conjgrad_looplimit);
+*/
+
+int ConjGradient(float3N &X, float3Nx3N &A, float3N &B)
+{
+	// Solves for unknown X in equation AX=B
+	conjgrad_loopcount = 0;
+	int n = B.count;
+	float3N q(n), d(n), tmp(n), r(n);
+	r = B - Mul(tmp, A, X);  // just use B if X known to be zero
+	d = r;
+	float s = dot(r, r);
+	float starget = s * squared(conjgrad_epsilon);
+	while (s > starget && conjgrad_loopcount++ < conjgrad_looplimit)
+	{
+		Mul(q, A, d);  // q = A*d;
+		float a = s / dot(d, q);
+		X = X + d * a;
+		if (conjgrad_loopcount % 50 == 0)
+		{
+			r = B - Mul(tmp, A, X);
+		}
+		else
+		{
+			r = r - q * a;
+		}
+		float s_prev = s;
+		s = dot(r, r);
+		d = r + d * (s / s_prev);
+	}
+	conjgrad_lasterror = s;
+	return conjgrad_loopcount < conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
+}
+
+int ConjGradientMod(float3N &X, float3Nx3N &A, float3N &B, int3 hack)
+{
+	// obsolete!!!
+	// Solves for unknown X in equation AX=B
+	conjgrad_loopcount = 0;
+	int n = B.count;
+	float3N q(n), d(n), tmp(n), r(n);
+	r = B - Mul(tmp, A, X);  // just use B if X known to be zero
+	r[hack[0]] = r[hack[1]] = r[hack[2]] = float3(0, 0, 0);
+	d = r;
+	float s = dot(r, r);
+	float starget = s * squared(conjgrad_epsilon);
+	while (s > starget && conjgrad_loopcount++ < conjgrad_looplimit)
+	{
+		Mul(q, A, d);  // q = A*d;
+		q[hack[0]] = q[hack[1]] = q[hack[2]] = float3(0, 0, 0);
+		float a = s / dot(d, q);
+		X = X + d * a;
+		if (conjgrad_loopcount % 50 == 0)
+		{
+			r = B - Mul(tmp, A, X);
+			r[hack[0]] = r[hack[1]] = r[hack[2]] = float3(0, 0, 0);
+		}
+		else
+		{
+			r = r - q * a;
+		}
+		float s_prev = s;
+		s = dot(r, r);
+		d = r + d * (s / s_prev);
+		d[hack[0]] = d[hack[1]] = d[hack[2]] = float3(0, 0, 0);
+	}
+	conjgrad_lasterror = s;
+	return conjgrad_loopcount < conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
+}
+
+static inline void filter(float3N &V, const float3Nx3N &S)
+{
+	for (int i = 0; i < S.blocks.count; i++)
+	{
+		V[S.blocks[i].r] = V[S.blocks[i].r] * S.blocks[i].m;
+	}
+}
+
+int ConjGradientFiltered(float3N &X, const float3Nx3N &A, const float3N &B, const float3Nx3N &S)
+{
+	// Solves for unknown X in equation AX=B
+	conjgrad_loopcount = 0;
+	int n = B.count;
+	float3N q(n), d(n), tmp(n), r(n);
+	r = B - Mul(tmp, A, X);  // just use B if X known to be zero
+	filter(r, S);
+	d = r;
+	float s = dot(r, r);
+	float starget = s * squared(conjgrad_epsilon);
+	while (s > starget && conjgrad_loopcount++ < conjgrad_looplimit)
+	{
+		Mul(q, A, d);  // q = A*d;
+		filter(q, S);
+		float a = s / dot(d, q);
+		X = X + d * a;
+		if (conjgrad_loopcount % 50 == 0)
+		{
+			r = B - Mul(tmp, A, X);
+			filter(r, S);
+		}
+		else
+		{
+			r = r - q * a;
+		}
+		float s_prev = s;
+		s = dot(r, r);
+		d = r + d * (s / s_prev);
+		filter(d, S);
+	}
+	conjgrad_lasterror = s;
+	return conjgrad_loopcount < conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
+}
+
+// test big vector math library:
+static void testfloat3N()
+{
+	float3N a(2), b(2), c(2);
+	a[0] = float3(1, 2, 3);
+	a[1] = float3(4, 5, 6);
+	b[0] = float3(10, 20, 30);
+	b[1] = float3(40, 50, 60);
+	//      c =  a+b+b * 10.0f;
+	//      float d = dot(a+b,-b);
+	int k;
+	k = 0;
+}
+class dotest
+{
+public:
+	dotest() { testfloat3N(); }
+} do_test_at_program_startup;

Engine/lib/bullet/examples/Experiments/ImplicitCloth/stan/vec3n.h

@@ -0,0 +1,435 @@
+//
+// Big Vector and Sparse Matrix Classes
+//
+//   (c) S Melax 2006
+//
+// The focus is on 3D applications, so
+// the big vector is an array of float3s
+// and the matrix class uses 3x3 blocks.
+//
+// This file includes both:
+//  - basic non-optimized version
+//  - an expression optimized version
+//
+// Optimized Expressions
+//
+// We want to write sweet looking code such as V=As+Bt with big vectors.
+// However, we dont want the extra overheads with allocating memory for temps and excessing copying.
+// Instead of a full Template Metaprogramming approach, we explicitly write
+// classes to specifically handle all the expressions we are likely to use.
+// Most applicable lines of code will be of the same handful of basic forms,
+// but with different parameters for the operands.
+// In the future, if we ever need a longer expression with more operands,
+// then we will just add whatever additional building blocks that are necessary - not a big deal.
+// This approach is much simpler to develop, debug and optimize (restrict keyword, simd etc)
+// than template metaprogramming is.  We do not rely on the implementation
+// of a particular compiler to be able to expand extensive nested inline codes.
+// Additionally, we reliably get our optimizations even within a debug build.
+// Therefore we believe that our Optimized Expressions
+// are a good compromise that give us the best of both worlds.
+// The code within those important algorithms, which use this library,
+// can now remain clean and readable yet still execute quickly.
+//
+
+#ifndef SM_VEC3N_H
+#define SM_VEC3N_H
+
+#include "vecmath.h"
+#include "array.h"
+
+//#include <malloc.h>
+//template <class T> void * vec4<T>::operator new[](size_t n){ return _mm_malloc(n,64); }
+//template <class T> void vec4<T>::operator delete[](void *a) { _mm_free(a); }
+
+struct HalfConstraint
+{
+	float3 n;
+	int vi;
+	float s, t;
+	HalfConstraint(const float3 &_n, int _vi, float _t) : n(_n), vi(_vi), s(0), t(_t) {}
+	HalfConstraint() : vi(-1) {}
+};
+
+class float3Nx3N
+{
+public:
+	class Block
+	{
+	public:
+		float3x3 m;
+		int r, c;
+		float unused[16];
+
+		Block() {}
+		Block(short _r, short _c) : r(_r), c(_c) { m.x = m.y = m.z = float3(0, 0, 0); }
+	};
+	Array<Block> blocks;  // the first n blocks use as the diagonal.
+	int n;
+	void Zero();
+	void InitDiagonal(float d);
+	void Identity() { InitDiagonal(1.0f); }
+	float3Nx3N() : n(0) {}
+	float3Nx3N(int _n) : n(_n)
+	{
+		for (int i = 0; i < n; i++) blocks.Add(Block((short)i, (short)i));
+	}
+	template <class E>
+	float3Nx3N &operator=(const E &expression)
+	{
+		expression.evalequals(*this);
+		return *this;
+	}
+	template <class E>
+	float3Nx3N &operator+=(const E &expression)
+	{
+		expression.evalpluseq(*this);
+		return *this;
+	}
+	template <class E>
+	float3Nx3N &operator-=(const E &expression)
+	{
+		expression.evalmnuseq(*this);
+		return *this;
+	}
+};
+
+class float3N : public Array<float3>
+{
+public:
+	float3N(int _count = 0)
+	{
+		SetSize(_count);
+	}
+	void Zero();
+	void Init(const float3 &v);  // sets each subvector to v
+	template <class E>
+	float3N &operator=(const E &expression)
+	{
+		expression.evalequals(*this);
+		return *this;
+	}
+	template <class E>
+	float3N &operator+=(const E &expression)
+	{
+		expression.evalpluseq(*this);
+		return *this;
+	}
+	template <class E>
+	float3N &operator-=(const E &expression)
+	{
+		expression.evalmnuseq(*this);
+		return *this;
+	}
+	float3N &operator=(const float3N &V)
+	{
+		this->copy(V);
+		return *this;
+	}
+};
+
+int ConjGradient(float3N &X, float3Nx3N &A, float3N &B);
+int ConjGradientFiltered(float3N &X, const float3Nx3N &A, const float3N &B, const float3Nx3N &S, Array<HalfConstraint> &H);
+int ConjGradientFiltered(float3N &X, const float3Nx3N &A, const float3N &B, const float3Nx3N &S);
+
+inline float3N &Mul(float3N &r, const float3Nx3N &m, const float3N &v)
+{
+	int i;
+	for (i = 0; i < r.count; i++) r[i] = float3(0, 0, 0);
+	for (i = 0; i < m.blocks.count; i++)
+	{
+		r[m.blocks[i].r] += m.blocks[i].m * v[m.blocks[i].c];
+	}
+	return r;
+}
+
+inline float dot(const float3N &a, const float3N &b)
+{
+	float d = 0;
+	for (int i = 0; i < a.count; i++)
+	{
+		d += dot(a[i], b[i]);
+	}
+	return d;
+}
+
+inline void float3Nx3N::Zero()
+{
+	for (int i = 0; i < blocks.count; i++)
+	{
+		blocks[i].m = float3x3(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	}
+}
+
+inline void float3Nx3N::InitDiagonal(float d)
+{
+	for (int i = 0; i < blocks.count; i++)
+	{
+		blocks[i].m = (blocks[i].c == blocks[i].r) ? float3x3(d, 0, 0, 0, d, 0, 0, 0, d) : float3x3(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	}
+}
+
+inline void float3N::Zero()
+{
+	for (int i = 0; i < count; i++)
+	{
+		element[i] = float3(0, 0, 0);
+	}
+}
+
+inline void float3N::Init(const float3 &v)
+{
+	for (int i = 0; i < count; i++)
+	{
+		element[i] = v;
+	}
+}
+
+#ifdef WE_LIKE_SLOW_CODE
+
+// Unoptimized Slow Basic Version of big vector operators.
+// Uses typical implmentation for operators +/-*=
+// These operators cause lots of unnecessary construction, memory allocation, and copying.
+
+inline float3N operator+(const float3N &a, const float3N &b)
+{
+	float3N r(a.count);
+	for (int i = 0; i < a.count; i++) r[i] = a[i] + b[i];
+	return r;
+}
+
+inline float3N operator*(const float3N &a, const float &s)
+{
+	float3N r(a.count);
+	for (int i = 0; i < a.count; i++) r[i] = a[i] * s;
+	return r;
+}
+inline float3N operator/(const float3N &a, const float &s)
+{
+	float3N r(a.count);
+	return Mul(r, a, 1.0f / s);
+}
+inline float3N operator-(const float3N &a, const float3N &b)
+{
+	float3N r(a.count);
+	for (int i = 0; i < a.count; i++) r[i] = a[i] - b[i];
+	return r;
+}
+inline float3N operator-(const float3N &a)
+{
+	float3N r(a.count);
+	for (int i = 0; i < a.count; i++) r[i] = -a[i];
+	return r;
+}
+
+inline float3N operator*(const float3Nx3N &m, const float3N &v)
+{
+	float3N r(v.count);
+	return Mul(r, m, v);
+}
+inline float3N &operator-=(float3N &A, const float3N &B)
+{
+	assert(A.count == B.count);
+	for (int i = 0; i < A.count; i++) A[i] -= B[i];
+	return A;
+}
+inline float3N &operator+=(float3N &A, const float3N &B)
+{
+	assert(A.count == B.count);
+	for (int i = 0; i < A.count; i++) A[i] += B[i];
+	return A;
+}
+
+#else
+
+// Optimized Expressions
+
+class exVneg
+{
+public:
+	const float3N &v;
+	exVneg(const float3N &_v) : v(_v) {}
+	void evalequals(float3N &r) const
+	{
+		for (int i = 0; i < v.count; i++) r[i] = -v[i];
+	}
+	void evalpluseq(float3N &r) const
+	{
+		for (int i = 0; i < v.count; i++) r[i] += -v[i];
+	}
+	void evalmnuseq(float3N &r) const
+	{
+		for (int i = 0; i < v.count; i++) r[i] -= -v[i];
+	}
+};
+
+class exVaddV
+{
+public:
+	const float3N &a;
+	const float3N &b;
+	exVaddV(const float3N &_a, const float3N &_b) : a(_a), b(_b) {}
+	void evalequals(float3N &r) const
+	{
+		for (int i = 0; i < a.count; i++) r[i] = a[i] + b[i];
+	}
+	void evalpluseq(float3N &r) const
+	{
+		for (int i = 0; i < a.count; i++) r[i] += a[i] + b[i];
+	}
+	void evalmnuseq(float3N &r) const
+	{
+		for (int i = 0; i < a.count; i++) r[i] -= a[i] + b[i];
+	}
+};
+
+class exVsubV
+{
+public:
+	const float3N &a;
+	const float3N &b;
+	exVsubV(const float3N &_a, const float3N &_b) : a(_a), b(_b) {}
+	void evalequals(float3N &r) const
+	{
+		for (int i = 0; i < a.count; i++) r[i] = a[i] - b[i];
+	}
+	void evalpluseq(float3N &r) const
+	{
+		for (int i = 0; i < a.count; i++) r[i] += a[i] - b[i];
+	}
+	void evalmnuseq(float3N &r) const
+	{
+		for (int i = 0; i < a.count; i++) r[i] -= a[i] - b[i];
+	}
+};
+
+class exVs
+{
+public:
+	const float3N &v;
+	const float s;
+	exVs(const float3N &_v, const float &_s) : v(_v), s(_s) {}
+	void evalequals(float3N &r) const
+	{
+		for (int i = 0; i < v.count; i++) r[i] = v[i] * s;
+	}
+	void evalpluseq(float3N &r) const
+	{
+		for (int i = 0; i < v.count; i++) r[i] += v[i] * s;
+	}
+	void evalmnuseq(float3N &r) const
+	{
+		for (int i = 0; i < v.count; i++) r[i] -= v[i] * s;
+	}
+};
+class exAsaddB
+{
+public:
+	const float3N &a;
+	const float3N &b;
+	const float s;
+	exAsaddB(const float3N &_a, const float &_s, const float3N &_b) : a(_a), s(_s), b(_b) {}
+	void evalequals(float3N &r) const
+	{
+		for (int i = 0; i < a.count; i++) r[i] = a[i] * s + b[i];
+	}
+	void evalpluseq(float3N &r) const
+	{
+		for (int i = 0; i < a.count; i++) r[i] += a[i] * s + b[i];
+	}
+	void evalmnuseq(float3N &r) const
+	{
+		for (int i = 0; i < a.count; i++) r[i] -= a[i] * s + b[i];
+	}
+};
+class exAsaddBt
+{
+public:
+	const float3N &a;
+	const float3N &b;
+	const float s;
+	const float t;
+	exAsaddBt(const float3N &_a, const float &_s, const float3N &_b, const float &_t) : a(_a), s(_s), b(_b), t(_t) {}
+	void evalequals(float3N &r) const
+	{
+		for (int i = 0; i < a.count; i++) r[i] = a[i] * s + b[i] * t;
+	}
+	void evalpluseq(float3N &r) const
+	{
+		for (int i = 0; i < a.count; i++) r[i] += a[i] * s + b[i] * t;
+	}
+	void evalmnuseq(float3N &r) const
+	{
+		for (int i = 0; i < a.count; i++) r[i] -= a[i] * s + b[i] * t;
+	}
+};
+
+class exMv
+{
+public:
+	const float3Nx3N &m;
+	const float3N &v;
+	exMv(const float3Nx3N &_m, const float3N &_v) : m(_m), v(_v) {}
+	void evalequals(float3N &r) const { Mul(r, m, v); }
+};
+
+class exMs
+{
+public:
+	const float3Nx3N &m;
+	const float s;
+	exMs(const float3Nx3N &_m, const float &_s) : m(_m), s(_s) {}
+	void evalequals(float3Nx3N &r) const
+	{
+		for (int i = 0; i < r.blocks.count; i++) r.blocks[i].m = m.blocks[i].m * s;
+	}
+	void evalpluseq(float3Nx3N &r) const
+	{
+		for (int i = 0; i < r.blocks.count; i++) r.blocks[i].m += m.blocks[i].m * s;
+	}
+	void evalmnuseq(float3Nx3N &r) const
+	{
+		for (int i = 0; i < r.blocks.count; i++) r.blocks[i].m -= m.blocks[i].m * s;
+	}
+};
+
+class exMAsMBt
+{
+public:
+	const float3Nx3N &a;
+	const float s;
+	const float3Nx3N &b;
+	const float t;
+	exMAsMBt(const float3Nx3N &_a, const float &_s, const float3Nx3N &_b, const float &_t) : a(_a), s(_s), b(_b), t(_t) {}
+	void evalequals(float3Nx3N &r) const
+	{
+		for (int i = 0; i < r.blocks.count; i++) r.blocks[i].m = a.blocks[i].m * s + b.blocks[i].m * t;
+	}
+	void evalpluseq(float3Nx3N &r) const
+	{
+		for (int i = 0; i < r.blocks.count; i++) r.blocks[i].m += a.blocks[i].m * s + b.blocks[i].m * t;
+	}
+	void evalmnuseq(float3Nx3N &r) const
+	{
+		for (int i = 0; i < r.blocks.count; i++) r.blocks[i].m -= a.blocks[i].m * s + b.blocks[i].m * t;
+	}
+};
+
+inline exVaddV operator+(const float3N &a, const float3N &b) { return exVaddV(a, b); }
+inline exVsubV operator+(const exVneg &E, const float3N &b) { return exVsubV(b, E.v); }
+inline exVsubV operator-(const float3N &a, const float3N &b) { return exVsubV(a, b); }
+inline exVs operator*(const float3N &V, const float &s) { return exVs(V, s); }
+inline exVs operator*(const exVs &E, const float &s) { return exVs(E.v, E.s * s); }
+inline exAsaddB operator+(const exVs &E, const float3N &b) { return exAsaddB(E.v, E.s, b); }
+inline exAsaddB operator+(const float3N &b, const exVs &E) { return exAsaddB(E.v, E.s, b); }
+inline exAsaddB operator-(const float3N &b, const exVs &E) { return exAsaddB(E.v, -E.s, b); }
+inline exAsaddBt operator+(const exVs &Ea, const exVs &Eb) { return exAsaddBt(Ea.v, Ea.s, Eb.v, Eb.s); }
+inline exAsaddBt operator-(const exVs &Ea, const exVs &Eb) { return exAsaddBt(Ea.v, Ea.s, Eb.v, -Eb.s); }
+inline exMv operator*(const float3Nx3N &m, const float3N &v) { return exMv(m, v); }
+inline exMs operator*(const exMs &E, const float &s) { return exMs(E.m, E.s * s); }
+inline exMs operator*(const float3Nx3N &m, const float &s) { return exMs(m, s); }
+inline exMAsMBt operator+(const exMs &Ea, const exMs &Eb) { return exMAsMBt(Ea.m, Ea.s, Eb.m, Eb.s); }
+inline exMAsMBt operator-(const exMs &Ea, const exMs &Eb) { return exMAsMBt(Ea.m, Ea.s, Eb.m, -Eb.s); }
+
+#endif
+
+#endif

Engine/lib/bullet/examples/Experiments/ImplicitCloth/stan/vecmath.h

@@ -0,0 +1,723 @@
+//
+//
+//  Typical 3d vector math code.
+//  By S Melax 1998-2008
+//
+//
+
+#ifndef SM_VEC_MATH_H
+#define SM_VEC_MATH_H
+
+#include <stdio.h>
+#include <math.h>
+#include <assert.h>
+#include <xmmintrin.h>
+
+#define M_PIf (3.1415926535897932384626433832795f)
+
+inline float DegToRad(float angle_degrees) { return angle_degrees * M_PIf / 180.0f; }  // returns Radians.
+inline float RadToDeg(float angle_radians) { return angle_radians * 180.0f / M_PIf; }  // returns Degrees.
+
+#define OFFSET(Class, Member) (((char *)(&(((Class *)NULL)->Member))) - ((char *)NULL))
+
+int argmin(const float a[], int n);
+int argmax(const float a[], int n);
+float squared(float a);
+float clamp(float a, const float minval = 0.0f, const float maxval = 1.0f);
+int clamp(int a, const int minval, const int maxval);
+float Round(float a, float precision);
+float Interpolate(const float &f0, const float &f1, float alpha);
+
+template <class T>
+void Swap(T &a, T &b)
+{
+	T tmp = a;
+	a = b;
+	b = tmp;
+}
+
+template <class T>
+T Max(const T &a, const T &b)
+{
+	return (a > b) ? a : b;
+}
+
+template <class T>
+T Min(const T &a, const T &b)
+{
+	return (a < b) ? a : b;
+}
+
+//for template normalize functions:
+inline float squareroot(float a) { return sqrtf(a); }
+inline double squareroot(double a) { return sqrt(a); }
+
+//----------------------------------
+
+//-------- 2D --------
+
+template <class T>
+class vec2
+{
+public:
+	T x, y;
+	inline vec2()
+	{
+		x = 0;
+		y = 0;
+	}
+	inline vec2(const T &_x, const T &_y)
+	{
+		x = _x;
+		y = _y;
+	}
+	inline T &operator[](int i) { return ((T *)this)[i]; }
+	inline const T &operator[](int i) const { return ((T *)this)[i]; }
+};
+
+typedef vec2<int> int2;
+typedef vec2<float> float2;
+
+template <class T>
+inline int operator==(const vec2<T> &a, const vec2<T> &b)
+{
+	return (a.x == b.x && a.y == b.y);
+}
+template <class T>
+inline vec2<T> operator-(const vec2<T> &a, const vec2<T> &b)
+{
+	return vec2<T>(a.x - b.x, a.y - b.y);
+}
+template <class T>
+inline vec2<T> operator+(const vec2<T> &a, const vec2<T> &b)
+{
+	return float2(a.x + b.x, a.y + b.y);
+}
+
+//--------- 3D ---------
+
+template <class T>
+class vec3
+{
+public:
+	T x, y, z;
+	inline vec3()
+	{
+		x = 0;
+		y = 0;
+		z = 0;
+	};
+	inline vec3(const T &_x, const T &_y, const T &_z)
+	{
+		x = _x;
+		y = _y;
+		z = _z;
+	};
+	inline T &operator[](int i) { return ((T *)this)[i]; }
+	inline const T &operator[](int i) const { return ((T *)this)[i]; }
+};
+
+typedef vec3<int> int3;
+typedef vec3<short> short3;
+typedef vec3<float> float3;
+
+// due to ambiguity there is no overloaded operators for v3*v3 use dot,cross,outerprod,cmul
+template <class T>
+inline int operator==(const vec3<T> &a, const vec3<T> &b)
+{
+	return (a.x == b.x && a.y == b.y && a.z == b.z);
+}
+template <class T>
+inline int operator!=(const vec3<T> &a, const vec3<T> &b)
+{
+	return !(a == b);
+}
+template <class T>
+inline vec3<T> operator+(const vec3<T> &a, const vec3<T> &b)
+{
+	return vec3<T>(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+template <class T>
+inline vec3<T> operator-(const vec3<T> &a, const vec3<T> &b)
+{
+	return vec3<T>(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+template <class T>
+inline vec3<T> operator-(const vec3<T> &v)
+{
+	return vec3<T>(-v.x, -v.y, -v.z);
+}
+template <class T>
+inline vec3<T> operator*(const vec3<T> &v, const T &s)
+{
+	return vec3<T>(v.x * s, v.y * s, v.z * s);
+}
+template <class T>
+inline vec3<T> operator*(T s, const vec3<T> &v)
+{
+	return v * s;
+}
+template <class T>
+inline vec3<T> operator/(const vec3<T> &v, T s)
+{
+	return vec3<T>(v.x / s, v.y / s, v.z / s);
+}
+template <class T>
+inline T dot(const vec3<T> &a, const vec3<T> &b)
+{
+	return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+template <class T>
+inline vec3<T> cmul(const vec3<T> &a, const vec3<T> &b)
+{
+	return vec3<T>(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+template <class T>
+inline vec3<T> cross(const vec3<T> &a, const vec3<T> &b)
+{
+	return vec3<T>(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
+}
+template <class T>
+inline T magnitude(const vec3<T> &v)
+{
+	return squareroot(dot(v, v));
+}
+template <class T>
+inline vec3<T> normalize(const vec3<T> &v)
+{
+	return v / magnitude(v);
+}
+template <class T>
+inline vec3<T> &operator+=(vec3<T> &a, const vec3<T> &b)
+{
+	a.x += b.x;
+	a.y += b.y;
+	a.z += b.z;
+	return a;
+}
+template <class T>
+inline vec3<T> &operator-=(vec3<T> &a, const vec3<T> &b)
+{
+	a.x -= b.x;
+	a.y -= b.y;
+	a.z -= b.z;
+	return a;
+}
+template <class T>
+inline vec3<T> &operator*=(vec3<T> &v, T s)
+{
+	v.x *= s;
+	v.y *= s;
+	v.z *= s;
+	return v;
+}
+template <class T>
+inline vec3<T> &operator/=(vec3<T> &v, T s)
+{
+	v.x /= s;
+	v.y /= s;
+	v.z /= s;
+	return v;
+}
+
+float3 safenormalize(const float3 &v);
+float3 vabs(const float3 &v);
+float3 Interpolate(const float3 &v0, const float3 &v1, float alpha);
+float3 Round(const float3 &a, float precision);
+template <class T>
+inline vec3<T> VectorMin(const vec3<T> &a, const vec3<T> &b)
+{
+	return vec3<T>(Min(a.x, b.x), Min(a.y, b.y), Min(a.z, b.z));
+}
+template <class T>
+inline vec3<T> VectorMax(const vec3<T> &a, const vec3<T> &b)
+{
+	return vec3<T>(Max(a.x, b.x), Max(a.y, b.y), Max(a.z, b.z));
+}
+int overlap(const float3 &bmina, const float3 &bmaxa, const float3 &bminb, const float3 &bmaxb);
+
+template <class T>
+class mat3x3
+{
+public:
+	vec3<T> x, y, z;  // the 3 rows of the Matrix
+	inline mat3x3() {}
+	inline mat3x3(const T &xx, const T &xy, const T &xz, const T &yx, const T &yy, const T &yz, const T &zx, const T &zy, const T &zz) : x(xx, xy, xz), y(yx, yy, yz), z(zx, zy, zz) {}
+	inline mat3x3(const vec3<T> &_x, const vec3<T> &_y, const vec3<T> &_z) : x(_x), y(_y), z(_z) {}
+	inline vec3<T> &operator[](int i) { return (&x)[i]; }
+	inline const vec3<T> &operator[](int i) const { return (&x)[i]; }
+	inline T &operator()(int r, int c) { return ((&x)[r])[c]; }
+	inline const T &operator()(int r, int c) const { return ((&x)[r])[c]; }
+};
+typedef mat3x3<float> float3x3;
+
+float3x3 Transpose(const float3x3 &m);
+template <class T>
+vec3<T> operator*(const vec3<T> &v, const mat3x3<T> &m)
+{
+	return vec3<T>((m.x.x * v.x + m.y.x * v.y + m.z.x * v.z),
+				   (m.x.y * v.x + m.y.y * v.y + m.z.y * v.z),
+				   (m.x.z * v.x + m.y.z * v.y + m.z.z * v.z));
+}
+
+float3 operator*(const float3x3 &m, const float3 &v);
+float3x3 operator*(const float3x3 &m, const float &s);
+float3x3 operator*(const float3x3 &ma, const float3x3 &mb);
+float3x3 operator/(const float3x3 &a, const float &s);
+float3x3 operator+(const float3x3 &a, const float3x3 &b);
+float3x3 operator-(const float3x3 &a, const float3x3 &b);
+float3x3 &operator+=(float3x3 &a, const float3x3 &b);
+float3x3 &operator-=(float3x3 &a, const float3x3 &b);
+float3x3 &operator*=(float3x3 &a, const float &s);
+float Determinant(const float3x3 &m);
+float3x3 Inverse(const float3x3 &a);  // its just 3x3 so we simply do that cofactor method
+float3x3 outerprod(const float3 &a, const float3 &b);
+
+//-------- 4D Math --------
+
+template <class T>
+class vec4
+{
+public:
+	T x, y, z, w;
+	inline vec4()
+	{
+		x = 0;
+		y = 0;
+		z = 0;
+		w = 0;
+	};
+	inline vec4(const T &_x, const T &_y, const T &_z, const T &_w)
+	{
+		x = _x;
+		y = _y;
+		z = _z;
+		w = _w;
+	}
+	inline vec4(const vec3<T> &v, const T &_w)
+	{
+		x = v.x;
+		y = v.y;
+		z = v.z;
+		w = _w;
+	}
+	//operator float *() { return &x;};
+	T &operator[](int i) { return ((T *)this)[i]; }
+	const T &operator[](int i) const { return ((T *)this)[i]; }
+	inline const vec3<T> &xyz() const { return *((vec3<T> *)this); }
+	inline vec3<T> &xyz() { return *((vec3<T> *)this); }
+};
+
+typedef vec4<float> float4;
+typedef vec4<int> int4;
+typedef vec4<unsigned char> byte4;
+
+template <class T>
+inline int operator==(const vec4<T> &a, const vec4<T> &b)
+{
+	return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w);
+}
+template <class T>
+inline int operator!=(const vec4<T> &a, const vec4<T> &b)
+{
+	return !(a == b);
+}
+template <class T>
+inline vec4<T> operator+(const vec4<T> &a, const vec4<T> &b)
+{
+	return vec4<T>(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
+}
+template <class T>
+inline vec4<T> operator-(const vec4<T> &a, const vec4<T> &b)
+{
+	return vec4<T>(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
+}
+template <class T>
+inline vec4<T> operator-(const vec4<T> &v)
+{
+	return vec4<T>(-v.x, -v.y, -v.z, -v.w);
+}
+template <class T>
+inline vec4<T> operator*(const vec4<T> &v, const T &s)
+{
+	return vec4<T>(v.x * s, v.y * s, v.z * s, v.w * s);
+}
+template <class T>
+inline vec4<T> operator*(T s, const vec4<T> &v)
+{
+	return v * s;
+}
+template <class T>
+inline vec4<T> operator/(const vec4<T> &v, T s)
+{
+	return vec4<T>(v.x / s, v.y / s, v.z / s, v.w / s);
+}
+template <class T>
+inline T dot(const vec4<T> &a, const vec4<T> &b)
+{
+	return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+template <class T>
+inline vec4<T> cmul(const vec4<T> &a, const vec4<T> &b)
+{
+	return vec4<T>(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);
+}
+template <class T>
+inline vec4<T> &operator+=(vec4<T> &a, const vec4<T> &b)
+{
+	a.x += b.x;
+	a.y += b.y;
+	a.z += b.z;
+	a.w += b.w;
+	return a;
+}
+template <class T>
+inline vec4<T> &operator-=(vec4<T> &a, const vec4<T> &b)
+{
+	a.x -= b.x;
+	a.y -= b.y;
+	a.z -= b.z;
+	a.w -= b.w;
+	return a;
+}
+template <class T>
+inline vec4<T> &operator*=(vec4<T> &v, T s)
+{
+	v.x *= s;
+	v.y *= s;
+	v.z *= s;
+	v.w *= s;
+	return v;
+}
+template <class T>
+inline vec4<T> &operator/=(vec4<T> &v, T s)
+{
+	v.x /= s;
+	v.y /= s;
+	v.z /= s;
+	v.w /= s;
+	return v;
+}
+template <class T>
+inline T magnitude(const vec4<T> &v)
+{
+	return squareroot(dot(v, v));
+}
+template <class T>
+inline vec4<T> normalize(const vec4<T> &v)
+{
+	return v / magnitude(v);
+}
+
+struct D3DXMATRIX;
+
+template <class T>
+class mat4x4
+{
+public:
+	vec4<T> x, y, z, w;  // the 4 rows
+	inline mat4x4() {}
+	inline mat4x4(const vec4<T> &_x, const vec4<T> &_y, const vec4<T> &_z, const vec4<T> &_w) : x(_x), y(_y), z(_z), w(_w) {}
+	inline mat4x4(const T &m00, const T &m01, const T &m02, const T &m03,
+				  const T &m10, const T &m11, const T &m12, const T &m13,
+				  const T &m20, const T &m21, const T &m22, const T &m23,
+				  const T &m30, const T &m31, const T &m32, const T &m33)
+		: x(m00, m01, m02, m03), y(m10, m11, m12, m13), z(m20, m21, m22, m23), w(m30, m31, m32, m33) {}
+	inline vec4<T> &operator[](int i)
+	{
+		assert(i >= 0 && i < 4);
+		return (&x)[i];
+	}
+	inline const vec4<T> &operator[](int i) const
+	{
+		assert(i >= 0 && i < 4);
+		return (&x)[i];
+	}
+	inline T &operator()(int r, int c)
+	{
+		assert(r >= 0 && r < 4 && c >= 0 && c < 4);
+		return ((&x)[r])[c];
+	}
+	inline const T &operator()(int r, int c) const
+	{
+		assert(r >= 0 && r < 4 && c >= 0 && c < 4);
+		return ((&x)[r])[c];
+	}
+	inline operator T *() { return &x.x; }
+	inline operator const T *() const { return &x.x; }
+	operator struct D3DXMATRIX *() { return (struct D3DXMATRIX *)this; }
+	operator const struct D3DXMATRIX *() const { return (struct D3DXMATRIX *)this; }
+};
+
+typedef mat4x4<float> float4x4;
+
+float4x4 operator*(const float4x4 &a, const float4x4 &b);
+float4 operator*(const float4 &v, const float4x4 &m);
+float4x4 Inverse(const float4x4 &m);
+float4x4 MatrixRigidInverse(const float4x4 &m);
+float4x4 MatrixTranspose(const float4x4 &m);
+float4x4 MatrixPerspectiveFov(float fovy, float Aspect, float zn, float zf);
+float4x4 MatrixTranslation(const float3 &t);
+float4x4 MatrixRotationZ(const float angle_radians);
+float4x4 MatrixLookAt(const float3 &eye, const float3 &at, const float3 &up);
+int operator==(const float4x4 &a, const float4x4 &b);
+
+//-------- Quaternion ------------
+
+template <class T>
+class quaternion : public vec4<T>
+{
+public:
+	inline quaternion()
+	{
+		this->x = this->y = this->z = 0.0f;
+		this->w = 1.0f;
+	}
+	inline quaternion(const T &_x, const T &_y, const T &_z, const T &_w)
+	{
+		this->x = _x;
+		this->y = _y;
+		this->z = _z;
+		this->w = _w;
+	}
+	inline explicit quaternion(const vec4<T> &v) : vec4<T>(v) {}
+	T angle() const { return acosf(this->w) * 2.0f; }
+	vec3<T> axis() const
+	{
+		vec3<T> a(this->x, this->y, this->z);
+		if (fabsf(angle()) < 0.0000001f) return vec3<T>(1, 0, 0);
+		return a * (1 / sinf(angle() / 2.0f));
+	}
+	inline vec3<T> xdir() const { return vec3<T>(1 - 2 * (this->y * this->y + this->z * this->z), 2 * (this->x * this->y + this->w * this->z),
+												 2 * (this->x * this->z - this->w * this->y)); }
+	inline vec3<T> ydir() const { return vec3<T>(2 * (this->x * this->y - this->w * this->z), 1 - 2 * (this->x * this->x + this->z * this->z), 2 * (this->y * this->z + this->w * this->x)); }
+	inline vec3<T> zdir() const { return vec3<T>(2 * (this->x * this->z + this->w * this->y),
+												 2 * (this->y * this->z - this->w * this->x), 1 - 2 * (this->x * this->x + this->y * this->y)); }
+	inline mat3x3<T> getmatrix() const { return mat3x3<T>(xdir(), ydir(), zdir()); }
+	//operator float3x3() { return getmatrix(); }
+	void Normalize();
+};
+
+template <class T>
+inline quaternion<T> quatfrommat(const mat3x3<T> &m)
+{
+	T magw = m[0][0] + m[1][1] + m[2][2];
+	T magxy;
+	T magzw;
+	vec3<T> pre;
+	vec3<T> prexy;
+	vec3<T> prezw;
+	quaternion<T> postxy;
+	quaternion<T> postzw;
+	quaternion<T> post;
+	int wvsz = (magw > m[2][2]);
+	magzw = (wvsz) ? magw : m[2][2];
+	prezw = (wvsz) ? vec3<T>(1.0f, 1.0f, 1.0f) : vec3<T>(-1.0f, -1.0f, 1.0f);
+	postzw = (wvsz) ? quaternion<T>(0.0f, 0.0f, 0.0f, 1.0f) : quaternion<T>(0.0f, 0.0f, 1.0f, 0.0f);
+	int xvsy = (m[0][0] > m[1][1]);
+	magxy = (xvsy) ? m[0][0] : m[1][1];
+	prexy = (xvsy) ? vec3<T>(1.0f, -1.0f, -1.0f) : vec3<T>(-1.0f, 1.0f, -1.0f);
+	postxy = (xvsy) ? quaternion<T>(1.0f, 0.0f, 0.0f, 0.0f) : quaternion<T>(0.0f, 1.0f, 0.0f, 0.0f);
+	int zwvsxy = (magzw > magxy);
+	pre = (zwvsxy) ? prezw : prexy;
+	post = (zwvsxy) ? postzw : postxy;
+
+	T t = pre.x * m[0][0] + pre.y * m[1][1] + pre.z * m[2][2] + 1.0f;
+	T s = 1 / sqrt(t) * 0.5f;
+	quaternion<T> qp;
+	qp.x = (pre.y * m[1][2] - pre.z * m[2][1]) * s;
+	qp.y = (pre.z * m[2][0] - pre.x * m[0][2]) * s;
+	qp.z = (pre.x * m[0][1] - pre.y * m[1][0]) * s;
+	qp.w = t * s;
+	return qp * post;
+}
+
+typedef quaternion<float> Quaternion;
+
+inline Quaternion QuatFromAxisAngle(const float3 &_v, float angle_radians)
+{
+	float3 v = normalize(_v) * sinf(angle_radians / 2.0f);
+	return Quaternion(v.x, v.y, v.z, cosf(angle_radians / 2.0f));
+}
+
+template <class T>
+inline quaternion<T> Conjugate(const quaternion<T> &q)
+{
+	return quaternion<T>(-q.x, -q.y, -q.z, q.w);
+}
+template <class T>
+inline quaternion<T> Inverse(const quaternion<T> &q)
+{
+	return Conjugate(q);
+}
+template <class T>
+inline quaternion<T> normalize(const quaternion<T> &a)
+{
+	return quaternion<T>(normalize((vec4<T> &)a));
+}
+template <class T>
+inline quaternion<T> &operator*=(quaternion<T> &a, T s)
+{
+	return (quaternion<T> &)((vec4<T> &)a *= s);
+}
+template <class T>
+inline quaternion<T> operator*(const quaternion<T> &a, float s)
+{
+	return quaternion<T>((vec4<T> &)a * s);
+}
+template <class T>
+inline quaternion<T> operator+(const quaternion<T> &a, const quaternion<T> &b)
+{
+	return quaternion<T>((vec4<T> &)a + (vec4<T> &)b);
+}
+template <class T>
+inline quaternion<T> operator-(const quaternion<T> &a, const quaternion<T> &b)
+{
+	return quaternion<T>((vec4<T> &)a - (vec4<T> &)b);
+}
+template <class T>
+inline quaternion<T> operator-(const quaternion<T> &b)
+{
+	return quaternion<T>(-(vec4<T> &)b);
+}
+template <class T>
+inline quaternion<T> operator*(const quaternion<T> &a, const quaternion<T> &b)
+{
+	return quaternion<T>(
+		a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,   //x
+		a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,   //y
+		a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,   //z
+		a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z);  //w
+}
+
+float3 rotate(const Quaternion &q, const float3 &v);
+//float3		operator*( const Quaternion& q, const float3& v );
+//float3		operator*( const float3& v, const Quaternion& q );
+
+Quaternion slerp(const Quaternion &a, const Quaternion &b, float t);
+Quaternion Interpolate(const Quaternion &q0, const Quaternion &q1, float t);
+Quaternion RotationArc(float3 v0, float3 v1);  // returns quat q where q*v0*q^-1=v1
+float4x4 MatrixFromQuatVec(const Quaternion &q, const float3 &v);
+
+inline Quaternion QuatFromMat(const float3 &t, const float3 &b, const float3 &n)
+{
+	return normalize(quatfrommat<float>(float3x3(t, b, n)));
+}
+
+//---------------- Pose ------------------
+
+class Pose
+{
+public:
+	float3 position;
+	Quaternion orientation;
+	Pose() {}
+	Pose(const float3 &p, const Quaternion &q) : position(p), orientation(q) {}
+	Pose &pose() { return *this; }
+	const Pose &pose() const { return *this; }
+};
+
+inline float3 operator*(const Pose &a, const float3 &v)
+{
+	return a.position + rotate(a.orientation, v);
+}
+
+inline Pose operator*(const Pose &a, const Pose &b)
+{
+	return Pose(a.position + rotate(a.orientation, b.position), a.orientation * b.orientation);
+}
+
+inline Pose Inverse(const Pose &a)
+{
+	Quaternion q = Inverse(a.orientation);
+	return Pose(rotate(q, -a.position), q);
+}
+
+inline Pose slerp(const Pose &p0, const Pose &p1, float t)
+{
+	return Pose(p0.position * (1.0f - t) + p1.position * t, slerp(p0.orientation, p1.orientation, t));
+}
+
+inline float4x4 MatrixFromPose(const Pose &pose)
+{
+	return MatrixFromQuatVec(pose.orientation, pose.position);
+}
+
+//------ Euler Angle -----
+
+Quaternion YawPitchRoll(float yaw, float pitch, float roll);
+float Yaw(const Quaternion &q);
+float Pitch(const Quaternion &q);
+float Roll(const Quaternion &q);
+float Yaw(const float3 &v);
+float Pitch(const float3 &v);
+
+//------- Plane ----------
+class Plane : public float4
+{
+public:
+	float3 &normal() { return xyz(); }
+	const float3 &normal() const { return xyz(); }
+	float &dist() { return w; }              // distance below origin - the D from plane equasion Ax+By+Cz+D=0
+	const float &dist() const { return w; }  // distance below origin - the D from plane equasion Ax+By+Cz+D=0
+	Plane(const float3 &n, float d) : float4(n, d) {}
+	Plane() { dist() = 0; }
+	explicit Plane(const float4 &v) : float4(v) {}
+};
+
+Plane Transform(const Plane &p, const float3 &translation, const Quaternion &rotation);
+
+inline Plane PlaneFlip(const Plane &p) { return Plane(-p.normal(), -p.dist()); }
+inline int operator==(const Plane &a, const Plane &b) { return (a.normal() == b.normal() && a.dist() == b.dist()); }
+inline int coplanar(const Plane &a, const Plane &b) { return (a == b || a == PlaneFlip(b)); }
+
+float3 PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1);
+float3 PlaneProject(const Plane &plane, const float3 &point);
+float3 PlanesIntersection(const Plane &p0, const Plane &p1, const Plane &p2);
+float3 PlanesIntersection(const Plane *planes, int planes_count, const float3 &seed = float3(0, 0, 0));
+
+int Clip(const Plane &p, const float3 *verts_in, int count, float *verts_out);                                                            // verts_out must be preallocated with sufficient size >= count+1 or more if concave
+int ClipPolyPoly(const float3 &normal, const float3 *clipper, int clipper_count, const float3 *verts_in, int in_count, float3 *scratch);  //scratch must be preallocated
+
+//--------- Utility Functions ------
+
+float3 PlaneLineIntersection(const float3 &normal, const float dist, const float3 &p0, const float3 &p1);
+float3 LineProject(const float3 &p0, const float3 &p1, const float3 &a);  // projects a onto infinite line p0p1
+float LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a);
+int BoxInside(const float3 &p, const float3 &bmin, const float3 &bmax);
+int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax, float3 *impact);
+float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint = NULL, float3 *vpoint = NULL);
+float3 TriNormal(const float3 &v0, const float3 &v1, const float3 &v2);
+float3 NormalOf(const float3 *vert, const int n);
+Quaternion VirtualTrackBall(const float3 &cop, const float3 &cor, const float3 &dir0, const float3 &dir1);
+int Clip(const float3 &plane_normal, float plane_dist, const float3 *verts_in, int count, float *verts_out);                              // verts_out must be preallocated with sufficient size >= count+1 or more if concave
+int ClipPolyPoly(const float3 &normal, const float3 *clipper, int clipper_count, const float3 *verts_in, int in_count, float3 *scratch);  //scratch must be preallocated
+float3 Diagonal(const float3x3 &M);
+Quaternion Diagonalizer(const float3x3 &A);
+float3 Orth(const float3 &v);
+int SolveQuadratic(float a, float b, float c, float *ta, float *tb);  // if true returns roots ta,tb where ta<=tb
+int HitCheckPoly(const float3 *vert, const int n, const float3 &v0, const float3 &v1, float3 *impact = NULL, float3 *normal = NULL);
+int HitCheckRaySphere(const float3 &sphereposition, float radius, const float3 &_v0, const float3 &_v1, float3 *impact, float3 *normal);
+int HitCheckRayCylinder(const float3 &p0, const float3 &p1, float radius, const float3 &_v0, const float3 &_v1, float3 *impact, float3 *normal);
+int HitCheckSweptSphereTri(const float3 &p0, const float3 &p1, const float3 &p2, float radius, const float3 &v0, const float3 &_v1, float3 *impact, float3 *normal);
+void BoxLimits(const float3 *verts, int verts_count, float3 &bmin_out, float3 &bmax_out);
+void BoxLimits(const float4 *verts, int verts_count, float3 &bmin_out, float3 &bmax_out);
+
+template <class T>
+inline int maxdir(const T *p, int count, const T &dir)
+{
+	assert(count);
+	int m = 0;
+	for (int i = 1; i < count; i++)
+	{
+		if (dot(p[i], dir) > dot(p[m], dir)) m = i;
+	}
+	return m;
+}
+
+float3 CenterOfMass(const float3 *vertices, const int3 *tris, const int count);
+float3x3 Inertia(const float3 *vertices, const int3 *tris, const int count, const float3 &com = float3(0, 0, 0));
+float Volume(const float3 *vertices, const int3 *tris, const int count);
+int calchull(float3 *verts, int verts_count, int3 *&tris_out, int &tris_count, int vlimit);  // computes convex hull see hull.cpp
+
+#endif  // VEC_MATH_H