text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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.
*/

/// This file was created by Alex Silverman

#ifndef BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
#define BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H

#include "btBvhTriangleMeshShape.h"
#include "btMaterial.h"

///The BvhTriangleMaterialMeshShape extends the btBvhTriangleMeshShape. Its main contribution is the interface into a material array, which allows per-triangle friction and restitution.
ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape
{
    btAlignedObjectArray <btMaterial*> m_materialList;

public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

    btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true):
        btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh)
        {
            m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;

            const unsigned char *vertexbase;
            int numverts;
            PHY_ScalarType type;
            int stride;
            const unsigned char *indexbase;
            int indexstride;
            int numfaces;
            PHY_ScalarType indicestype;

            //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));

            for(int i = 0; i < meshInterface->getNumSubParts(); i++)
            {
                m_meshInterface->getLockedReadOnlyVertexIndexBase(
                    &vertexbase,
                    numverts,
                    type,
                    stride,
                    &indexbase,
                    indexstride,
                    numfaces,
                    indicestype,
                    i);
                //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16));
            }
        }

	///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb
	btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true):
        btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh)
        {
            m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;

            const unsigned char *vertexbase;
            int numverts;
            PHY_ScalarType type;
            int stride;
            const unsigned char *indexbase;
            int indexstride;
            int numfaces;
            PHY_ScalarType indicestype;

            //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));

            for(int i = 0; i < meshInterface->getNumSubParts(); i++)
            {
                m_meshInterface->getLockedReadOnlyVertexIndexBase(
                    &vertexbase,
                    numverts,
                    type,
                    stride,
                    &indexbase,
                    indexstride,
                    numfaces,
                    indicestype,
                    i);
                //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16));
            }
        }

    virtual ~btMultimaterialTriangleMeshShape()
    {
/*
        for(int i = 0; i < m_meshInterface->getNumSubParts(); i++)
        {
            btAlignedFree(m_materialValues[i]);
            m_materialLookup[i] = NULL;
        }
        btAlignedFree(m_materialValues);
        m_materialLookup = NULL;
*/
    }
	//debugging
	virtual const char*	getName()const {return "MULTIMATERIALTRIANGLEMESH";}

    ///Obtains the material for a specific triangle
    const btMaterial * getMaterialProperties(int partID, int triIndex);

}
;

#endif //BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2011 Advanced Micro Devices, Inc.  http://bulletphysics.org

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.
*/

///This file was written by Erwin Coumans

#ifndef _BT_POLYHEDRAL_FEATURES_H
#define _BT_POLYHEDRAL_FEATURES_H

#include "LinearMath/btTransform.h"
#include "LinearMath/btAlignedObjectArray.h"

#define TEST_INTERNAL_OBJECTS 1

struct btFace
{
	btAlignedObjectArray<int>	m_indices;
//	btAlignedObjectArray<int>	m_connectedFaces;
	btScalar	m_plane[4];
};

ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron
{
	public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

	btConvexPolyhedron();
	virtual	~btConvexPolyhedron();

	btAlignedObjectArray<btVector3>	m_vertices;
	btAlignedObjectArray<btFace>	m_faces;
	btAlignedObjectArray<btVector3> m_uniqueEdges;

	btVector3		m_localCenter;
	btVector3		m_extents;
	btScalar		m_radius;
	btVector3		mC;
	btVector3		mE;

	void	initialize();
	bool testContainment() const;

	void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
};

#endif //_BT_POLYHEDRAL_FEATURES_H

repo_name: JoshParnell/libphx

repo_language: C

mime_type: text/x-c++


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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 BT_SPHERE_MINKOWSKI_H
#define BT_SPHERE_MINKOWSKI_H

#include "btConvexInternalShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"

/// The btSphereShape implements an implicit sphere, centered around a local
/// origin with radius.
ATTRIBUTE_ALIGNED16(class)
btSphereShape : public btConvexInternalShape
{
public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	btSphereShape(btScalar radius) : btConvexInternalShape()
	{
		m_shapeType = SPHERE_SHAPE_PROXYTYPE;
		m_localScaling.setValue(1.0, 1.0, 1.0);
		m_implicitShapeDimensions.setZero();
		m_implicitShapeDimensions.setX(radius);
		m_collisionMargin = radius;
		m_padding = 0;
	}

	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;

	/// notice that the vectors should be unit length
	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;

	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
	btScalar     getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX(); }

	void setUnscaledRadius(btScalar radius)
	{
		m_implicitShapeDimensions.setX(radius);
		btConvexInternalShape::setMargin(radius);
	}

	/// debugging
	virtual const char* getName() const { return "SPHERE"; }

	virtual void setMargin(btScalar margin) { btConvexInternalShape::setMargin(margin); }

	virtual btScalar getMargin() const
	{
		// to improve gjk behaviour, use radius+margin as the full margin, so never
		// get into the penetration case this means, non-uniform scaling is not
		// supported anymore
		return getRadius();
	}
};

#endif //BT_SPHERE_MINKOWSKI_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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 BT_DISCRETE_DYNAMICS_WORLD_H
#define BT_DISCRETE_DYNAMICS_WORLD_H

#include "btDynamicsWorld.h"

class btDispatcher;
class btOverlappingPairCache;
class btConstraintSolver;
class btSimulationIslandManager;
class btTypedConstraint;
class btActionInterface;
class btPersistentManifold;
class btIDebugDraw;
struct InplaceSolverIslandCallback;

#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btThreads.h"

/// btDiscreteDynamicsWorld provides discrete rigid body simulation those
/// classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorld : public btDynamicsWorld
{
protected:
	btAlignedObjectArray<btTypedConstraint*> m_sortedConstraints;
	InplaceSolverIslandCallback*             m_solverIslandCallback;
	btConstraintSolver*                      m_constraintSolver;
	btSimulationIslandManager*               m_islandManager;
	btAlignedObjectArray<btTypedConstraint*> m_constraints;
	btAlignedObjectArray<btRigidBody*>       m_nonStaticRigidBodies;
	btVector3                                m_gravity;

	//for variable timesteps
	btScalar m_localTime;
	btScalar m_fixedTimeStep;
	//for variable timesteps

	bool m_ownsIslandManager;
	bool m_ownsConstraintSolver;
	bool m_synchronizeAllMotionStates;
	bool m_applySpeculativeContactRestitution;

	btAlignedObjectArray<btActionInterface*> m_actions;

	int  m_profileTimings;
	bool m_latencyMotionStateInterpolation;

	btAlignedObjectArray<btPersistentManifold*> m_predictiveManifolds;
	btSpinMutex                                 m_predictiveManifoldsMutex; // used to synchronize threads creating predictive contacts

	virtual void predictUnconstrainedMotion(btScalar timeStep);
	void         integrateTransformsInternal(btRigidBody * *bodies, int numBodies, btScalar timeStep); // can be called in parallel
	virtual void integrateTransforms(btScalar timeStep);
	virtual void calculateSimulationIslands();
	virtual void solveConstraints(btContactSolverInfo & solverInfo);
	virtual void updateActivationState(btScalar timeStep);
	void         updateActions(btScalar timeStep);
	void         startProfiling(btScalar timeStep);
	virtual void internalSingleStepSimulation(btScalar timeStep);
	void         releasePredictiveContacts();
	void         createPredictiveContactsInternal(btRigidBody * *bodies, int numBodies, btScalar timeStep); // can be called in parallel
	virtual void createPredictiveContacts(btScalar timeStep);
	virtual void saveKinematicState(btScalar timeStep);
	void         serializeRigidBodies(btSerializer * serializer);
	void         serializeDynamicsWorldInfo(btSerializer * serializer);

public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	/// this btDiscreteDynamicsWorld constructor gets created objects from the
	/// user, and will not delete those
	btDiscreteDynamicsWorld(
	    btDispatcher*             dispatcher,
	    btBroadphaseInterface*    pairCache,
	    btConstraintSolver*       constraintSolver,
	    btCollisionConfiguration* collisionConfiguration);

	virtual ~btDiscreteDynamicsWorld();

	/// if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
	virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));

	virtual void synchronizeMotionStates();

	/// this can be useful to synchronize a single rigid body -> graphics object
	void synchronizeSingleMotionState(btRigidBody * body);

	virtual void addConstraint(btTypedConstraint * constraint, bool disableCollisionsBetweenLinkedBodies = false);
	virtual void removeConstraint(btTypedConstraint * constraint);
	virtual void addAction(btActionInterface*);
	virtual void removeAction(btActionInterface*);

	btSimulationIslandManager*       getSimulationIslandManager() { return m_islandManager; }
	const btSimulationIslandManager* getSimulationIslandManager() const { return m_islandManager; }
	btCollisionWorld*                getCollisionWorld() { return this; }

	virtual void      setGravity(const btVector3& gravity);
	virtual btVector3 getGravity() const;

	virtual void addCollisionObject(
	    btCollisionObject* collisionObject,
	    int                collisionFilterGroup = btBroadphaseProxy::StaticFilter,
	    int                collisionFilterMask  = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);

	virtual void addRigidBody(btRigidBody * body);
	virtual void addRigidBody(btRigidBody * body, int group, int mask);
	virtual void removeRigidBody(btRigidBody * body);

	/// removeCollisionObject will first check if it is a rigid body, if so call
	/// removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
	virtual void removeCollisionObject(btCollisionObject * collisionObject);

	virtual void                     debugDrawConstraint(btTypedConstraint * constraint);
	virtual void                     debugDrawWorld();
	virtual void                     setConstraintSolver(btConstraintSolver * solver);
	virtual btConstraintSolver*      getConstraintSolver();
	virtual int                      getNumConstraints() const;
	virtual btTypedConstraint*       getConstraint(int index);
	virtual const btTypedConstraint* getConstraint(int index) const;

	virtual btDynamicsWorldType	getWorldType() const { return BT_DISCRETE_DYNAMICS_WORLD; }

	/// the forces on each rigidbody is accumulating together with gravity. clear
	/// this after each timestep.
	virtual void clearForces();

	/// apply gravity, call this once per timestep
	virtual void applyGravity();

	virtual void setNumTasks(int numTasks) { (void) numTasks; }

	/// obsolete, use updateActions instead
	virtual void updateVehicles(btScalar timeStep) { updateActions(timeStep); }
	/// obsolete, use addAction instead
	virtual void addVehicle(btActionInterface * vehicle);
	/// obsolete, use removeAction instead
	virtual void removeVehicle(btActionInterface * vehicle);
	/// obsolete, use addAction instead
	virtual void addCharacter(btActionInterface * character);
	/// obsolete, use removeAction instead
	virtual void removeCharacter(btActionInterface * character);

	void setSynchronizeAllMotionStates(bool synchronizeAll) { m_synchronizeAllMotionStates = synchronizeAll; }
	bool getSynchronizeAllMotionStates() const { return m_synchronizeAllMotionStates; }
	void setApplySpeculativeContactRestitution(bool enable) { m_applySpeculativeContactRestitution = enable; }
	bool getApplySpeculativeContactRestitution() const { return m_applySpeculativeContactRestitution; }

	/// Preliminary serialization test for Bullet 2.76. Loading those files
	/// requires a separate parser (see Bullet/Demos/SerializeDemo)
	virtual void serialize(btSerializer * serializer);

	/// Interpolate motion state between previous and current transform, instead
	/// of current and next transform. This can relieve discontinuities in the
	/// rendering, due to penetrations
	void setLatencyMotionStateInterpolation(bool latencyInterpolation) { m_latencyMotionStateInterpolation = latencyInterpolation; }
	bool getLatencyMotionStateInterpolation() const { return m_latencyMotionStateInterpolation; }
};

