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_FIXED_CONSTRAINT_H
#define BT_MULTIBODY_FIXED_CONSTRAINT_H

#include "btMultiBodyConstraint.h"

class btMultiBodyFixedConstraint : public btMultiBodyConstraint
{
protected:

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

public:

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

	virtual ~btMultiBodyFixedConstraint();

	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);

};

#endif //BT_MULTIBODY_FIXED_CONSTRAINT_H


text:
/*
Copyright (c) 2015 Google Inc.

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_MULTIBODY_JOINT_FEEDBACK_H
#define BT_MULTIBODY_JOINT_FEEDBACK_H

#include "LinearMath/btSpatialAlgebra.h"

struct btMultiBodyJointFeedback
{
	btSpatialForceVector	m_reactionForces;
};

#endif //BT_MULTIBODY_JOINT_FEEDBACK_H

repo_name: JoshParnell/libphx

repo_language: C

mime_type: text/x-c++


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_MULTIBODY_CONSTRAINT_H
#define BT_MULTIBODY_CONSTRAINT_H

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

class btMultiBody;
struct btSolverInfo;

#include "btMultiBodySolverConstraint.h"

struct btMultiBodyJacobianData
{
	btAlignedObjectArray<btScalar>		m_jacobians;
	btAlignedObjectArray<btScalar>		m_deltaVelocitiesUnitImpulse;	//holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
	btAlignedObjectArray<btScalar>		m_deltaVelocities;				//holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
	btAlignedObjectArray<btScalar>		scratch_r;
	btAlignedObjectArray<btVector3>		scratch_v;
	btAlignedObjectArray<btMatrix3x3>	scratch_m;
	btAlignedObjectArray<btSolverBody>*	m_solverBodyPool;
	int									m_fixedBodyId;

};

ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraint
{
protected:

	btMultiBody*	m_bodyA;
    btMultiBody*	m_bodyB;
    int				m_linkA;
    int				m_linkB;

    int				m_numRows;
    int				m_jacSizeA;
    int				m_jacSizeBoth;
    int				m_posOffset;

	bool			m_isUnilateral;
	int				m_numDofsFinalized;
	btScalar		m_maxAppliedImpulse;

    // warning: the data block lay out is not consistent for all constraints
    // data block laid out as follows:
    // cached impulses. (one per row.)
    // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
    // positions. (one per row.)
    btAlignedObjectArray<btScalar> m_data;

	void	applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);

	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
																btMultiBodyJacobianData& data,
                                     btScalar* jacOrgA, btScalar* jacOrgB,
                                     const btVector3& constraintNormalAng,

																const btVector3& constraintNormalLin,
																const btVector3& posAworld, const btVector3& posBworld,
																btScalar posError,
																const btContactSolverInfo& infoGlobal,
                                     btScalar lowerLimit, btScalar upperLimit,
                                     bool angConstraint = false,

																btScalar relaxation = 1.f,
																bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);

public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

	btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
	virtual ~btMultiBodyConstraint();

	void updateJacobianSizes();
	void allocateJacobiansMultiDof();

	//many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later.
	virtual void setFrameInB(const btMatrix3x3& frameInB) {}
	virtual void setPivotInB(const btVector3& pivotInB){}

	virtual void finalizeMultiDof()=0;

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

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

	int	getNumRows() const
	{
		return m_numRows;
	}

	btMultiBody*	getMultiBodyA()
	{
		return m_bodyA;
	}
    btMultiBody*	getMultiBodyB()
	{
		return m_bodyB;
	}

	void	internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
	{
		btAssert(dof>=0);
		btAssert(dof < getNumRows());
		m_data[dof] = appliedImpulse;

	}

	btScalar	getAppliedImpulse(int dof)
	{
		btAssert(dof>=0);
		btAssert(dof < getNumRows());
		return m_data[dof];
	}
	// current constraint position
    // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
    // NOTE: ignored position for friction rows.
    btScalar getPosition(int row) const
	{
		return m_data[m_posOffset + row];
	}

    void setPosition(int row, btScalar pos)
	{
		m_data[m_posOffset + row] = pos;
	}

	bool isUnilateral() const
	{
		return m_isUnilateral;
	}

	// jacobian blocks.
    // each of size 6 + num_links. (jacobian2 is null if no body2.)
    // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
    btScalar* jacobianA(int row)
	{
		return &m_data[m_numRows + row * m_jacSizeBoth];
	}
    const btScalar* jacobianA(int row) const
	{
		return &m_data[m_numRows + (row * m_jacSizeBoth)];
	}
    btScalar* jacobianB(int row)
	{
		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
	}
    const btScalar* jacobianB(int row) const
	{
		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
	}

	btScalar	getMaxAppliedImpulse() const
	{
		return m_maxAppliedImpulse;
	}
	void	setMaxAppliedImpulse(btScalar maxImp)
	{
		m_maxAppliedImpulse = maxImp;
	}

	virtual void debugDraw(class btIDebugDraw* drawer)=0;

	virtual void setGearRatio(btScalar ratio) {}
	virtual void setGearAuxLink(int gearAuxLink) {}
	virtual void setRelativePositionTarget(btScalar relPosTarget){}
	virtual void setErp(btScalar erp){}

};

#endif //BT_MULTIBODY_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.
*/

#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H
#define BT_MULTIBODY_CONSTRAINT_SOLVER_H

#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "btMultiBodySolverConstraint.h"

#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS

class btMultiBody;

#include "btMultiBodyConstraint.h"

ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
{

protected:

	btMultiBodyConstraintArray			m_multiBodyNonContactConstraints;

	btMultiBodyConstraintArray			m_multiBodyNormalContactConstraints;
	btMultiBodyConstraintArray			m_multiBodyFrictionContactConstraints;

	btMultiBodyJacobianData				m_data;

	//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
	btMultiBodyConstraint**					m_tmpMultiBodyConstraints;
	int										m_tmpNumMultiBodyConstraints;

	btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);

	void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);

    btMultiBodySolverConstraint&	addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);

    btMultiBodySolverConstraint&	addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,
                                                            btScalar combinedTorsionalFriction,
                                                            btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);

	void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow,
																 btScalar* jacA,btScalar* jacB,
																 btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
																 const btContactSolverInfo& infoGlobal);

	void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
																 const btVector3& contactNormal,
																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
																 btScalar& relaxation,
																 bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);

    //either rolling or spinning friction
    void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
                                         const btVector3& contactNormal,
                                         btManifoldPoint& cp,
                                        btScalar combinedTorsionalFriction,
                                        const btContactSolverInfo& infoGlobal,
                                         btScalar& relaxation,
                                         bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);

	void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
	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 btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
	void	applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
	void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime);
public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

	///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);

	virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
};

#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_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_MULTIBODY_LINK_H
#define BT_MULTIBODY_LINK_H