#endif //BT_DISCRETE_DYNAMICS_WORLD_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

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 BT_SIMULATION_ISLAND_MANAGER_MT_H
#define BT_SIMULATION_ISLAND_MANAGER_MT_H

#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"

class btTypedConstraint;

///
/// SimulationIslandManagerMt -- Multithread capable version of SimulationIslandManager
///                       Splits the world up into islands which can be solved in parallel.
///                       In order to solve islands in parallel, an IslandDispatch function
///                       must be provided which will dispatch calls to multiple threads.
///                       The amount of parallelism that can be achieved depends on the number
///                       of islands. If only a single island exists, then no parallelism is
///                       possible.
///
class btSimulationIslandManagerMt : public btSimulationIslandManager
{
public:
    struct Island
    {
        // a simulation island consisting of bodies, manifolds and constraints,
        // to be passed into a constraint solver.
        btAlignedObjectArray<btCollisionObject*> bodyArray;
        btAlignedObjectArray<btPersistentManifold*> manifoldArray;
        btAlignedObjectArray<btTypedConstraint*> constraintArray;
        int id;  // island id
        bool isSleeping;

        void append( const Island& other );  // add bodies, manifolds, constraints to my own
    };
    struct	IslandCallback
    {
        virtual ~IslandCallback() {};

        virtual	void processIsland( btCollisionObject** bodies,
                                    int numBodies,
                                    btPersistentManifold** manifolds,
                                    int numManifolds,
                                    btTypedConstraint** constraints,
                                    int numConstraints,
                                    int islandId
                                    ) = 0;
    };
    typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
    static void serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
    static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
protected:
    btAlignedObjectArray<Island*> m_allocatedIslands;  // owner of all Islands
    btAlignedObjectArray<Island*> m_activeIslands;  // islands actively in use
    btAlignedObjectArray<Island*> m_freeIslands;  // islands ready to be reused
    btAlignedObjectArray<Island*> m_lookupIslandFromId;  // big lookup table to map islandId to Island pointer
    Island* m_batchIsland;
    int m_minimumSolverBatchSize;
    int m_batchIslandMinBodyCount;
    IslandDispatchFunc m_islandDispatch;

    Island* getIsland( int id );
    virtual Island* allocateIsland( int id, int numBodies );
    virtual void initIslandPools();
    virtual void addBodiesToIslands( btCollisionWorld* collisionWorld );
    virtual void addManifoldsToIslands( btDispatcher* dispatcher );
    virtual void addConstraintsToIslands( btAlignedObjectArray<btTypedConstraint*>& constraints );
    virtual void mergeIslands();

public:
	btSimulationIslandManagerMt();
	virtual ~btSimulationIslandManagerMt();

    virtual void buildAndProcessIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld, btAlignedObjectArray<btTypedConstraint*>& constraints, IslandCallback* callback );

	virtual void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);

    int getMinimumSolverBatchSize() const
    {
        return m_minimumSolverBatchSize;
    }
    void setMinimumSolverBatchSize( int sz )
    {
        m_minimumSolverBatchSize = sz;
    }
    IslandDispatchFunc getIslandDispatchFunction() const
    {
        return m_islandDispatch;
    }
    // allow users to set their own dispatch function for multithreaded dispatch
    void setIslandDispatchFunction( IslandDispatchFunc func )
    {
        m_islandDispatch = func;
    }
};

#endif //BT_SIMULATION_ISLAND_MANAGER_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

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 BT_DYNAMICS_WORLD_H
#define BT_DYNAMICS_WORLD_H

#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"

class btTypedConstraint;
class btActionInterface;
class btConstraintSolver;
class btDynamicsWorld;

/// Type for the callback for each tick
typedef void (*btInternalTickCallback)(btDynamicsWorld* world, btScalar timeStep);

enum btDynamicsWorldType
{
	BT_SIMPLE_DYNAMICS_WORLD         = 1,
	BT_DISCRETE_DYNAMICS_WORLD       = 2,
	BT_CONTINUOUS_DYNAMICS_WORLD     = 3,
	BT_SOFT_RIGID_DYNAMICS_WORLD     = 4,
	BT_GPU_DYNAMICS_WORLD            = 5,
	BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6
};

/// The btDynamicsWorld is the interface class for several dynamics
/// implementation, basic, discrete, parallel, and continuous etc.
class btDynamicsWorld : public btCollisionWorld
{
protected:
	btInternalTickCallback m_internalTickCallback;
	btInternalTickCallback m_internalPreTickCallback;
	void*                  m_worldUserInfo;
	btContactSolverInfo    m_solverInfo;

public:
	btDynamicsWorld(
	    btDispatcher*             dispatcher,
	    btBroadphaseInterface*    broadphase,
	    btCollisionConfiguration* collisionConfiguration)
	    : btCollisionWorld(dispatcher, broadphase, collisionConfiguration),
	      m_internalTickCallback(0),
	      m_internalPreTickCallback(0),
	      m_worldUserInfo(0)
	{
	}

	virtual ~btDynamicsWorld() {}

	/// stepSimulation proceeds the simulation over 'timeStep', units in
	/// preferably in seconds. By default, Bullet will subdivide the timestep in
	/// constant substeps of each 'fixedTimeStep'. in order to keep the
	/// simulation real-time, the maximum number of substeps can be clamped to
	/// 'maxSubSteps'. You can disable subdividing the timestep/substepping by
	/// passing maxSubSteps=0 as second argument to stepSimulation, but in that
	/// case you have to keep the timeStep constant.
	virtual int stepSimulation(
	    btScalar timeStep,
	    int      maxSubSteps   = 1,
	    btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)) = 0;

	virtual void debugDrawWorld() = 0;
	virtual void addConstraint   (btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies = false) { (void) constraint; (void) disableCollisionsBetweenLinkedBodies; }
	virtual void removeConstraint(btTypedConstraint* constraint) { (void) constraint; }
	virtual void addAction       (btActionInterface* action) = 0;
	virtual void removeAction    (btActionInterface* action) = 0;

	/// once a rigidbody is added to the dynamics world, it will get this gravity
	/// assigned existing rigidbodies in the world get gravity assigned too,
	/// during this method
	virtual void      setGravity(const btVector3& gravity) = 0;
	virtual btVector3 getGravity() const                   = 0;

	virtual void                     synchronizeMotionStates() = 0;
	virtual void                     addRigidBody(btRigidBody* body) = 0;
	virtual void                     addRigidBody(btRigidBody* body, int group, int mask) = 0;
	virtual void                     removeRigidBody(btRigidBody* body) = 0;
	virtual void                     setConstraintSolver(btConstraintSolver* solver) = 0;
	virtual btConstraintSolver*      getConstraintSolver() = 0;
	virtual int                      getNumConstraints() const { return 0; }
	virtual btTypedConstraint*       getConstraint(int index) { (void) index; return 0; }
	virtual const btTypedConstraint* getConstraint(int index) const { (void) index; return 0; }
	virtual btDynamicsWorldType      getWorldType() const = 0;
	virtual void                     clearForces() = 0;

	/// Set the callback for when an internal tick (simulation substep) happens,
	/// optional user info
	void setInternalTickCallback(
	    btInternalTickCallback cb,
	    void*                  worldUserInfo = 0,
	    bool                   isPreTick     = false)
	{
		if (isPreTick)
		{
			m_internalPreTickCallback = cb;
		}
		else
		{
			m_internalTickCallback = cb;
		}
		m_worldUserInfo = worldUserInfo;
	}

	void                       setWorldUserInfo(void* worldUserInfo) { m_worldUserInfo = worldUserInfo; }
	void*                      getWorldUserInfo() const              { return m_worldUserInfo; }
	btContactSolverInfo&       getSolverInfo()                       { return m_solverInfo; }
	const btContactSolverInfo& getSolverInfo() const                 { return m_solverInfo; }

	/// obsolete, use addAction instead.
	virtual void addVehicle(btActionInterface* vehicle) { (void) vehicle; }
	/// obsolete, use removeAction instead
	virtual void removeVehicle(btActionInterface* vehicle) { (void) vehicle; }
	/// obsolete, use addAction instead.
	virtual void addCharacter(btActionInterface* character) { (void) character; }
	/// obsolete, use removeAction instead
	virtual void removeCharacter(btActionInterface* character) { (void) character; }
};

/// do not change those serialization structures, it requires an updated
/// sBulletDNAstr/sBulletDNAstr64
struct btDynamicsWorldDoubleData
{
	btContactSolverInfoDoubleData m_solverInfo;
	btVector3DoubleData           m_gravity;
};

/// do not change those serialization structures, it requires an updated
/// sBulletDNAstr/sBulletDNAstr64
struct btDynamicsWorldFloatData
{
	btContactSolverInfoFloatData m_solverInfo;
	btVector3FloatData           m_gravity;
};

#endif // BT_DYNAMICS_WORLD_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

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 _BT_ACTION_INTERFACE_H
#define _BT_ACTION_INTERFACE_H

class btIDebugDraw;
class btCollisionWorld;

#include "LinearMath/btScalar.h"
#include "btRigidBody.h"

///Basic interface to allow actions such as vehicles and characters to be updated inside a btDynamicsWorld
class btActionInterface
{
protected:

	static btRigidBody& getFixedBody();

public:

	virtual ~btActionInterface()
	{
	}

	virtual void updateAction( btCollisionWorld* collisionWorld, btScalar deltaTimeStep)=0;

	virtual void debugDraw(btIDebugDraw* debugDrawer) = 0;

};

#endif //_BT_ACTION_INTERFACE_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

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 BT_SIMPLE_DYNAMICS_WORLD_H
#define BT_SIMPLE_DYNAMICS_WORLD_H

#include "btDynamicsWorld.h"

class btDispatcher;
class btOverlappingPairCache;
class btConstraintSolver;

///The btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds.
///Please use btDiscreteDynamicsWorld instead
class btSimpleDynamicsWorld : public btDynamicsWorld
{
protected:

	btConstraintSolver*	m_constraintSolver;

	bool	m_ownsConstraintSolver;

	void	predictUnconstrainedMotion(btScalar timeStep);

	void	integrateTransforms(btScalar timeStep);

	btVector3	m_gravity;

public:

	///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
	btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);

	virtual ~btSimpleDynamicsWorld();

	///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead
	virtual int	stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));

	virtual void	setGravity(const btVector3& gravity);

	virtual btVector3 getGravity () const;

	virtual void	addRigidBody(btRigidBody* body);

	virtual void	addRigidBody(btRigidBody* body, int group, int mask);

	virtual void	removeRigidBody(btRigidBody* body);

	virtual void	debugDrawWorld();

	virtual void	addAction(btActionInterface* action);

	virtual void	removeAction(btActionInterface* action);

	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
	virtual void	removeCollisionObject(btCollisionObject* collisionObject);

	virtual void	updateAabbs();

	virtual void	synchronizeMotionStates();

	virtual void	setConstraintSolver(btConstraintSolver* solver);

	virtual btConstraintSolver* getConstraintSolver();

	virtual btDynamicsWorldType	getWorldType() const
	{
		return BT_SIMPLE_DYNAMICS_WORLD;
	}

	virtual void	clearForces();

};

#endif //BT_SIMPLE_DYNAMICS_WORLD_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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 BT_DISCRETE_DYNAMICS_WORLD_MT_H
#define BT_DISCRETE_DYNAMICS_WORLD_MT_H

#include "btDiscreteDynamicsWorld.h"
#include "btSimulationIslandManagerMt.h"
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"

struct InplaceSolverIslandCallbackMt;

///
/// btConstraintSolverPoolMt - masquerades as a constraint solver, but really it is a threadsafe pool of them.
///
///  Each solver in the pool is protected by a mutex.  When solveGroup is called from a thread,
///  the pool looks for a solver that isn't being used by another thread, locks it, and dispatches the
///  call to the solver.
///  So long as there are at least as many solvers as there are hardware threads, it should never need to
///  spin wait.
///
class btConstraintSolverPoolMt : public btConstraintSolver
{
public:
    // create the solvers for me
    explicit btConstraintSolverPoolMt( int numSolvers );

    // pass in fully constructed solvers (destructor will delete them)
    btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers );

    virtual ~btConstraintSolverPoolMt();

    ///solve a group of constraints
    virtual btScalar solveGroup( btCollisionObject** bodies,
        int numBodies,
        btPersistentManifold** manifolds,
        int numManifolds,
        btTypedConstraint** constraints,
        int numConstraints,
        const btContactSolverInfo& info,
        btIDebugDraw* debugDrawer,
        btDispatcher* dispatcher
    ) BT_OVERRIDE;

    virtual	void reset() BT_OVERRIDE;
    virtual btConstraintSolverType getSolverType() const BT_OVERRIDE { return m_solverType; }

private:
    const static size_t kCacheLineSize = 128;
    struct ThreadSolver
    {
        btConstraintSolver* solver;
        btSpinMutex mutex;
        char _cachelinePadding[ kCacheLineSize - sizeof( btSpinMutex ) - sizeof( void* ) ];  // keep mutexes from sharing a cache line
    };
    btAlignedObjectArray<ThreadSolver> m_solvers;
    btConstraintSolverType m_solverType;

    ThreadSolver* getAndLockThreadSolver();
    void init( btConstraintSolver** solvers, int numSolvers );
};

///
/// btDiscreteDynamicsWorldMt -- a version of DiscreteDynamicsWorld with some minor changes to support
///                              solving simulation islands on multiple threads.
///
///  Should function exactly like btDiscreteDynamicsWorld.
///  Also 3 methods that iterate over all of the rigidbodies can run in parallel:
///     - predictUnconstrainedMotion
///     - integrateTransforms
///     - createPredictiveContacts
///
ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
{
protected:
    InplaceSolverIslandCallbackMt* m_solverIslandCallbackMt;

    virtual void solveConstraints(btContactSolverInfo& solverInfo) BT_OVERRIDE;

    virtual void predictUnconstrainedMotion( btScalar timeStep ) BT_OVERRIDE;

    struct UpdaterCreatePredictiveContacts : public btIParallelForBody
    {
        btScalar timeStep;
        btRigidBody** rigidBodies;
        btDiscreteDynamicsWorldMt* world;

        void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
        {
            world->createPredictiveContactsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep );
        }
    };
    virtual void createPredictiveContacts( btScalar timeStep ) BT_OVERRIDE;

    struct UpdaterIntegrateTransforms : public btIParallelForBody
    {
        btScalar timeStep;
        btRigidBody** rigidBodies;
        btDiscreteDynamicsWorldMt* world;

        void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
        {
            world->integrateTransformsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep );
        }
    };
    virtual void integrateTransforms( btScalar timeStep ) BT_OVERRIDE;

public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
        btBroadphaseInterface* pairCache,
        btConstraintSolverPoolMt* constraintSolver,   // Note this should be a solver-pool for multi-threading
        btCollisionConfiguration* collisionConfiguration
    );
	virtual ~btDiscreteDynamicsWorldMt();
};

#endif //BT_DISCRETE_DYNAMICS_WORLD_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

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 BT_RIGIDBODY_H
#define BT_RIGIDBODY_H

#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btTransform.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"

class btCollisionShape;
class btMotionState;
class btTypedConstraint;

extern btScalar gDeactivationTime;
extern bool     gDisableDeactivation;

#ifdef BT_USE_DOUBLE_PRECISION
#	define btRigidBodyData btRigidBodyDoubleData
#	define btRigidBodyDataName "btRigidBodyDoubleData"
#else
#	define btRigidBodyData btRigidBodyFloatData
#	define btRigidBodyDataName "btRigidBodyFloatData"
#endif // BT_USE_DOUBLE_PRECISION