#include "LinearMath/btQuaternion.h"
#include "LinearMath/btVector3.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"

enum	btMultiBodyLinkFlags
{
	BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,
	BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,
};

//both defines are now permanently enabled
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
#define TEST_SPATIAL_ALGEBRA_LAYER

//
// Various spatial helper functions
//

//namespace {

#include "LinearMath/btSpatialAlgebra.h"

//}

//
// Link struct
//

struct btMultibodyLink
{

	BT_DECLARE_ALIGNED_ALLOCATOR();

    btScalar m_mass;         // mass of link
    btVector3 m_inertiaLocal;   // inertia of link (local frame; diagonal)

    int m_parent;         // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.

    btQuaternion m_zeroRotParentToThis;    // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.

    btVector3 m_dVector;   // vector from the inboard joint pos to this link's COM. (local frame.) constant.
	//this is set to zero for planar joint (see also m_eVector comment)

    // m_eVector is constant, but depends on the joint type:
    // revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
	// planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
	// todo: fix the planar so it is consistent with the other joints

    btVector3 m_eVector;

	btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;

	enum eFeatherstoneJointType
	{
		eRevolute = 0,
		ePrismatic = 1,
		eSpherical = 2,
		ePlanar = 3,
		eFixed = 4,
		eInvalid
	};

	// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
    // for prismatic: m_axesTop[0] = zero;
    //                m_axesBottom[0] = unit vector along the joint axis.
    // for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
    //               m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
	//
	// for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
	//				  m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
	//
	// for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
	//			   m_axesTop[1][2] = zero
	//			   m_axesBottom[0] = zero
	//			   m_axesBottom[1][2] = unit vectors along the translational axes on that plane
	btSpatialMotionVector m_axes[6];
	void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
	void setAxisBottom(int dof, const btVector3 &axis)
	{
		m_axes[dof].m_bottomVec = axis;
	}
	void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
	{
		m_axes[dof].m_topVec.setValue(x, y, z);
	}
	void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
	{
		m_axes[dof].m_bottomVec.setValue(x, y, z);
	}
	const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
	const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }

	int m_dofOffset, m_cfgOffset;

    btQuaternion m_cachedRotParentToThis;   // rotates vectors in parent frame to vectors in local frame
    btVector3 m_cachedRVector;                // vector from COM of parent to COM of this link, in local frame.

    btVector3 m_appliedForce;    // In WORLD frame
    btVector3 m_appliedTorque;   // In WORLD frame

btVector3 m_appliedConstraintForce;    // In WORLD frame
    btVector3 m_appliedConstraintTorque;   // In WORLD frame

	btScalar m_jointPos[7];

    //m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
    //It gets set to zero after each internal stepSimulation call
	btScalar m_jointTorque[6];

	class btMultiBodyLinkCollider* m_collider;
	int m_flags;

	int m_dofCount, m_posVarCount;				//redundant but handy

	eFeatherstoneJointType m_jointType;

	struct btMultiBodyJointFeedback*	m_jointFeedback;

	btTransform	m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics

	const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
	const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
    const void* m_userPtr;//m_userPtr ptr needs to be managed by the developer/user!

	btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
	btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
	btScalar m_jointLowerLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
	btScalar m_jointUpperLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
	btScalar m_jointMaxForce; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
	btScalar m_jointMaxVelocity;//todo: implement this internally. It is unused for now, it is set by a URDF loader.

	// ctor: set some sensible defaults
	btMultibodyLink()
		: 	m_mass(1),
			m_parent(-1),
			m_zeroRotParentToThis(0, 0, 0, 1),
			m_cachedRotParentToThis(0, 0, 0, 1),
			m_collider(0),
			m_flags(0),
			m_dofCount(0),
			m_posVarCount(0),
			m_jointType(btMultibodyLink::eInvalid),
			m_jointFeedback(0),
			m_linkName(0),
			m_jointName(0),
            m_userPtr(0),
			m_jointDamping(0),
			m_jointFriction(0),
			m_jointLowerLimit(0),
			m_jointUpperLimit(0),
			m_jointMaxForce(0),
			m_jointMaxVelocity(0)
	{

		m_inertiaLocal.setValue(1, 1, 1);
		setAxisTop(0, 0., 0., 0.);
		setAxisBottom(0, 1., 0., 0.);
		m_dVector.setValue(0, 0, 0);
		m_eVector.setValue(0, 0, 0);
		m_cachedRVector.setValue(0, 0, 0);
		m_appliedForce.setValue( 0, 0, 0);
		m_appliedTorque.setValue(0, 0, 0);
		//
		m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
		m_jointPos[3] = 1.f;			//"quat.w"
		m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
		m_cachedWorldTransform.setIdentity();
	}

    // routine to update m_cachedRotParentToThis and m_cachedRVector
	void updateCacheMultiDof(btScalar *pq = 0)
	{
		btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);

		switch(m_jointType)
		{
			case eRevolute:
			{
				m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);

				break;
			}
			case ePrismatic:
			{
				// m_cachedRotParentToThis never changes, so no need to update
				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);

				break;
			}
			case eSpherical:
			{
				m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);

				break;
			}
			case ePlanar:
			{
				m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
				m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector);

				break;
			}
			case eFixed:
			{
				m_cachedRotParentToThis = m_zeroRotParentToThis;
				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);

				break;
			}
			default:
			{
				//invalid type
				btAssert(0);
			}
		}
	}
};

#endif //BT_MULTIBODY_LINK_H


text:
/*
 * PURPOSE:
 *   Class representing an articulated rigid body. Stores the body's
 *   current state, allows forces and torques to be set, handles
 *   timestepping and implements Featherstone's algorithm.
 *
 * COPYRIGHT:
 *   Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
 *   Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
 *   Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements

 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_MULTIBODY_H
#define BT_MULTIBODY_H

#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btAlignedObjectArray.h"

///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms
#ifdef BT_USE_DOUBLE_PRECISION
	#define btMultiBodyData	btMultiBodyDoubleData
	#define btMultiBodyDataName	"btMultiBodyDoubleData"
	#define btMultiBodyLinkData btMultiBodyLinkDoubleData
	#define btMultiBodyLinkDataName	"btMultiBodyLinkDoubleData"
#else
	#define btMultiBodyData	btMultiBodyFloatData
	#define btMultiBodyDataName	"btMultiBodyFloatData"
	#define btMultiBodyLinkData btMultiBodyLinkFloatData
	#define btMultiBodyLinkDataName	"btMultiBodyLinkFloatData"
#endif //BT_USE_DOUBLE_PRECISION

#include "btMultiBodyLink.h"
class btMultiBodyLinkCollider;

ATTRIBUTE_ALIGNED16(class) btMultiBody
{
public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

    //
    // initialization
    //

	btMultiBody(int n_links,             // NOT including the base
		btScalar mass,                // mass of base
		const btVector3 &inertia,    // inertia of base, in base frame; assumed diagonal
		bool fixedBase,           // whether the base is fixed (true) or can move (false)
		bool canSleep, bool deprecatedMultiDof=true);

	virtual ~btMultiBody();

	//note: fixed link collision with parent is always disabled
	void setupFixed(int linkIndex,
						   btScalar mass,
						   const btVector3 &inertia,
						   int parent,
						   const btQuaternion &rotParentToThis,
						   const btVector3 &parentComToThisPivotOffset,
                           const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true);

	void setupPrismatic(int i,
                               btScalar mass,
                               const btVector3 &inertia,
                               int parent,
                               const btQuaternion &rotParentToThis,
                               const btVector3 &jointAxis,
                               const btVector3 &parentComToThisPivotOffset,
							   const btVector3 &thisPivotToThisComOffset,
							   bool disableParentCollision);

    void setupRevolute(int linkIndex,            // 0 to num_links-1
                       btScalar mass,
                       const btVector3 &inertia,
                       int parentIndex,
                       const btQuaternion &rotParentToThis,  // rotate points in parent frame to this frame, when q = 0
                       const btVector3 &jointAxis,    // in my frame
                       const btVector3 &parentComToThisPivotOffset,    // vector from parent COM to joint axis, in PARENT frame
                       const btVector3 &thisPivotToThisComOffset,       // vector from joint axis to my COM, in MY frame
					   bool disableParentCollision=false);

	void setupSpherical(int linkIndex,											// 0 to num_links-1
                       btScalar mass,
                       const btVector3 &inertia,
                       int parent,
                       const btQuaternion &rotParentToThis,		// rotate points in parent frame to this frame, when q = 0
                       const btVector3 &parentComToThisPivotOffset,			// vector from parent COM to joint axis, in PARENT frame
                       const btVector3 &thisPivotToThisComOffset,				// vector from joint axis to my COM, in MY frame
					   bool disableParentCollision=false);

	void setupPlanar(int i,											// 0 to num_links-1
                       btScalar mass,
                       const btVector3 &inertia,
                       int parent,
                       const btQuaternion &rotParentToThis,		// rotate points in parent frame to this frame, when q = 0
					   const btVector3 &rotationAxis,
                       const btVector3 &parentComToThisComOffset,			// vector from parent COM to this COM, in PARENT frame
					   bool disableParentCollision=false);

	const btMultibodyLink& getLink(int index) const
	{
		return m_links[index];
	}

	btMultibodyLink& getLink(int index)
	{
		return m_links[index];
	}

	void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base
	{
		m_baseCollider = collider;
	}
	const btMultiBodyLinkCollider* getBaseCollider() const
	{
		return m_baseCollider;
	}
	btMultiBodyLinkCollider* getBaseCollider()
	{
		return m_baseCollider;
	}

	btMultiBodyLinkCollider* getLinkCollider(int index)
	{
		if (index >= 0 && index < getNumLinks())
		{
			return getLink(index).m_collider;
		}
		return 0;
	}

    //
    // get parent
    // input: link num from 0 to num_links-1
    // output: link num from 0 to num_links-1, OR -1 to mean the base.
    //
    int getParent(int link_num) const;

    //
    // get number of m_links, masses, moments of inertia
    //

    int getNumLinks() const { return m_links.size(); }
	int getNumDofs() const { return m_dofCount; }
	int getNumPosVars() const { return m_posVarCnt; }
    btScalar getBaseMass() const { return m_baseMass; }
    const btVector3 & getBaseInertia() const { return m_baseInertia; }
    btScalar getLinkMass(int i) const;
    const btVector3 & getLinkInertia(int i) const;

    //
    // change mass (incomplete: can only change base mass and inertia at present)
    //

    void setBaseMass(btScalar mass) { m_baseMass = mass; }
    void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; }

    //
    // get/set pos/vel/rot/omega for the base link
    //

    const btVector3 & getBasePos() const { return m_basePos; }    // in world frame
    const btVector3 getBaseVel() const
	{
		return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]);
	}     // in world frame
    const btQuaternion & getWorldToBaseRot() const
	{
		return m_baseQuat;
	}     // rotates world vectors into base frame
    btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); }   // in world frame

    void setBasePos(const btVector3 &pos)
	{
		m_basePos = pos;
	}

	void setBaseWorldTransform(const btTransform& tr)
	{
		setBasePos(tr.getOrigin());
		setWorldToBaseRot(tr.getRotation().inverse());

	}

	btTransform getBaseWorldTransform() const
	{
		btTransform tr;
		tr.setOrigin(getBasePos());
		tr.setRotation(getWorldToBaseRot().inverse());
		return tr;
	}

    void setBaseVel(const btVector3 &vel)
	{

		m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2];
	}
    void setWorldToBaseRot(const btQuaternion &rot)
	{
		m_baseQuat = rot;					//m_baseQuat asumed to ba alias!?
	}
    void setBaseOmega(const btVector3 &omega)
	{
		m_realBuf[0]=omega[0];
		m_realBuf[1]=omega[1];
		m_realBuf[2]=omega[2];
	}

    //
    // get/set pos/vel for child m_links (i = 0 to num_links-1)
    //

    btScalar getJointPos(int i) const;
    btScalar getJointVel(int i) const;

	btScalar * getJointVelMultiDof(int i);
	btScalar * getJointPosMultiDof(int i);

	const btScalar * getJointVelMultiDof(int i) const ;
	const btScalar * getJointPosMultiDof(int i) const ;

    void setJointPos(int i, btScalar q);
    void setJointVel(int i, btScalar qdot);
	void setJointPosMultiDof(int i, btScalar *q);
    void setJointVelMultiDof(int i, btScalar *qdot);

    //
    // direct access to velocities as a vector of 6 + num_links elements.
    // (omega first, then v, then joint velocities.)
    //
    const btScalar * getVelocityVector() const
	{
		return &m_realBuf[0];
	}
/*    btScalar * getVelocityVector()
	{
		return &real_buf[0];
	}
  */

    //
    // get the frames of reference (positions and orientations) of the child m_links
    // (i = 0 to num_links-1)
    //

    const btVector3 & getRVector(int i) const;   // vector from COM(parent(i)) to COM(i), in frame i's coords
    const btQuaternion & getParentToLocalRot(int i) const;   // rotates vectors in frame parent(i) to vectors in frame i.

    //
    // transform vectors in local frame of link i to world frame (or vice versa)
    //
    btVector3 localPosToWorld(int i, const btVector3 &vec) const;
    btVector3 localDirToWorld(int i, const btVector3 &vec) const;
    btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
    btVector3 worldDirToLocal(int i, const btVector3 &vec) const;

    //
    // transform a frame in local coordinate to a frame in world coordinate
    //
    btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const;

    //
    // calculate kinetic energy and angular momentum
    // useful for debugging.
    //

    btScalar getKineticEnergy() const;
    btVector3 getAngularMomentum() const;

    //
    // set external forces and torques. Note all external forces/torques are given in the WORLD frame.
    //

    void clearForcesAndTorques();
   void clearConstraintForces();

	void clearVelocities();

    void addBaseForce(const btVector3 &f)
	{
		m_baseForce += f;
	}
    void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
    void addLinkForce(int i, const btVector3 &f);
    void addLinkTorque(int i, const btVector3 &t);

 void addBaseConstraintForce(const btVector3 &f)
        {
                m_baseConstraintForce += f;
        }
    void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; }
    void addLinkConstraintForce(int i, const btVector3 &f);
    void addLinkConstraintTorque(int i, const btVector3 &t);