enum btRigidBodyFlags
{
	BT_DISABLE_WORLD_GRAVITY = 1,
	/// BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83
	/// and onwards. and it BT_ENABLE_GYROPSCOPIC_FORCE becomes equivalent to
	/// BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY See Demos/GyroscopicDemo and
	/// computeGyroscopicImpulseImplicit
	BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT       = 2,
	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD = 4,
	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY  = 8,
	BT_ENABLE_GYROPSCOPIC_FORCE               = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY,
};

/// The btRigidBody is the main class for rigid body objects. It is derived from
/// btCollisionObject, so it keeps a pointer to a btCollisionShape. It is
/// recommended for performance and memory use to share btCollisionShape objects
/// whenever possible. There are 3 types of rigid bodies:
/// - A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid
///      body dynamics.
/// - B) Fixed objects with zero mass. They are not moving (basically collision
///      objects)
/// - C) Kinematic objects, which are objects without mass, but the user can
///      move them. There is on-way interaction, and Bullet calculates a
///      velocity based on the timestep and previous and current world
///      transform.
/// Bullet automatically deactivates dynamic rigid bodies, when the velocity is
/// below a threshold for a given time. Deactivated (sleeping) rigid bodies
/// don't take any processing time, except a minor broadphase collision
/// detection impact (to allow active objects to activate/wake up sleeping
/// objects)
class btRigidBody : public btCollisionObject
{
	btMatrix3x3 m_invInertiaTensorWorld;
	btVector3   m_linearVelocity;
	btVector3   m_angularVelocity;
	btScalar    m_inverseMass;
	btVector3   m_linearFactor;

	btVector3 m_gravity;
	btVector3 m_gravity_acceleration;
	btVector3 m_invInertiaLocal;
	btVector3 m_totalForce;
	btVector3 m_totalTorque;

	btScalar m_linearDamping;
	btScalar m_angularDamping;

	bool     m_additionalDamping;
	btScalar m_additionalDampingFactor;
	btScalar m_additionalLinearDampingThresholdSqr;
	btScalar m_additionalAngularDampingThresholdSqr;
	btScalar m_additionalAngularDampingFactor;

	btScalar m_linearSleepingThreshold;
	btScalar m_angularSleepingThreshold;

	/// m_optionalMotionState allows to automatic synchronize the world transform
	/// for active objects
	btMotionState* m_optionalMotionState;

	// keep track of typed constraints referencing this rigid body, to disable
	// collision between linked bodies
	btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;

	int m_rigidbodyFlags;
	int m_debugBodyId;

protected:
	ATTRIBUTE_ALIGNED16(btVector3 m_deltaLinearVelocity);
	btVector3 m_deltaAngularVelocity;
	btVector3 m_angularFactor;
	btVector3 m_invMass;
	btVector3 m_pushVelocity;
	btVector3 m_turnVelocity;

public:
	/// The btRigidBodyConstructionInfo structure provides information to create
	/// a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid
	/// body. For dynamic objects, you can use the collision shape to approximate
	/// the local inertia tensor, otherwise use the zero vector (default
	/// argument) You can use the motion state to synchronize the world transform
	/// between physics and graphics objects. And if the motion state is
	/// provided, the rigid body will initialize its initial world transform from
	/// the motion state, m_startWorldTransform is only used when you don't
	/// provide a motion state.
	struct btRigidBodyConstructionInfo
	{
		btScalar m_mass;

		/// When a motionState is provided, the rigid body will initialize its world
		/// transform from the motion state In this case, m_startWorldTransform is
		/// ignored.
		btMotionState* m_motionState;
		btTransform    m_startWorldTransform;

		btCollisionShape* m_collisionShape;
		btVector3         m_localInertia;
		btScalar          m_linearDamping;
		btScalar          m_angularDamping;

		/// best simulation results when friction is non-zero
		btScalar m_friction;

		/// the m_rollingFriction prevents rounded shapes, such as spheres,
		/// cylinders and capsules from rolling forever.
		/// See Bullet/Demos/RollingFrictionDemo for usage
		btScalar m_rollingFriction;

		/// torsional friction around contact normal
		btScalar m_spinningFriction;

		/// best simulation results using zero restitution.
		btScalar m_restitution;

		btScalar m_linearSleepingThreshold;
		btScalar m_angularSleepingThreshold;

		/// Additional damping can help avoiding lowpass jitter motion, help
		/// stability for ragdolls etc. Such damping is undesirable, so once the
		/// overall simulation quality of the rigid body dynamics system has
		/// improved, this should become obsolete
		bool     m_additionalDamping;
		btScalar m_additionalDampingFactor;
		btScalar m_additionalLinearDampingThresholdSqr;
		btScalar m_additionalAngularDampingThresholdSqr;
		btScalar m_additionalAngularDampingFactor;

		btRigidBodyConstructionInfo(
		    btScalar          mass,
		    btMotionState*    motionState,
		    btCollisionShape* collisionShape,
		    const btVector3&  localInertia = btVector3(0, 0, 0))
		    : m_mass(mass),
		      m_motionState(motionState),
		      m_collisionShape(collisionShape),
		      m_localInertia(localInertia),
		      m_linearDamping(btScalar(0.)),
		      m_angularDamping(btScalar(0.)),
		      m_friction(btScalar(0.5)),
		      m_rollingFriction(btScalar(0)),
		      m_spinningFriction(btScalar(0)),
		      m_restitution(btScalar(0.)),
		      m_linearSleepingThreshold(btScalar(0.8)),
		      m_angularSleepingThreshold(btScalar(1.f)),
		      m_additionalDamping(false),
		      m_additionalDampingFactor(btScalar(0.005)),
		      m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
		      m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
		      m_additionalAngularDampingFactor(btScalar(0.01))
		{
			m_startWorldTransform.setIdentity();
		}
	};

	/// btRigidBody constructor using construction info
	btRigidBody(const btRigidBodyConstructionInfo& constructionInfo);

	/// btRigidBody constructor for backwards compatibility.
	/// To specify friction (etc) during rigid body construction, please use the
	/// other constructor (using btRigidBodyConstructionInfo)
	btRigidBody(
	    btScalar          mass,
	    btMotionState*    motionState,
	    btCollisionShape* collisionShape,
	    const btVector3&  localInertia = btVector3(0, 0, 0));

	virtual ~btRigidBody()
	{
		// No constraints should point to this rigidbody
		// Remove constraints from the dynamics world before you delete the related
		// rigidbodies.
		btAssert(m_constraintRefs.size() == 0);
	}

protected:
	/// setupRigidBody is only used internally by the constructor
	void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo);