void addJointTorque(int i, btScalar Q);
	void addJointTorqueMultiDof(int i, int dof, btScalar Q);
	void addJointTorqueMultiDof(int i, const btScalar *Q);

    const btVector3 & getBaseForce() const { return m_baseForce; }
    const btVector3 & getBaseTorque() const { return m_baseTorque; }
    const btVector3 & getLinkForce(int i) const;
    const btVector3 & getLinkTorque(int i) const;
    btScalar getJointTorque(int i) const;
	btScalar * getJointTorqueMultiDof(int i);

    //
    // dynamics routines.
    //

    // timestep the velocities (given the external forces/torques set using addBaseForce etc).
    // also sets up caches for calcAccelerationDeltas.
    //
    // Note: the caller must provide three vectors which are used as
    // temporary scratch space. The idea here is to reduce dynamic
    // memory allocation: the same scratch vectors can be re-used
    // again and again for different Multibodies, instead of each
    // btMultiBody allocating (and then deallocating) their own
    // individual scratch buffers. This gives a considerable speed
    // improvement, at least on Windows (where dynamic memory
    // allocation appears to be fairly slow).
    //

	void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
                        btAlignedObjectArray<btScalar> &scratch_r,
                        btAlignedObjectArray<btVector3> &scratch_v,
                        btAlignedObjectArray<btMatrix3x3> &scratch_m,
			bool isConstraintPass=false
		);

///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead
        void stepVelocitiesMultiDof(btScalar dt,
                        btAlignedObjectArray<btScalar> &scratch_r,
                        btAlignedObjectArray<btVector3> &scratch_v,
                        btAlignedObjectArray<btMatrix3x3> &scratch_m,
                        bool isConstraintPass=false)
	{
		computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt,scratch_r,scratch_v,scratch_m,isConstraintPass);
        }

    // calcAccelerationDeltasMultiDof
    // input: force vector (in same format as jacobian, i.e.:
    //                      3 torque values, 3 force values, num_links joint torque values)
    // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
    // (existing contents of output array are replaced)
    // calcAccelerationDeltasMultiDof must have been called first.
	void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
                                btAlignedObjectArray<btScalar> &scratch_r,
                                btAlignedObjectArray<btVector3> &scratch_v) const;

	void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier)
	{
		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
                {
                        m_deltaV[dof] += delta_vee[dof] * multiplier;
                }
	}
	void processDeltaVeeMultiDof2()
	{
		applyDeltaVeeMultiDof(&m_deltaV[0],1);

		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
                {
			m_deltaV[dof] = 0.f;
		}
	}

	void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier)
	{
		//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
		//	printf("%.4f ", delta_vee[dof]*multiplier);
		//printf("\n");

		//btScalar sum = 0;
		//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
		//{
		//	sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
		//}
		//btScalar l = btSqrt(sum);

		//if (l>m_maxAppliedImpulse)
		//{
		//	multiplier *= m_maxAppliedImpulse/l;
		//}

		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
		{
			m_realBuf[dof] += delta_vee[dof] * multiplier;
			btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity);
		}
    }

    // timestep the positions (given current velocities).
	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);

    //
    // contacts
    //

    // This routine fills out a contact constraint jacobian for this body.
    // the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
    // 'normal' & 'contact_point' are both given in world coordinates.

	void fillContactJacobianMultiDof(int link,
                             const btVector3 &contact_point,
                             const btVector3 &normal,
                             btScalar *jac,
                             btAlignedObjectArray<btScalar> &scratch_r,
                             btAlignedObjectArray<btVector3> &scratch_v,
							 btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }

	//a more general version of fillContactJacobianMultiDof which does not assume..
	//.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
	void fillConstraintJacobianMultiDof(int link,
                             const btVector3 &contact_point,
							 const btVector3 &normal_ang,
                             const btVector3 &normal_lin,
                             btScalar *jac,
                             btAlignedObjectArray<btScalar> &scratch_r,
                             btAlignedObjectArray<btVector3> &scratch_v,
                             btAlignedObjectArray<btMatrix3x3> &scratch_m) const;

    //
    // sleeping
    //
	void	setCanSleep(bool canSleep)
	{
		m_canSleep = canSleep;
	}

	bool getCanSleep()const
	{
		return m_canSleep;
	}

    bool isAwake() const { return m_awake; }
    void wakeUp();
    void goToSleep();
    void checkMotionAndSleepIfRequired(btScalar timestep);

	bool hasFixedBase() const
	{
		    return m_fixedBase;
	}

	int getCompanionId() const
	{
		return m_companionId;
	}
	void setCompanionId(int id)
	{
		//printf("for %p setCompanionId(%d)\n",this, id);
		m_companionId = id;
	}

	void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
	{
		m_links.resize(numLinks);
	}

	btScalar getLinearDamping() const
	{
			return m_linearDamping;
	}
	void setLinearDamping( btScalar damp)
	{
		m_linearDamping = damp;
	}
	btScalar getAngularDamping() const
	{
		return m_angularDamping;
	}
	void setAngularDamping( btScalar damp)
	{
		m_angularDamping = damp;
	}

	bool getUseGyroTerm() const
	{
		return m_useGyroTerm;
	}
	void setUseGyroTerm(bool useGyro)
	{
		m_useGyroTerm = useGyro;
	}
	btScalar	getMaxCoordinateVelocity() const
	{
		return m_maxCoordinateVelocity ;
	}
	void	setMaxCoordinateVelocity(btScalar maxVel)
	{
		m_maxCoordinateVelocity = maxVel;
	}

	btScalar	getMaxAppliedImpulse() const
	{
		return m_maxAppliedImpulse;
	}
	void	setMaxAppliedImpulse(btScalar maxImp)
	{
		m_maxAppliedImpulse = maxImp;
	}
	void	setHasSelfCollision(bool hasSelfCollision)
	{
		m_hasSelfCollision = hasSelfCollision;
	}
	bool hasSelfCollision() const
	{
		return m_hasSelfCollision;
	}

	void finalizeMultiDof();

	void useRK4Integration(bool use) { m_useRK4 = use; }
	bool isUsingRK4Integration() const { return m_useRK4; }
	void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
	bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; }

	bool isPosUpdated() const
	{
		return __posUpdated;
	}
	void setPosUpdated(bool updated)
	{
		__posUpdated = updated;
	}

	//internalNeedsJointFeedback is for internal use only
	bool internalNeedsJointFeedback() const
	{
		return m_internalNeedsJointFeedback;
	}
	void	forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);

	void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;

	void	updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);

	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;

	const char*				getBaseName() const
	{
		return m_baseName;
	}
	///memory of setBaseName needs to be manager by user
	void	setBaseName(const char* name)
	{
		m_baseName = name;
	}

	///users can point to their objects, userPointer is not used by Bullet
	void*	getUserPointer() const
	{
		return m_userObjectPointer;
	}

	int	getUserIndex() const
	{
		return m_userIndex;
	}

	int	getUserIndex2() const
	{
		return m_userIndex2;
	}
	///users can point to their objects, userPointer is not used by Bullet
	void	setUserPointer(void* userPointer)
	{
		m_userObjectPointer = userPointer;
	}

	///users can point to their objects, userPointer is not used by Bullet
	void	setUserIndex(int index)
	{
		m_userIndex = index;
	}

	void	setUserIndex2(int index)
	{
		m_userIndex2 = index;
	}

private:
    btMultiBody(const btMultiBody &);  // not implemented
    void operator=(const btMultiBody &);  // not implemented

	void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, btScalar result[6]) const;
	void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;

	void updateLinksDofOffsets()
	{
		int dofOffset = 0, cfgOffset = 0;
		for(int bidx = 0; bidx < m_links.size(); ++bidx)
		{
			m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset;
			dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount;
		}
	}

	void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;

private:

	btMultiBodyLinkCollider* m_baseCollider;//can be NULL
	const char*				m_baseName;//memory needs to be manager by user!

    btVector3 m_basePos;       // position of COM of base (world frame)
    btQuaternion m_baseQuat;   // rotates world points into base frame

    btScalar m_baseMass;         // mass of the base
    btVector3 m_baseInertia;   // inertia of the base (in local frame; diagonal)

    btVector3 m_baseForce;     // external force applied to base. World frame.
    btVector3 m_baseTorque;    // external torque applied to base. World frame.

    btVector3 m_baseConstraintForce;     // external force applied to base. World frame.
    btVector3 m_baseConstraintTorque;    // external torque applied to base. World frame.

    btAlignedObjectArray<btMultibodyLink> m_links;    // array of m_links, excluding the base. index from 0 to num_links-1.

    //
    // realBuf:
    //  offset         size            array
    //   0              6 + num_links   v (base_omega; base_vel; joint_vels)					MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized]
    //   6+num_links    num_links       D
    //
    // vectorBuf:
    //  offset         size         array
    //   0              num_links    h_top
    //   num_links      num_links    h_bottom
    //
    // matrixBuf:
    //  offset         size         array
    //   0              num_links+1  rot_from_parent
    //
   btAlignedObjectArray<btScalar> m_deltaV;
    btAlignedObjectArray<btScalar> m_realBuf;
    btAlignedObjectArray<btVector3> m_vectorBuf;
    btAlignedObjectArray<btMatrix3x3> m_matrixBuf;

	btMatrix3x3 m_cachedInertiaTopLeft;
	btMatrix3x3 m_cachedInertiaTopRight;
	btMatrix3x3 m_cachedInertiaLowerLeft;
	btMatrix3x3 m_cachedInertiaLowerRight;
	bool m_cachedInertiaValid;

    bool m_fixedBase;

    // Sleep parameters.
    bool m_awake;
    bool m_canSleep;
    btScalar m_sleepTimer;

	void* m_userObjectPointer;
	int m_userIndex2;
	int m_userIndex;

	int	m_companionId;
	btScalar	m_linearDamping;
	btScalar	m_angularDamping;
	bool	m_useGyroTerm;
	btScalar	m_maxAppliedImpulse;
	btScalar	m_maxCoordinateVelocity;
	bool		m_hasSelfCollision;

		bool __posUpdated;
		int m_dofCount, m_posVarCnt;
	bool m_useRK4, m_useGlobalVelocities;

	///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
	bool m_internalNeedsJointFeedback;
};

struct btMultiBodyLinkDoubleData
{
	btQuaternionDoubleData	m_zeroRotParentToThis;
	btVector3DoubleData		m_parentComToThisComOffset;
	btVector3DoubleData		m_thisPivotToThisComOffset;
	btVector3DoubleData		m_jointAxisTop[6];
	btVector3DoubleData		m_jointAxisBottom[6];

	btVector3DoubleData		m_linkInertia;   // inertia of the base (in local frame; diagonal)
	double					m_linkMass;
	int						m_parentIndex;
	int						m_jointType;

	int						m_dofCount;
	int						m_posVarCount;
	double					m_jointPos[7];
	double					m_jointVel[6];
	double					m_jointTorque[6];

	double					m_jointDamping;
	double					m_jointFriction;
	double					m_jointLowerLimit;
	double					m_jointUpperLimit;
	double					m_jointMaxForce;
	double					m_jointMaxVelocity;

	char					*m_linkName;
	char					*m_jointName;
	btCollisionObjectDoubleData	*m_linkCollider;
	char					*m_paddingPtr;

};

struct btMultiBodyLinkFloatData
{
	btQuaternionFloatData	m_zeroRotParentToThis;
	btVector3FloatData		m_parentComToThisComOffset;
	btVector3FloatData		m_thisPivotToThisComOffset;
	btVector3FloatData		m_jointAxisTop[6];
	btVector3FloatData		m_jointAxisBottom[6];
	btVector3FloatData	m_linkInertia;   // inertia of the base (in local frame; diagonal)
	int						m_dofCount;
	float				m_linkMass;
	int					m_parentIndex;
	int					m_jointType;

	float					m_jointPos[7];
	float					m_jointVel[6];
	float					m_jointTorque[6];
	int						m_posVarCount;
	float					m_jointDamping;
	float					m_jointFriction;
	float					m_jointLowerLimit;
	float					m_jointUpperLimit;
	float					m_jointMaxForce;
	float					m_jointMaxVelocity;

	char				*m_linkName;
	char				*m_jointName;
	btCollisionObjectFloatData	*m_linkCollider;
	char				*m_paddingPtr;

};

///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct	btMultiBodyDoubleData
{
	btTransformDoubleData m_baseWorldTransform;
	btVector3DoubleData m_baseInertia;   // inertia of the base (in local frame; diagonal)
	double	m_baseMass;

	char	*m_baseName;
	btMultiBodyLinkDoubleData	*m_links;
	btCollisionObjectDoubleData	*m_baseCollider;
	char	*m_paddingPtr;
	int		m_numLinks;
	char	m_padding[4];
};