public:
	void proceedToTransform(const btTransform& newTrans);

	/// to keep collision detection and dynamics separate we don't store a
	/// rigidbody pointer but a rigidbody is derived from btCollisionObject, so
	/// we can safely perform an upcast
	static const btRigidBody* upcast(const btCollisionObject* colObj)
	{
		if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
			return (const btRigidBody*) colObj;
		return 0;
	}

	static btRigidBody* upcast(btCollisionObject* colObj)
	{
		if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
			return (btRigidBody*) colObj;
		return 0;
	}

	/// continuous collision detection needs prediction
	void predictIntegratedTransform(btScalar step, btTransform& predictedTransform);

	void             saveKinematicState(btScalar step);
	void             applyGravity();
	void             setGravity(const btVector3& acceleration);
	const btVector3& getGravity() const { return m_gravity_acceleration; }
	void             setDamping(btScalar lin_damping, btScalar ang_damping);
	btScalar         getLinearDamping() const { return m_linearDamping; }
	btScalar         getAngularDamping() const { return m_angularDamping; }
	btScalar         getLinearSleepingThreshold() const { return m_linearSleepingThreshold; }
	btScalar         getAngularSleepingThreshold() const { return m_angularSleepingThreshold; }
	void             applyDamping(btScalar timeStep);

	SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_collisionShape; }
	SIMD_FORCE_INLINE btCollisionShape*       getCollisionShape() { return m_collisionShape; }

	void setMassProps(btScalar mass, const btVector3& inertia);

	const btVector3& getLinearFactor() const { return m_linearFactor; }
	void             setLinearFactor(const btVector3& linearFactor)
	{
		m_linearFactor = linearFactor;
		m_invMass      = m_linearFactor * m_inverseMass;
	}

	btScalar           getInvMass() const { return m_inverseMass; }
	const btMatrix3x3& getInvInertiaTensorWorld() const { return m_invInertiaTensorWorld; }

	void             integrateVelocities(btScalar step);
	void             setCenterOfMassTransform(const btTransform& xform);
	void             applyCentralForce(const btVector3& force) { m_totalForce += force * m_linearFactor; }
	const btVector3& getTotalForce() const { return m_totalForce; }
	const btVector3& getTotalTorque() const { return m_totalTorque; }
	const btVector3& getInvInertiaDiagLocal() const { return m_invInertiaLocal; }
	void             setInvInertiaDiagLocal(const btVector3& diagInvInertia) { m_invInertiaLocal = diagInvInertia; }

	void setSleepingThresholds(btScalar linear, btScalar angular)
	{
		m_linearSleepingThreshold  = linear;
		m_angularSleepingThreshold = angular;
	}

	void applyTorque(const btVector3& torque)
	{
		m_totalTorque += torque * m_angularFactor;
	}

	void applyForce(const btVector3& force, const btVector3& rel_pos)
	{
		applyCentralForce(force);
		applyTorque(rel_pos.cross(force * m_linearFactor));
	}

	void applyCentralImpulse(const btVector3& impulse)
	{
		m_linearVelocity += impulse * m_linearFactor * m_inverseMass;
	}

	void applyTorqueImpulse(const btVector3& torque)
	{
		m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
	}

	void applyImpulse(const btVector3& impulse, const btVector3& rel_pos)
	{
		if (m_inverseMass != btScalar(0.))
		{
			applyCentralImpulse(impulse);
			if (m_angularFactor)
			{
				applyTorqueImpulse(rel_pos.cross(impulse * m_linearFactor));
			}
		}
	}

	void clearForces()
	{
		m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
		m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
	}

	void               updateInertiaTensor();
	const btVector3&   getCenterOfMassPosition() const { return m_worldTransform.getOrigin(); }
	btQuaternion       getOrientation() const;
	const btTransform& getCenterOfMassTransform() const { return m_worldTransform; }
	const btVector3&   getLinearVelocity() const { return m_linearVelocity; }
	const btVector3&   getAngularVelocity() const { return m_angularVelocity; }

	inline void setLinearVelocity(const btVector3& lin_vel)
	{
		m_updateRevision++;
		m_linearVelocity = lin_vel;
	}

	inline void setAngularVelocity(const btVector3& ang_vel)
	{
		m_updateRevision++;
		m_angularVelocity = ang_vel;
	}

	btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
	{
		// we also calculate lin/ang velocity for kinematic objects
		return m_linearVelocity + m_angularVelocity.cross(rel_pos);

		// for kinematic objects, we could also use use:
		//return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
	}

	void translate(const btVector3& v) { m_worldTransform.getOrigin() += v; }
	void getAabb(btVector3& aabbMin, btVector3& aabbMax) const;

	SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const
	{
		btVector3 r0  = pos - getCenterOfMassPosition();
		btVector3 c0  = (r0).cross(normal);
		btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
		return m_inverseMass + normal.dot(vec);
	}

	SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const
	{
		btVector3 vec = axis * getInvInertiaTensorWorld();
		return axis.dot(vec);
	}

	SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep)
	{
		if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
			return;

		if ((getLinearVelocity().length2() < m_linearSleepingThreshold * m_linearSleepingThreshold) &&
		    (getAngularVelocity().length2() < m_angularSleepingThreshold * m_angularSleepingThreshold))
		{
			m_deactivationTime += timeStep;
		}
		else
		{
			m_deactivationTime = btScalar(0.);
			setActivationState(0);
		}
	}

	SIMD_FORCE_INLINE bool wantsSleeping()
	{
		if (getActivationState() == DISABLE_DEACTIVATION) return false;

		// disable deactivation
		if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
			return false;

		if ((getActivationState() == ISLAND_SLEEPING) ||
		    (getActivationState() == WANTS_DEACTIVATION))
			return true;

		if (m_deactivationTime > gDeactivationTime)
		{
			return true;
		}
		return false;
	}

	const btBroadphaseProxy* getBroadphaseProxy() const
	{
		return m_broadphaseHandle;
	}
	btBroadphaseProxy* getBroadphaseProxy() { return m_broadphaseHandle; }
	void               setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy)
	{
		m_broadphaseHandle = broadphaseProxy;
	}

	/// btMotionState allows to automatic synchronize the world transform for
	/// active objects
	btMotionState*       getMotionState() { return m_optionalMotionState; }
	const btMotionState* getMotionState() const { return m_optionalMotionState; }
	void                 setMotionState(btMotionState* motionState)
	{
		m_optionalMotionState = motionState;
		if (m_optionalMotionState)
		{
			motionState->getWorldTransform(m_worldTransform);
		}
	}

	// for experimental overriding of friction/contact solver func
	int m_contactSolverType;
	int m_frictionSolverType;

	const btVector3& getAngularFactor() const { return m_angularFactor; }
	void             setAngularFactor(const btVector3& angFac)
	{
		m_updateRevision++;
		m_angularFactor = angFac;
	}

	void setAngularFactor(btScalar angFac)
	{
		m_updateRevision++;
		m_angularFactor.setValue(angFac, angFac, angFac);
	}

	/// is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?
	bool isInWorld() const { return (getBroadphaseProxy() != 0); }

	void               addConstraintRef(btTypedConstraint* c);
	void               removeConstraintRef(btTypedConstraint* c);
	btTypedConstraint* getConstraintRef(int index) { return m_constraintRefs[index]; }
	int                getNumConstraintRefs() const { return m_constraintRefs.size(); }
	void               setFlags(int flags) { m_rigidbodyFlags = flags; }
	int                getFlags() const { return m_rigidbodyFlags; }

	/// perform implicit force computation in world space
	btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const;

	/// perform implicit force computation in body space (inertial frame)
	btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const;

	/// explicit version is best avoided, it gains energy
	btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const;
	btVector3 getLocalInertia() const;

	virtual int calculateSerializeBufferSize() const;

	/// fills the dataBuffer and returns the struct name (and 0 on failure)
	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;

	virtual void serializeSingleObject(class btSerializer* serializer) const;
};

//@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData
/// do not change those serialization structures, it requires an updated
/// sBulletDNAstr/sBulletDNAstr64
struct btRigidBodyFloatData
{
	btCollisionObjectFloatData m_collisionObjectData;
	btMatrix3x3FloatData       m_invInertiaTensorWorld;
	btVector3FloatData         m_linearVelocity;
	btVector3FloatData         m_angularVelocity;
	btVector3FloatData         m_angularFactor;
	btVector3FloatData         m_linearFactor;
	btVector3FloatData         m_gravity;
	btVector3FloatData         m_gravity_acceleration;
	btVector3FloatData         m_invInertiaLocal;
	btVector3FloatData         m_totalForce;
	btVector3FloatData         m_totalTorque;
	float                      m_inverseMass;
	float                      m_linearDamping;
	float                      m_angularDamping;
	float                      m_additionalDampingFactor;
	float                      m_additionalLinearDampingThresholdSqr;
	float                      m_additionalAngularDampingThresholdSqr;
	float                      m_additionalAngularDampingFactor;
	float                      m_linearSleepingThreshold;
	float                      m_angularSleepingThreshold;
	int                        m_additionalDamping;
};

/// do not change those serialization structures, it requires an updated
/// sBulletDNAstr/sBulletDNAstr64
struct btRigidBodyDoubleData
{
	btCollisionObjectDoubleData m_collisionObjectData;
	btMatrix3x3DoubleData       m_invInertiaTensorWorld;
	btVector3DoubleData         m_linearVelocity;
	btVector3DoubleData         m_angularVelocity;
	btVector3DoubleData         m_angularFactor;
	btVector3DoubleData         m_linearFactor;
	btVector3DoubleData         m_gravity;
	btVector3DoubleData         m_gravity_acceleration;
	btVector3DoubleData         m_invInertiaLocal;
	btVector3DoubleData         m_totalForce;
	btVector3DoubleData         m_totalTorque;
	double                      m_inverseMass;
	double                      m_linearDamping;
	double                      m_angularDamping;
	double                      m_additionalDampingFactor;
	double                      m_additionalLinearDampingThresholdSqr;
	double                      m_additionalAngularDampingThresholdSqr;
	double                      m_additionalAngularDampingFactor;
	double                      m_linearSleepingThreshold;
	double                      m_angularSleepingThreshold;
	int                         m_additionalDamping;
	char                        m_padding[4];
};

#endif // BT_RIGIDBODY_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org

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.
*/
///original version written by Erwin Coumans, October 2013

#ifndef BT_MLCP_SOLVER_INTERFACE_H
#define BT_MLCP_SOLVER_INTERFACE_H

#include "LinearMath/btMatrixX.h"

class btMLCPSolverInterface
{
public:
	virtual ~btMLCPSolverInterface()
	{
	}

	//return true is it solves the problem successfully
	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)=0;
};

#endif //BT_MLCP_SOLVER_INTERFACE_H


text:
/* Copyright (C) 2004-2013 MBSim Development Team

Code was converted for the Bullet Continuous Collision Detection and Physics Library

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.
*/

//The original version is here
//https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
//This file is re-distributed under the ZLib license, with permission of the original author (Kilian Grundl)
//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
//STL/std::vector replaced by btAlignedObjectArray

#ifndef BT_NUMERICS_LEMKE_ALGORITHM_H_
#define BT_NUMERICS_LEMKE_ALGORITHM_H_

#include "LinearMath/btMatrixX.h"

#include <vector> //todo: replace by btAlignedObjectArray

class btLemkeAlgorithm
{
public:

  btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int & DEBUGLEVEL_ = 0) :
	  DEBUGLEVEL(DEBUGLEVEL_)
  {
	setSystem(M_, q_);
  }

  /* GETTER / SETTER */
  /**
   * \brief return info of solution process
   */
  int getInfo() {
	return info;
  }

  /**
   * \brief get the number of steps until the solution was found
   */
  int getSteps(void) {
	return steps;
  }

  /**
   * \brief set system with Matrix M and vector q
   */
  void setSystem(const btMatrixXu & M_, const btVectorXu & q_)
	{
		m_M = M_;
		m_q = q_;
  }
  /***************************************************/

  /**
   * \brief solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd)
   */
  btVectorXu solve(unsigned int maxloops = 0);

  virtual ~btLemkeAlgorithm() {
  }