///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct	btMultiBodyFloatData
{
	char	*m_baseName;
	btMultiBodyLinkFloatData	*m_links;
	btCollisionObjectFloatData	*m_baseCollider;
	btTransformFloatData m_baseWorldTransform;
	btVector3FloatData m_baseInertia;   // inertia of the base (in local frame; diagonal)

	float	m_baseMass;
	int		m_numLinks;
};

#endif


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_MULTIBODY_DYNAMICS_WORLD_H
#define BT_MULTIBODY_DYNAMICS_WORLD_H

#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"

#define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY

class btMultiBody;
class btMultiBodyConstraint;
class btMultiBodyConstraintSolver;
struct MultiBodyInplaceSolverIslandCallback;

///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet
///This implementation is still preliminary/experimental.
class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld
{
protected:
	btAlignedObjectArray<btMultiBody*> m_multiBodies;
	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
	btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
	btMultiBodyConstraintSolver*	m_multiBodyConstraintSolver;
	MultiBodyInplaceSolverIslandCallback*	m_solverMultiBodyIslandCallback;

	//cached data to avoid memory allocations
	btAlignedObjectArray<btQuaternion> m_scratch_world_to_local;
	btAlignedObjectArray<btVector3> m_scratch_local_origin;
	btAlignedObjectArray<btQuaternion> m_scratch_world_to_local1;
	btAlignedObjectArray<btVector3> m_scratch_local_origin1;
	btAlignedObjectArray<btScalar> m_scratch_r;
	btAlignedObjectArray<btVector3> m_scratch_v;
	btAlignedObjectArray<btMatrix3x3> m_scratch_m;

	virtual void	calculateSimulationIslands();
	virtual void	updateActivationState(btScalar timeStep);
	virtual void	solveConstraints(btContactSolverInfo& solverInfo);

	virtual void	serializeMultiBodies(btSerializer* serializer);

public:

	btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);

	virtual ~btMultiBodyDynamicsWorld ();

	virtual void	addMultiBody(btMultiBody* body, int group= btBroadphaseProxy::DefaultFilter, int mask=btBroadphaseProxy::AllFilter);

	virtual void	removeMultiBody(btMultiBody* body);

	virtual int		getNumMultibodies() const
	{
		return m_multiBodies.size();
	}

	btMultiBody*	getMultiBody(int mbIndex)
	{
		return m_multiBodies[mbIndex];
	}

	const btMultiBody*	getMultiBody(int mbIndex) const
	{
		return m_multiBodies[mbIndex];
	}

	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);

	virtual int     getNumMultiBodyConstraints() const
	{
        return m_multiBodyConstraints.size();
	}

	virtual btMultiBodyConstraint*	getMultiBodyConstraint( int constraintIndex)
	{
        return m_multiBodyConstraints[constraintIndex];
	}

	virtual const btMultiBodyConstraint*	getMultiBodyConstraint( int constraintIndex) const
	{
        return m_multiBodyConstraints[constraintIndex];
	}

	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);

	virtual void	integrateTransforms(btScalar timeStep);

	virtual void	debugDrawWorld();

	virtual void	debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);

	void	forwardKinematics();
	virtual void clearForces();
	virtual void clearMultiBodyConstraintForces();
	virtual void clearMultiBodyForces();
	virtual void applyGravity();

	virtual	void	serialize(btSerializer* serializer);

};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_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_JOINT_MOTOR_H
#define BT_MULTIBODY_JOINT_MOTOR_H

#include "btMultiBodyConstraint.h"
struct btSolverInfo;

class btMultiBodyJointMotor : public btMultiBodyConstraint
{
protected:

	btScalar	m_desiredVelocity;
	btScalar	m_desiredPosition;
	btScalar    m_kd;
	btScalar    m_kp;
	btScalar	m_erp;
	btScalar	m_rhsClamp;//maximum error

public:

	btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
	btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
	virtual ~btMultiBodyJointMotor();
    virtual void finalizeMultiDof();

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

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

    virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
    {
        m_desiredVelocity = velTarget;
        m_kd = kd;
    }

    virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
    {
        m_desiredPosition = posTarget;
        m_kp = kp;
    }

	virtual void setErp(btScalar erp)
	{
		m_erp = erp;
	}
	virtual btScalar getErp() const
	{
		return m_erp;
	}
	virtual void setRhsClamp(btScalar rhsClamp)
	{
		m_rhsClamp = rhsClamp;
	}
	virtual void debugDraw(class btIDebugDraw* drawer)
	{
		//todo(erwincoumans)
	}
};

#endif //BT_MULTIBODY_JOINT_MOTOR_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_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H

#include "btMultiBodyConstraint.h"
struct btSolverInfo;

class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
{
protected:

	btScalar	m_lowerBound;
	btScalar	m_upperBound;
public:

	btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
	virtual ~btMultiBodyJointLimitConstraint();

	virtual void finalizeMultiDof();

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

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

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

};

#endif //BT_MULTIBODY_JOINT_LIMIT_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_POINT2POINT_H
#define BT_MULTIBODY_POINT2POINT_H

#include "btMultiBodyConstraint.h"

//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST

ATTRIBUTE_ALIGNED16(class) btMultiBodyPoint2Point : public btMultiBodyConstraint
{
protected:

	btRigidBody*	m_rigidBodyA;
	btRigidBody*	m_rigidBodyB;
	btVector3		m_pivotInA;
	btVector3		m_pivotInB;

public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

	btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
	btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);

	virtual ~btMultiBodyPoint2Point();

	virtual void finalizeMultiDof();

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

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

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

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

	virtual void debugDraw(class btIDebugDraw* drawer);

};

#endif //BT_MULTIBODY_POINT2POINT_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_MULTIBODY_SOLVER_CONSTRAINT_H
#define BT_MULTIBODY_SOLVER_CONSTRAINT_H

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

class btMultiBody;
class btMultiBodyConstraint;
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"

///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint
{
	BT_DECLARE_ALIGNED_ALLOCATOR();

	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_orgConstraint(0), m_orgDofIndex(-1)
	{}

	int				m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
	int				m_jacAindex;
	int				m_deltaVelBindex;
	int				m_jacBindex;

	btVector3		m_relpos1CrossNormal;
	btVector3		m_contactNormal1;
	btVector3		m_relpos2CrossNormal;
	btVector3		m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always

	btVector3		m_angularComponentA;
	btVector3		m_angularComponentB;

	mutable btSimdScalar	m_appliedPushImpulse;
	mutable btSimdScalar	m_appliedImpulse;

	btScalar	m_friction;
	btScalar	m_jacDiagABInv;
	btScalar		m_rhs;
	btScalar		m_cfm;

    btScalar		m_lowerLimit;
	btScalar		m_upperLimit;
	btScalar		m_rhsPenetration;
    union
	{
		void*		m_originalContactPoint;
		btScalar	m_unusedPadding4;
	};

	int	m_overrideNumSolverIterations;
    int			m_frictionIndex;

	int m_solverBodyIdA;
	btMultiBody* m_multiBodyA;
	int			m_linkA;

	int m_solverBodyIdB;
	btMultiBody* m_multiBodyB;
	int			m_linkB;

	//for writing back applied impulses
	btMultiBodyConstraint*	m_orgConstraint;
	int m_orgDofIndex;

	enum		btSolverConstraintType
	{
		BT_SOLVER_CONTACT_1D = 0,
		BT_SOLVER_FRICTION_1D
	};
};