protected:
  int findLexicographicMinimum(const btMatrixXu &A, const int & pivotColIndex);
  bool LexicographicPositive(const btVectorXu & v);
  void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis);
  bool greaterZero(const btVectorXu & vector);
  bool validBasis(const btAlignedObjectArray<int>& basis);

  btMatrixXu m_M;
  btVectorXu m_q;

  /**
   * \brief number of steps until the Lemke algorithm found a solution
   */
  unsigned int steps;

  /**
   * \brief define level of debug output
   */
  int DEBUGLEVEL;

  /**
   * \brief did the algorithm find a solution
   *
   * -1 : not successful
   *  0 : successful
   */
  int info;
};

#endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org

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.
*/
///original version written by Erwin Coumans, October 2013

#ifndef BT_DANTZIG_SOLVER_H
#define BT_DANTZIG_SOLVER_H

#include "btMLCPSolverInterface.h"
#include "btDantzigLCP.h"

class btDantzigSolver : public btMLCPSolverInterface
{
protected:

	btScalar m_acceptableUpperLimitSolution;

	btAlignedObjectArray<char>	m_tempBuffer;

	btAlignedObjectArray<btScalar> m_A;
	btAlignedObjectArray<btScalar> m_b;
	btAlignedObjectArray<btScalar> m_x;
	btAlignedObjectArray<btScalar> m_lo;
	btAlignedObjectArray<btScalar> m_hi;
	btAlignedObjectArray<int>	m_dependencies;
	btDantzigScratchMemory m_scratchMemory;
public:

	btDantzigSolver()
		:m_acceptableUpperLimitSolution(btScalar(1000))
	{
	}

	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
	{
		bool result = true;
		int n = b.rows();
		if (n)
		{
			int nub = 0;
			btAlignedObjectArray<btScalar> ww;
			ww.resize(n);

			const btScalar* Aptr = A.getBufferPointer();
			m_A.resize(n*n);
			for (int i=0;i<n*n;i++)
			{
				m_A[i] = Aptr[i];

			}

			m_b.resize(n);
			m_x.resize(n);
			m_lo.resize(n);
			m_hi.resize(n);
			m_dependencies.resize(n);
			for (int i=0;i<n;i++)
			{
				m_lo[i] = lo[i];
				m_hi[i] = hi[i];
				m_b[i] = b[i];
				m_x[i] = x[i];
				m_dependencies[i] = limitDependency[i];
			}

			result = btSolveDantzigLCP (n,&m_A[0],&m_x[0],&m_b[0],&ww[0],nub,&m_lo[0],&m_hi[0],&m_dependencies[0],m_scratchMemory);
			if (!result)
				return result;

//			printf("numAllocas = %d\n",numAllocas);
			for (int i=0;i<n;i++)
			{
				volatile btScalar xx = m_x[i];
				if (xx != m_x[i])
					return false;
				if (x[i] >= m_acceptableUpperLimitSolution)
				{
					return false;
				}

				if (x[i] <= -m_acceptableUpperLimitSolution)
				{
					return false;
				}
			}

			for (int i=0;i<n;i++)
			{
				x[i] = m_x[i];
			}

		}

		return result;
	}
};

#endif //BT_DANTZIG_SOLVER_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org

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.
*/
///original version written by Erwin Coumans, October 2013

#ifndef BT_MLCP_SOLVER_H
#define BT_MLCP_SOLVER_H

#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "LinearMath/btMatrixX.h"
#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"

class btMLCPSolver : public btSequentialImpulseConstraintSolver
{

protected:

	btMatrixXu m_A;
	btVectorXu m_b;
	btVectorXu m_x;
	btVectorXu m_lo;
	btVectorXu m_hi;

	///when using 'split impulse' we solve two separate (M)LCPs
	btVectorXu m_bSplit;
	btVectorXu m_xSplit;
	btVectorXu m_bSplit1;
	btVectorXu m_xSplit2;

	btAlignedObjectArray<int> m_limitDependencies;
	btAlignedObjectArray<btSolverConstraint*>	m_allConstraintPtrArray;
	btMLCPSolverInterface* m_solver;
	int m_fallback;

    /// The following scratch variables are not stateful -- contents are cleared prior to each use.
    /// They are only cached here to avoid extra memory allocations and deallocations and to ensure
    /// that multiple instances of the solver can be run in parallel.
    btMatrixXu m_scratchJ3;
    btMatrixXu m_scratchJInvM3;
    btAlignedObjectArray<int> m_scratchOfs;
    btMatrixXu m_scratchMInv;
    btMatrixXu m_scratchJ;
    btMatrixXu m_scratchJTranspose;
    btMatrixXu m_scratchTmp;

	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);

	virtual void createMLCP(const btContactSolverInfo& infoGlobal);
	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);

	//return true is it solves the problem successfully
	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);

public:

	btMLCPSolver(	 btMLCPSolverInterface* solver);
	virtual ~btMLCPSolver();

	void setMLCPSolver(btMLCPSolverInterface* solver)
	{
		m_solver = solver;
	}

	int getNumFallbacks() const
	{
		return m_fallback;
	}
	void setNumFallbacks(int num)
	{
		m_fallback = num;
	}

	virtual btConstraintSolverType	getSolverType() const
	{
		return BT_MLCP_SOLVER;
	}

};

#endif //BT_MLCP_SOLVER_H


text:
/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of                                          *
 *   The BSD-style license that is included with this library in         *
 *   the file LICENSE-BSD.TXT.                                           *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

/*

given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i)
satisfies one of
	(1) x = lo, w >= 0
	(2) x = hi, w <= 0
	(3) lo < x < hi, w = 0
A is a matrix of dimension n*n, everything else is a vector of size n*1.
lo and hi can be +/- dInfinity as needed. the first `nub' variables are
unbounded, i.e. hi and lo are assumed to be +/- dInfinity.

we restrict lo(i) <= 0 and hi(i) >= 0.

the original data (A,b) may be modified by this function.

if the `findex' (friction index) parameter is nonzero, it points to an array
of index values. in this case constraints that have findex[i] >= 0 are
special. all non-special constraints are solved for, then the lo and hi values
for the special constraints are set:
  hi[i] = abs( hi[i] * x[findex[i]] )
  lo[i] = -hi[i]
and the solution continues. this mechanism allows a friction approximation
to be implemented. the first `nub' variables are assumed to have findex < 0.

*/

#ifndef _BT_LCP_H_
#define _BT_LCP_H_

#include <stdlib.h>
#include <stdio.h>
#include <assert.h>

#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"

struct btDantzigScratchMemory
{
	btAlignedObjectArray<btScalar> m_scratch;
	btAlignedObjectArray<btScalar> L;
	btAlignedObjectArray<btScalar> d;
	btAlignedObjectArray<btScalar> delta_w;
	btAlignedObjectArray<btScalar> delta_x;
	btAlignedObjectArray<btScalar> Dell;
	btAlignedObjectArray<btScalar> ell;
	btAlignedObjectArray<btScalar*> Arows;
	btAlignedObjectArray<int> p;
	btAlignedObjectArray<int> C;
	btAlignedObjectArray<bool> state;
};

//return false if solving failed
bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w,
	int nub, btScalar *lo, btScalar *hi, int *findex,btDantzigScratchMemory& scratch);

#endif //_BT_LCP_H_


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org

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.
*/
///original version written by Erwin Coumans, October 2013

#ifndef BT_LEMKE_SOLVER_H
#define BT_LEMKE_SOLVER_H

#include "btMLCPSolverInterface.h"
#include "btLemkeAlgorithm.h"

///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time.
///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team
class btLemkeSolver : public btMLCPSolverInterface
{
protected:

public:

	btScalar	m_maxValue;
	int			m_debugLevel;
	int			m_maxLoops;
	bool		m_useLoHighBounds;