typedef btAlignedObjectArray<btMultiBodySolverConstraint>	btMultiBodyConstraintArray;

#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_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_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
#define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H

class btIDebugDraw;
class btPersistentManifold;
class btDispatcher;
class btCollisionObject;
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h"
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"

typedef btSimdScalar (*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);

/// The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the
/// Projected Gauss Seidel (iterative LCP) method.
ATTRIBUTE_ALIGNED16(class)
btSequentialImpulseConstraintSolver : public btConstraintSolver
{
protected:
	btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
	btConstraintArray                  m_tmpSolverContactConstraintPool;
	btConstraintArray                  m_tmpSolverNonContactConstraintPool;
	btConstraintArray                  m_tmpSolverContactFrictionConstraintPool;
	btConstraintArray                  m_tmpSolverContactRollingFrictionConstraintPool;

	btAlignedObjectArray<int>                                  m_orderTmpConstraintPool;
	btAlignedObjectArray<int>                                  m_orderNonContactConstraintPool;
	btAlignedObjectArray<int>                                  m_orderFrictionConstraintPool;
	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
	int                                                        m_maxOverrideNumSolverIterations;
	int                                                        m_fixedBodyId;

	/// When running solvers on multiple threads, a race condition exists for
	/// Kinematic objects that participate in more than one solver. The
	/// getOrInitSolverBody() function writes the companionId of each body
	/// (storing the index of the solver body for the current solver). For normal
	/// dynamic bodies it isn't an issue because they can only be in one island
	/// (and therefore one thread) at a time. But kinematic bodies can be in
	/// multiple islands at once. To avoid this race condition, this solver does
	/// not write the companionId, instead it stores the solver body index in this
	/// solver-local table, indexed by the uniqueId of the body. only used for
	/// multithreading
	btAlignedObjectArray<int> m_kinematicBodyUniqueIdToSolverBodyTable;

	btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
	btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
	btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;

	/// used to check if SOLVER_SIMD flag has been changed
	int m_cachedSolverMode;

	void setupSolverFunctions(bool useSimd);
	btScalar m_leastSquaresResidual;

	void setupFrictionConstraint(
	    btSolverConstraint & solverConstraint,
	    const btVector3&           normalAxis,
	    int                        solverBodyIdA,
	    int                        solverBodyIdB,
	    btManifoldPoint&           cp,
	    const btVector3&           rel_pos1,
	    const btVector3&           rel_pos2,
	    btCollisionObject*         colObj0,
	    btCollisionObject*         colObj1,
	    btScalar                   relaxation,
	    const btContactSolverInfo& infoGlobal,
	    btScalar                   desiredVelocity = 0.,
	    btScalar                   cfmSlip         = 0.);

	void setupTorsionalFrictionConstraint(
	    btSolverConstraint & solverConstraint,
	    const btVector3&   normalAxis,
	    int                solverBodyIdA,
	    int                solverBodyIdB,
	    btManifoldPoint&   cp,
	    btScalar           combinedTorsionalFriction,
	    const btVector3&   rel_pos1,
	    const btVector3&   rel_pos2,
	    btCollisionObject* colObj0,
	    btCollisionObject* colObj1,
	    btScalar           relaxation,
	    btScalar           desiredVelocity = 0.,
	    btScalar           cfmSlip         = 0.);

	btSolverConstraint& addFrictionConstraint(
	    const btVector3&           normalAxis,
	    int                        solverBodyIdA,
	    int                        solverBodyIdB,
	    int                        frictionIndex,
	    btManifoldPoint&           cp,
	    const btVector3&           rel_pos1,
	    const btVector3&           rel_pos2,
	    btCollisionObject*         colObj0,
	    btCollisionObject*         colObj1,
	    btScalar                   relaxation,
	    const btContactSolverInfo& infoGlobal,
	    btScalar                   desiredVelocity = 0.,
	    btScalar                   cfmSlip         = 0.);

	btSolverConstraint& addTorsionalFrictionConstraint(
	    const btVector3&   normalAxis,
	    int                solverBodyIdA,
	    int                solverBodyIdB,
	    int                frictionIndex,
	    btManifoldPoint&   cp,
	    btScalar           torsionalFriction,
	    const btVector3&   rel_pos1,
	    const btVector3&   rel_pos2,
	    btCollisionObject* colObj0,
	    btCollisionObject* colObj1,
	    btScalar           relaxation,
	    btScalar           desiredVelocity = 0,
	    btScalar           cfmSlip         = 0.f);

	void setupContactConstraint(
	    btSolverConstraint&        solverConstraint,
	    int                        solverBodyIdA,
	    int                        solverBodyIdB,
	    btManifoldPoint&           cp,
	    const btContactSolverInfo& infoGlobal,
	    btScalar&                  relaxation,
	    const btVector3&           rel_pos1,
	    const btVector3&           rel_pos2);

	static void applyAnisotropicFriction(
	    btCollisionObject* colObj,
	    btVector3&         frictionDirection,
	    int                frictionMode);

	void setFrictionConstraintImpulse(
	    btSolverConstraint&        solverConstraint,
	    int                        solverBodyIdA,
	    int                        solverBodyIdB,
	    btManifoldPoint&           cp,
	    const btContactSolverInfo& infoGlobal);

	/// m_btSeed2 is used for re-arranging the constraint rows. improves
	/// convergence/quality of friction
	unsigned long m_btSeed2;

	btScalar     restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold);
	virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
	void         convertContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal);

	btSimdScalar resolveSplitPenetrationSIMD(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
	{
		return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
	}

	btSimdScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
	{
		return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
	}

	//internal method
	int  getOrInitSolverBody(btCollisionObject & body, btScalar timeStep);
	void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);

	btSimdScalar resolveSingleConstraintRowGeneric(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& contactConstraint);
	btSimdScalar resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& contactConstraint);
	btSimdScalar resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& contactConstraint);
	btSimdScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& contactConstraint);

	btSimdScalar resolveSplitPenetrationImpulse(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
	{
		return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
	}

protected:
	virtual void solveGroupCacheFriendlySplitImpulseIterations(
	    btCollisionObject**        bodies,
	    int                        numBodies,
	    btPersistentManifold**     manifoldPtr,
	    int                        numManifolds,
	    btTypedConstraint**        constraints,
	    int                        numConstraints,
	    const btContactSolverInfo& infoGlobal,
	    btIDebugDraw*              debugDrawer);

	virtual btScalar solveGroupCacheFriendlyFinish(
	    btCollisionObject**        bodies,
	    int                        numBodies,
	    const btContactSolverInfo& infoGlobal);

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

	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);

public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	btSequentialImpulseConstraintSolver();
	virtual ~btSequentialImpulseConstraintSolver();

	virtual btScalar solveGroup(
	    btCollisionObject**        bodies,
	    int                        numBodies,
	    btPersistentManifold**     manifold,
	    int                        numManifolds,
	    btTypedConstraint**        constraints,
	    int                        numConstraints,
	    const btContactSolverInfo& info,
	    btIDebugDraw*              debugDrawer,
	    btDispatcher*              dispatcher);

	/// clear internal cached data and reset random seed
	virtual void reset();

	unsigned long                  btRand2();
	int                            btRandInt2(int n);
	void                           setRandSeed(unsigned long seed) { m_btSeed2 = seed; }
	unsigned long                  getRandSeed() const { return m_btSeed2; }
	virtual btConstraintSolverType getSolverType() const { return BT_SEQUENTIAL_IMPULSE_SOLVER; }
	btSingleConstraintRowSolver    getActiveConstraintRowSolverGeneric() { return m_resolveSingleConstraintRowGeneric; }
	void                           setConstraintRowSolverGeneric(btSingleConstraintRowSolver rowSolver) { m_resolveSingleConstraintRowGeneric = rowSolver; }
	btSingleConstraintRowSolver    getActiveConstraintRowSolverLowerLimit() { return m_resolveSingleConstraintRowLowerLimit; }
	void                           setConstraintRowSolverLowerLimit(btSingleConstraintRowSolver rowSolver) { m_resolveSingleConstraintRowLowerLimit = rowSolver; }

	// Various implementations of solving a single constraint row using a generic
	// equality constraint, using scalar reference, SSE2 or SSE4
	btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
	btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
	btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();

	// Various implementations of solving a single constraint row using an
	// inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4
	btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
	btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
	btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
};

#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2012 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.
*/

#ifndef BT_GEAR_CONSTRAINT_H
#define BT_GEAR_CONSTRAINT_H

#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"

#ifdef BT_USE_DOUBLE_PRECISION
#define btGearConstraintData	btGearConstraintDoubleData
#define btGearConstraintDataName	"btGearConstraintDoubleData"
#else
#define btGearConstraintData	btGearConstraintFloatData
#define btGearConstraintDataName	"btGearConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION

///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio.
///See Bullet/Demos/ConstraintDemo for an example use.
class btGearConstraint : public btTypedConstraint
{
protected:
	btVector3	m_axisInA;
	btVector3	m_axisInB;
	bool		m_useFrameA;
	btScalar	m_ratio;

public:
	btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f);
	virtual ~btGearConstraint ();

	///internal method used by the constraint solver, don't use them directly
	virtual void getInfo1 (btConstraintInfo1* info);

	///internal method used by the constraint solver, don't use them directly
	virtual void getInfo2 (btConstraintInfo2* info);

	void setAxisA(btVector3& axisA)
	{
		m_axisInA = axisA;
	}
	void setAxisB(btVector3& axisB)
	{
		m_axisInB = axisB;
	}
	void setRatio(btScalar ratio)
	{
		m_ratio = ratio;
	}
	const btVector3& getAxisA() const
	{
		return m_axisInA;
	}
	const btVector3& getAxisB() const
	{
		return m_axisInB;
	}
	btScalar getRatio() const
	{
		return m_ratio;
	}

	virtual	void	setParam(int num, btScalar value, int axis = -1)
	{
		(void) num;
		(void) value;
		(void) axis;
		btAssert(0);
	}

	///return the local value of parameter
	virtual	btScalar getParam(int num, int axis = -1) const
	{
		(void) num;
		(void) axis;
		btAssert(0);
		return 0.f;
	}

	virtual	int	calculateSerializeBufferSize() const;

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

///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btGearConstraintFloatData
{
	btTypedConstraintFloatData	m_typeConstraintData;

	btVector3FloatData			m_axisInA;
	btVector3FloatData			m_axisInB;

	float							m_ratio;
	char							m_padding[4];
};

struct btGearConstraintDoubleData
{
	btTypedConstraintDoubleData	m_typeConstraintData;

	btVector3DoubleData			m_axisInA;
	btVector3DoubleData			m_axisInB;

	double						m_ratio;
};

SIMD_FORCE_INLINE	int	btGearConstraint::calculateSerializeBufferSize() const
{
	return sizeof(btGearConstraintData);
}

	///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE	const char*	btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
	btGearConstraintData* gear = (btGearConstraintData*)dataBuffer;
	btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer);

	m_axisInA.serialize( gear->m_axisInA );
	m_axisInB.serialize( gear->m_axisInB );

	gear->m_ratio = m_ratio;

	// Fill padding with zeros to appease msan.
#ifndef BT_USE_DOUBLE_PRECISION
	gear->m_padding[0] = 0;
	gear->m_padding[1] = 0;
	gear->m_padding[2] = 0;
	gear->m_padding[3] = 0;
#endif

	return btGearConstraintDataName;
}

#endif //BT_GEAR_CONSTRAINT_H


text:
/*
Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.

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_UNIVERSAL_CONSTRAINT_H
#define BT_UNIVERSAL_CONSTRAINT_H

#include "LinearMath/btVector3.h"
#include "btTypedConstraint.h"
#include "btGeneric6DofConstraint.h"

/// Constraint similar to ODE Universal Joint
/// has 2 rotatioonal degrees of freedom, similar to Euler rotations around Z (axis 1)
/// and Y (axis 2)
/// Description from ODE manual :
/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular.
/// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal."

ATTRIBUTE_ALIGNED16(class) btUniversalConstraint : public btGeneric6DofConstraint
{
protected:
	btVector3	m_anchor;
	btVector3	m_axis1;
	btVector3	m_axis2;
public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

	// constructor
	// anchor, axis1 and axis2 are in world coordinate system
	// axis1 must be orthogonal to axis2
    btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2);
	// access
	const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
	const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
	const btVector3& getAxis1() { return m_axis1; }
	const btVector3& getAxis2() { return m_axis2; }
	btScalar getAngle1() { return getAngle(2); }
	btScalar getAngle2() { return getAngle(1); }
	// limits
	void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); }
	void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); }

	void setAxis( const btVector3& axis1, const btVector3& axis2);
};

#endif // BT_UNIVERSAL_CONSTRAINT_H