	btLemkeSolver()
		:m_maxValue(100000),
		m_debugLevel(0),
		m_maxLoops(1000),
		m_useLoHighBounds(true)
	{
	}
	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
	{

		if (m_useLoHighBounds)
		{

		BT_PROFILE("btLemkeSolver::solveMLCP");
		int n = A.rows();
		if (0==n)
			return true;

		bool fail = false;

		btVectorXu solution(n);
		btVectorXu q1;
		q1.resize(n);
		for (int row=0;row<n;row++)
		{
			q1[row] = -b[row];
		}

	//		cout << "A" << endl;
	//		cout << A << endl;

			/////////////////////////////////////

			//slow matrix inversion, replace with LU decomposition
			btMatrixXu A1;
			btMatrixXu B(n,n);
			{
				BT_PROFILE("inverse(slow)");
				A1.resize(A.rows(),A.cols());
				for (int row=0;row<A.rows();row++)
				{
					for (int col=0;col<A.cols();col++)
					{
						A1.setElem(row,col,A(row,col));
					}
				}

				btMatrixXu matrix;
				matrix.resize(n,2*n);
				for (int row=0;row<n;row++)
				{
					for (int col=0;col<n;col++)
					{
						matrix.setElem(row,col,A1(row,col));
					}
				}

				btScalar ratio,a;
				int i,j,k;
				for(i = 0; i < n; i++){
				for(j = n; j < 2*n; j++){
					if(i==(j-n))
						matrix.setElem(i,j,1.0);
					else
						matrix.setElem(i,j,0.0);
				}
			}
			for(i = 0; i < n; i++){
				for(j = 0; j < n; j++){
					if(i!=j)
					{
						btScalar v = matrix(i,i);
						if (btFuzzyZero(v))
						{
							a = 0.000001f;
						}
						ratio = matrix(j,i)/matrix(i,i);
						for(k = 0; k < 2*n; k++){
							matrix.addElem(j,k,- ratio * matrix(i,k));
						}
					}
				}
			}
			for(i = 0; i < n; i++){
				a = matrix(i,i);
				if (btFuzzyZero(a))
				{
					a = 0.000001f;
				}
				btScalar invA = 1.f/a;
				for(j = 0; j < 2*n; j++){
					matrix.mulElem(i,j,invA);
				}
			}

			for (int row=0;row<n;row++)
				{
					for (int col=0;col<n;col++)
					{
						B.setElem(row,col,matrix(row,n+col));
					}
				}
			}

		btMatrixXu b1(n,1);

		btMatrixXu M(n*2,n*2);
		for (int row=0;row<n;row++)
		{
			b1.setElem(row,0,-b[row]);
			for (int col=0;col<n;col++)
			{
				btScalar v =B(row,col);
				M.setElem(row,col,v);
				M.setElem(n+row,n+col,v);
				M.setElem(n+row,col,-v);
				M.setElem(row,n+col,-v);

			}
		}

		btMatrixXu Bb1 = B*b1;
//		q = [ (-B*b1 - lo)'   (hi + B*b1)' ]'

		btVectorXu qq;
		qq.resize(n*2);
		for (int row=0;row<n;row++)
		{
			qq[row] = -Bb1(row,0)-lo[row];
			qq[n+row] = Bb1(row,0)+hi[row];
		}

		btVectorXu z1;

		btMatrixXu y1;
		y1.resize(n,1);
		btLemkeAlgorithm lemke(M,qq,m_debugLevel);
		{
			BT_PROFILE("lemke.solve");
			lemke.setSystem(M,qq);
			z1  = lemke.solve(m_maxLoops);
		}
		for (int row=0;row<n;row++)
		{
			y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]);
		}
		btMatrixXu y1_b1(n,1);
		for (int i=0;i<n;i++)
		{
			y1_b1.setElem(i,0,y1(i,0)-b1(i,0));
		}

		btMatrixXu x1;

		x1 = B*(y1_b1);

		for (int row=0;row<n;row++)
		{
			solution[row] = x1(row,0);//n];
		}

		int errorIndexMax = -1;
		int errorIndexMin = -1;
		float errorValueMax = -1e30;
		float errorValueMin = 1e30;

		for (int i=0;i<n;i++)
		{
			x[i] = solution[i];
			volatile btScalar check = x[i];
			if (x[i] != check)
			{
				//printf("Lemke result is #NAN\n");
				x.setZero();
				return false;
			}

			//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
			//we need to figure out why it happens, and fix it, or detect it properly)
			if (x[i]>m_maxValue)
			{
				if (x[i]> errorValueMax)
				{
					fail = true;
					errorIndexMax = i;
					errorValueMax = x[i];
				}
				////printf("x[i] = %f,",x[i]);
			}
			if (x[i]<-m_maxValue)
			{
				if (x[i]<errorValueMin)
				{
					errorIndexMin = i;
					errorValueMin = x[i];
					fail = true;
					//printf("x[i] = %f,",x[i]);
				}
			}
		}
		if (fail)
		{
			int m_errorCountTimes = 0;
			if (errorIndexMin<0)
				errorValueMin = 0.f;
			if (errorIndexMax<0)
				errorValueMax = 0.f;
			m_errorCountTimes++;
		//	printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
			for (int i=0;i<n;i++)
			{
				x[i]=0.f;
			}
		}
		return !fail;
	} else

	{
			int dimension = A.rows();
		if (0==dimension)
			return true;

//		printf("================ solving using Lemke/Newton/Fixpoint\n");

		btVectorXu q;
		q.resize(dimension);
		for (int row=0;row<dimension;row++)
		{
			q[row] = -b[row];
		}

		btLemkeAlgorithm lemke(A,q,m_debugLevel);

		lemke.setSystem(A,q);

		btVectorXu solution = lemke.solve(m_maxLoops);

		//check solution

		bool fail = false;
		int errorIndexMax = -1;
		int errorIndexMin = -1;
		float errorValueMax = -1e30;
		float errorValueMin = 1e30;

		for (int i=0;i<dimension;i++)
		{
			x[i] = solution[i+dimension];
			volatile btScalar check = x[i];
			if (x[i] != check)
			{
				x.setZero();
				return false;
			}

			//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
			//we need to figure out why it happens, and fix it, or detect it properly)
			if (x[i]>m_maxValue)
			{
				if (x[i]> errorValueMax)
				{
					fail = true;
					errorIndexMax = i;
					errorValueMax = x[i];
				}
				////printf("x[i] = %f,",x[i]);
			}
			if (x[i]<-m_maxValue)
			{
				if (x[i]<errorValueMin)
				{
					errorIndexMin = i;
					errorValueMin = x[i];
					fail = true;
					//printf("x[i] = %f,",x[i]);
				}
			}
		}
		if (fail)
		{
			static int errorCountTimes = 0;
			if (errorIndexMin<0)
				errorValueMin = 0.f;
			if (errorIndexMax<0)
				errorValueMax = 0.f;
			printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
			for (int i=0;i<dimension;i++)
			{
				x[i]=0.f;
			}
		}

		return !fail;
	}
	return true;

	}

};

#endif //BT_LEMKE_SOLVER_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org

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.
*/
///original version written by Erwin Coumans, October 2013

#ifndef BT_PATH_SOLVER_H
#define BT_PATH_SOLVER_H

//#define BT_USE_PATH
#ifdef BT_USE_PATH

extern "C" {
#include "PATH/SimpleLCP.h"
#include "PATH/License.h"
#include "PATH/Error_Interface.h"
};
  void __stdcall MyError(Void *data, Char *msg)
{
	printf("Path Error: %s\n",msg);
}
  void __stdcall MyWarning(Void *data, Char *msg)
{
	printf("Path Warning: %s\n",msg);
}

Error_Interface e;

#include "btMLCPSolverInterface.h"
#include "Dantzig/lcp.h"

class btPathSolver : public btMLCPSolverInterface
{
public:

	btPathSolver()
	{
		License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0");
		e.error_data = 0;
		e.warning = MyWarning;
		e.error = MyError;
		Error_SetInterface(&e);
	}

	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
	{
		MCP_Termination status;

		int numVariables = b.rows();
		if (0==numVariables)
			return true;

			/*	 - variables - the number of variables in the problem
			- m_nnz - the number of nonzeros in the M matrix
			- m_i - a vector of size m_nnz containing the row indices for M
			- m_j - a vector of size m_nnz containing the column indices for M
			- m_ij - a vector of size m_nnz containing the data for M
			- q - a vector of size variables
			- lb - a vector of size variables containing the lower bounds on x
			- ub - a vector of size variables containing the upper bounds on x
			*/
		btAlignedObjectArray<double> values;
		btAlignedObjectArray<int> rowIndices;
		btAlignedObjectArray<int> colIndices;

		for (int i=0;i<A.rows();i++)
		{
			for (int j=0;j<A.cols();j++)
			{
				if (A(i,j)!=0.f)
				{
					//add 1, because Path starts at 1, instead of 0
					rowIndices.push_back(i+1);
					colIndices.push_back(j+1);
					values.push_back(A(i,j));
				}
			}
		}
		int numNonZero = rowIndices.size();
		btAlignedObjectArray<double> zResult;
		zResult.resize(numVariables);
		btAlignedObjectArray<double> rhs;
		btAlignedObjectArray<double> upperBounds;
		btAlignedObjectArray<double> lowerBounds;
		for (int i=0;i<numVariables;i++)
		{
			upperBounds.push_back(hi[i]);
			lowerBounds.push_back(lo[i]);
			rhs.push_back(-b[i]);
		}

		SimpleLCP(numVariables,numNonZero,&rowIndices[0],&colIndices[0],&values[0],&rhs[0],&lowerBounds[0],&upperBounds[0], &status, &zResult[0]);

		if (status != MCP_Solved)
		{
			static const char* gReturnMsgs[] = {
				"Invalid return",
				"MCP_Solved: The problem was solved",
				"MCP_NoProgress: A stationary point was found",
				"MCP_MajorIterationLimit: Major iteration limit met",
				"MCP_MinorIterationLimit: Cumulative minor iteration limit met",
				"MCP_TimeLimit: Ran out of time",
				"MCP_UserInterrupt: Control-C, typically",
				"MCP_BoundError: Problem has a bound error",
				"MCP_DomainError: Could not find starting point",
				"MCP_Infeasible: Problem has no solution",
				"MCP_Error: An error occurred within the code",
				"MCP_LicenseError: License could not be found",
				"MCP_OK"
			};

			printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
			printf("using Projected Gauss Seidel fallback\n");

			return false;
		} else
		{
			for (int i=0;i<numVariables;i++)
			{
				x[i] = zResult[i];
				//check for #NAN
				if (x[i] != zResult[i])
					return false;
			}
			return true;

		}

	}
};

#endif //BT_USE_PATH

#endif //BT_PATH_SOLVER_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org

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.
*/
///original version written by Erwin Coumans, October 2013

#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H

#include "btMLCPSolverInterface.h"

///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix)
class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
{

public:

	btScalar m_leastSquaresResidualThreshold;
	btScalar m_leastSquaresResidual;

	btSolveProjectedGaussSeidel()
		:m_leastSquaresResidualThreshold(0),
		m_leastSquaresResidual(0)
	{
	}

	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
	{
		if (!A.rows())
			return true;
		//the A matrix is sparse, so compute the non-zero elements
		A.rowComputeNonZeroElements();

		//A is a m-n matrix, m rows, n columns
		btAssert(A.rows() == b.rows());

		int i, j, numRows = A.rows();

		btScalar delta;

		for (int k = 0; k <numIterations; k++)
		{
			m_leastSquaresResidual = 0.f;
			for (i = 0; i <numRows; i++)
			{
				delta = 0.0f;
				if (useSparsity)
				{
					for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
					{
						int j = A.m_rowNonZeroElements1[i][h];
						if (j != i)//skip main diagonal
						{
							delta += A(i,j) * x[j];
						}
					}
				} else
				{
					for (j = 0; j <i; j++)
						delta += A(i,j) * x[j];
					for (j = i+1; j<numRows; j++)
						delta += A(i,j) * x[j];
				}

				btScalar aDiag = A(i,i);
				btScalar xOld = x[i];
				x [i] = (b [i] - delta) / aDiag;
				btScalar s = 1.f;

				if (limitDependency[i]>=0)
				{
					s = x[limitDependency[i]];
					if (s<0)
						s=1;
				}

				if (x[i]<lo[i]*s)
					x[i]=lo[i]*s;
				if (x[i]>hi[i]*s)
					x[i]=hi[i]*s;
				btScalar diff = x[i] - xOld;
				m_leastSquaresResidual += diff*diff;
			}

			btScalar eps  = m_leastSquaresResidualThreshold;
			if ((m_leastSquaresResidual < eps) || (k >=(numIterations-1)))
			{
#ifdef VERBOSE_PRINTF_RESIDUAL
				printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual,k);
#endif
				break;
			}
		}
		return true;
	}

};

#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org

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 BT_FEATHERSTONE_LINK_COLLIDER_H
#define BT_FEATHERSTONE_LINK_COLLIDER_H

#include "BulletCollision/CollisionDispatch/btCollisionObject.h"

#include "btMultiBody.h"

class btMultiBodyLinkCollider : public btCollisionObject
{
//protected:
public:

	btMultiBody* m_multiBody;
	int m_link;

	btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
		:m_multiBody(multiBody),
		m_link(link)
	{
		m_checkCollideWith =  true;
		//we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands
		//this means that some constraints might point to bodies that are not in the islands, causing crashes
		//if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
		{
			m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
		}
		// else
		//{
		//	m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT);
		//}

		m_internalType = CO_FEATHERSTONE_LINK;
	}
	static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
	{
		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
			return (btMultiBodyLinkCollider*)colObj;
		return 0;
	}
	static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
	{
		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
			return (btMultiBodyLinkCollider*)colObj;
		return 0;
	}

	virtual bool checkCollideWithOverride(const  btCollisionObject* co) const
	{
		const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
		if (!other)
			return true;
		if (other->m_multiBody != this->m_multiBody)
			return true;
		if (!m_multiBody->hasSelfCollision())
			return false;

		//check if 'link' has collision disabled
		if (m_link>=0)
		{
			const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
			if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
			{
				int parent_of_this = m_link;
				while (1)
				{
					if (parent_of_this==-1)
						break;
					parent_of_this = m_multiBody->getLink(parent_of_this).m_parent;
					if (parent_of_this==other->m_link)
					{
						return false;
					}
				}
			}
			else if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
			{
				if ( link.m_parent == other->m_link)
					return false;
			}

		}

		if (other->m_link>=0)
		{
			const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
			if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
			{
				int parent_of_other = other->m_link;
				while (1)
				{
					if (parent_of_other==-1)
						break;
					parent_of_other = m_multiBody->getLink(parent_of_other).m_parent;
					if (parent_of_other==this->m_link)
						return false;
				}
			}
			else if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
			{
				if (otherLink.m_parent == this->m_link)
					return false;
			}
		}
		return true;
	}
};

#endif //BT_FEATHERSTONE_LINK_COLLIDER_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org

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.
*/

///This file was written by Erwin Coumans

#ifndef BT_MULTIBODY_GEAR_CONSTRAINT_H
#define BT_MULTIBODY_GEAR_CONSTRAINT_H

#include "btMultiBodyConstraint.h"

class btMultiBodyGearConstraint : public btMultiBodyConstraint
{
protected:

	btRigidBody*	m_rigidBodyA;
	btRigidBody*	m_rigidBodyB;
	btVector3		m_pivotInA;
	btVector3		m_pivotInB;
	btMatrix3x3     m_frameInA;
	btMatrix3x3     m_frameInB;
	btScalar			m_gearRatio;
	int			m_gearAuxLink;
	btScalar	m_erp;
	btScalar	m_relativePositionTarget;

public:

	//btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
	btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);

	virtual ~btMultiBodyGearConstraint();

	virtual void finalizeMultiDof();

	virtual int getIslandIdA() const;
	virtual int getIslandIdB() const;

	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
		btMultiBodyJacobianData& data,
		const btContactSolverInfo& infoGlobal);

    const btVector3& getPivotInA() const
    {
        return m_pivotInA;
    }

    void setPivotInA(const btVector3& pivotInA)
    {
        m_pivotInA = pivotInA;
    }

	const btVector3& getPivotInB() const
	{
		return m_pivotInB;
	}

	virtual void setPivotInB(const btVector3& pivotInB)
	{
		m_pivotInB = pivotInB;
	}

    const btMatrix3x3& getFrameInA() const
    {
        return m_frameInA;
    }

    void setFrameInA(const btMatrix3x3& frameInA)
    {
        m_frameInA = frameInA;
    }

    const btMatrix3x3& getFrameInB() const
    {
        return m_frameInB;
    }

    virtual void setFrameInB(const btMatrix3x3& frameInB)
    {
        m_frameInB = frameInB;
    }

	virtual void debugDraw(class btIDebugDraw* drawer)
	{
		//todo(erwincoumans)
	}

	virtual void setGearRatio(btScalar gearRatio)
	{
		m_gearRatio = gearRatio;
	}
	virtual void setGearAuxLink(int gearAuxLink)
	{
		m_gearAuxLink = gearAuxLink;
	}
	virtual void setRelativePositionTarget(btScalar relPosTarget)
	{
		m_relativePositionTarget = relPosTarget;
	}
	virtual void setErp(btScalar erp)
	{
		m_erp = erp;
	}
};

#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org

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.
*/

///This file was written by Erwin Coumans

#ifndef BT_MULTIBODY_SLIDER_CONSTRAINT_H
#define BT_MULTIBODY_SLIDER_CONSTRAINT_H

#include "btMultiBodyConstraint.h"

class btMultiBodySliderConstraint : public btMultiBodyConstraint
{
protected:

	btRigidBody*	m_rigidBodyA;
	btRigidBody*	m_rigidBodyB;
	btVector3		m_pivotInA;
	btVector3		m_pivotInB;
    btMatrix3x3     m_frameInA;
    btMatrix3x3     m_frameInB;
    btVector3       m_jointAxis;

public:

	btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
	btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);

	virtual ~btMultiBodySliderConstraint();

	virtual void finalizeMultiDof();

	virtual int getIslandIdA() const;
	virtual int getIslandIdB() const;

	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
		btMultiBodyJacobianData& data,
		const btContactSolverInfo& infoGlobal);

    const btVector3& getPivotInA() const
    {
        return m_pivotInA;
    }

    void setPivotInA(const btVector3& pivotInA)
    {
        m_pivotInA = pivotInA;
    }

	const btVector3& getPivotInB() const
	{
		return m_pivotInB;
	}

	virtual void setPivotInB(const btVector3& pivotInB)
	{
		m_pivotInB = pivotInB;
	}

    const btMatrix3x3& getFrameInA() const
    {
        return m_frameInA;
    }

    void setFrameInA(const btMatrix3x3& frameInA)
    {
        m_frameInA = frameInA;
    }

    const btMatrix3x3& getFrameInB() const
    {
        return m_frameInB;
    }

    virtual void setFrameInB(const btMatrix3x3& frameInB)
    {
        m_frameInB = frameInB;
    }

    const btVector3& getJointAxis() const
    {
        return m_jointAxis;
    }

    void setJointAxis(const btVector3& jointAxis)
    {
        m_jointAxis = jointAxis;
    }

	virtual void debugDraw(class btIDebugDraw* drawer);

};

#endif //BT_MULTIBODY_SLIDER_CONSTRAINT_H