virtualx-engine/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp

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

#include "btMultiBody.h"
#include "btMultiBodyLink.h"
#include "btMultiBodyLinkCollider.h"
#include "btMultiBodyJointFeedback.h"
#include "LinearMath/btTransformUtil.h"
#include "LinearMath/btSerializer.h"
//#include "Bullet3Common/b3Logging.h"
// #define INCLUDE_GYRO_TERM


namespace
{
const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
}  // namespace

void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
	const btVector3 &displacement,       // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
	const btVector3 &top_in,             // top part of input vector
	const btVector3 &bottom_in,          // bottom part of input vector
	btVector3 &top_out,                  // top part of output vector
	btVector3 &bottom_out)               // bottom part of output vector
{
	top_out = rotation_matrix * top_in;
	bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
}

namespace
{


#if 0
    void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
                                 const btVector3 &displacement,
                                 const btVector3 &top_in,
                                 const btVector3 &bottom_in,
                                 btVector3 &top_out,
                                 btVector3 &bottom_out)
    {
        top_out = rotation_matrix.transpose() * top_in;
        bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));		
    }

    btScalar SpatialDotProduct(const btVector3 &a_top,
                            const btVector3 &a_bottom,
                            const btVector3 &b_top,
                            const btVector3 &b_bottom)
    {
        return a_bottom.dot(b_top) + a_top.dot(b_bottom);
    }

	void SpatialCrossProduct(const btVector3 &a_top,
                            const btVector3 &a_bottom,
                            const btVector3 &b_top,
                            const btVector3 &b_bottom,
							btVector3 &top_out,
							btVector3 &bottom_out)
	{
		top_out = a_top.cross(b_top);
		bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
	}
#endif

}  // namespace

//
// Implementation of class btMultiBody
//

btMultiBody::btMultiBody(int n_links,
						 btScalar mass,
						 const btVector3 &inertia,
						 bool fixedBase,
						 bool canSleep,
						 bool /*deprecatedUseMultiDof*/)
	: m_baseCollider(0),
	  m_baseName(0),
	  m_basePos(0, 0, 0),
	  m_baseQuat(0, 0, 0, 1),
	  m_baseMass(mass),
	  m_baseInertia(inertia),

	  m_fixedBase(fixedBase),
	  m_awake(true),
	  m_canSleep(canSleep),
	  m_canWakeup(true),
	  m_sleepTimer(0),
	  m_userObjectPointer(0),
	  m_userIndex2(-1),
	  m_userIndex(-1),
	  m_companionId(-1),
	  m_linearDamping(0.04f),
	  m_angularDamping(0.04f),
	  m_useGyroTerm(true),
	  m_maxAppliedImpulse(1000.f),
	  m_maxCoordinateVelocity(100.f),
	  m_hasSelfCollision(true),
	  __posUpdated(false),
	  m_dofCount(0),
	  m_posVarCnt(0),
	  m_useRK4(false),
	  m_useGlobalVelocities(false),
	  m_internalNeedsJointFeedback(false)
{
	m_cachedInertiaTopLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
	m_cachedInertiaTopRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
	m_cachedInertiaLowerLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
	m_cachedInertiaLowerRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
	m_cachedInertiaValid = false;

	m_links.resize(n_links);
	m_matrixBuf.resize(n_links + 1);

	m_baseForce.setValue(0, 0, 0);
	m_baseTorque.setValue(0, 0, 0);

	clearConstraintForces();
	clearForcesAndTorques();
}

btMultiBody::~btMultiBody()
{
}

void btMultiBody::setupFixed(int i,
							 btScalar mass,
							 const btVector3 &inertia,
							 int parent,
							 const btQuaternion &rotParentToThis,
							 const btVector3 &parentComToThisPivotOffset,
							 const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/)
{
	m_links[i].m_mass = mass;
	m_links[i].m_inertiaLocal = inertia;
	m_links[i].m_parent = parent;
	m_links[i].setAxisTop(0, 0., 0., 0.);
	m_links[i].setAxisBottom(0, btVector3(0, 0, 0));
	m_links[i].m_zeroRotParentToThis = rotParentToThis;
	m_links[i].m_dVector = thisPivotToThisComOffset;
	m_links[i].m_eVector = parentComToThisPivotOffset;

	m_links[i].m_jointType = btMultibodyLink::eFixed;
	m_links[i].m_dofCount = 0;
	m_links[i].m_posVarCount = 0;

	m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;

	m_links[i].updateCacheMultiDof();

	updateLinksDofOffsets();
}

void btMultiBody::setupPrismatic(int i,
								 btScalar mass,
								 const btVector3 &inertia,
								 int parent,
								 const btQuaternion &rotParentToThis,
								 const btVector3 &jointAxis,
								 const btVector3 &parentComToThisPivotOffset,
								 const btVector3 &thisPivotToThisComOffset,
								 bool disableParentCollision)
{
	m_dofCount += 1;
	m_posVarCnt += 1;

	m_links[i].m_mass = mass;
	m_links[i].m_inertiaLocal = inertia;
	m_links[i].m_parent = parent;
	m_links[i].m_zeroRotParentToThis = rotParentToThis;
	m_links[i].setAxisTop(0, 0., 0., 0.);
	m_links[i].setAxisBottom(0, jointAxis);
	m_links[i].m_eVector = parentComToThisPivotOffset;
	m_links[i].m_dVector = thisPivotToThisComOffset;
	m_links[i].m_cachedRotParentToThis = rotParentToThis;

	m_links[i].m_jointType = btMultibodyLink::ePrismatic;
	m_links[i].m_dofCount = 1;
	m_links[i].m_posVarCount = 1;
	m_links[i].m_jointPos[0] = 0.f;
	m_links[i].m_jointTorque[0] = 0.f;

	if (disableParentCollision)
		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
	//

	m_links[i].updateCacheMultiDof();

	updateLinksDofOffsets();
}

void btMultiBody::setupRevolute(int i,
								btScalar mass,
								const btVector3 &inertia,
								int parent,
								const btQuaternion &rotParentToThis,
								const btVector3 &jointAxis,
								const btVector3 &parentComToThisPivotOffset,
								const btVector3 &thisPivotToThisComOffset,
								bool disableParentCollision)
{
	m_dofCount += 1;
	m_posVarCnt += 1;

	m_links[i].m_mass = mass;
	m_links[i].m_inertiaLocal = inertia;
	m_links[i].m_parent = parent;
	m_links[i].m_zeroRotParentToThis = rotParentToThis;
	m_links[i].setAxisTop(0, jointAxis);
	m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
	m_links[i].m_dVector = thisPivotToThisComOffset;
	m_links[i].m_eVector = parentComToThisPivotOffset;

	m_links[i].m_jointType = btMultibodyLink::eRevolute;
	m_links[i].m_dofCount = 1;
	m_links[i].m_posVarCount = 1;
	m_links[i].m_jointPos[0] = 0.f;
	m_links[i].m_jointTorque[0] = 0.f;

	if (disableParentCollision)
		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
	//
	m_links[i].updateCacheMultiDof();
	//
	updateLinksDofOffsets();
}

void btMultiBody::setupSpherical(int i,
								 btScalar mass,
								 const btVector3 &inertia,
								 int parent,
								 const btQuaternion &rotParentToThis,
								 const btVector3 &parentComToThisPivotOffset,
								 const btVector3 &thisPivotToThisComOffset,
								 bool disableParentCollision)
{
	m_dofCount += 3;
	m_posVarCnt += 4;

	m_links[i].m_mass = mass;
	m_links[i].m_inertiaLocal = inertia;
	m_links[i].m_parent = parent;
	m_links[i].m_zeroRotParentToThis = rotParentToThis;
	m_links[i].m_dVector = thisPivotToThisComOffset;
	m_links[i].m_eVector = parentComToThisPivotOffset;

	m_links[i].m_jointType = btMultibodyLink::eSpherical;
	m_links[i].m_dofCount = 3;
	m_links[i].m_posVarCount = 4;
	m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
	m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
	m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
	m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
	m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
	m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
	m_links[i].m_jointPos[3] = 1.f;
	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;

	if (disableParentCollision)
		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
	//
	m_links[i].updateCacheMultiDof();
	//
	updateLinksDofOffsets();
}

void btMultiBody::setupPlanar(int i,
							  btScalar mass,
							  const btVector3 &inertia,
							  int parent,
							  const btQuaternion &rotParentToThis,
							  const btVector3 &rotationAxis,
							  const btVector3 &parentComToThisComOffset,
							  bool disableParentCollision)
{
	m_dofCount += 3;
	m_posVarCnt += 3;

	m_links[i].m_mass = mass;
	m_links[i].m_inertiaLocal = inertia;
	m_links[i].m_parent = parent;
	m_links[i].m_zeroRotParentToThis = rotParentToThis;
	m_links[i].m_dVector.setZero();
	m_links[i].m_eVector = parentComToThisComOffset;

	//
	btVector3 vecNonParallelToRotAxis(1, 0, 0);
	if (rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
		vecNonParallelToRotAxis.setValue(0, 1, 0);
	//

	m_links[i].m_jointType = btMultibodyLink::ePlanar;
	m_links[i].m_dofCount = 3;
	m_links[i].m_posVarCount = 3;
	btVector3 n = rotationAxis.normalized();
	m_links[i].setAxisTop(0, n[0], n[1], n[2]);
	m_links[i].setAxisTop(1, 0, 0, 0);
	m_links[i].setAxisTop(2, 0, 0, 0);
	m_links[i].setAxisBottom(0, 0, 0, 0);
	btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
	m_links[i].setAxisBottom(1, cr[0], cr[1], cr[2]);
	cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
	m_links[i].setAxisBottom(2, cr[0], cr[1], cr[2]);
	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;

	if (disableParentCollision)
		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
	//
	m_links[i].updateCacheMultiDof();
	//
	updateLinksDofOffsets();

	m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).normalized());
	m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).normalized());
}

void btMultiBody::finalizeMultiDof()
{
	m_deltaV.resize(0);
	m_deltaV.resize(6 + m_dofCount);
	m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount);  //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
	m_vectorBuf.resize(2 * m_dofCount);                                           //two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
	m_matrixBuf.resize(m_links.size() + 1);
	for (int i = 0; i < m_vectorBuf.size(); i++)
	{
		m_vectorBuf[i].setValue(0, 0, 0);
	}
	updateLinksDofOffsets();
}

int btMultiBody::getParent(int link_num) const
{
	return m_links[link_num].m_parent;
}

btScalar btMultiBody::getLinkMass(int i) const
{
	return m_links[i].m_mass;
}

const btVector3 &btMultiBody::getLinkInertia(int i) const
{
	return m_links[i].m_inertiaLocal;
}

btScalar btMultiBody::getJointPos(int i) const
{
	return m_links[i].m_jointPos[0];
}

btScalar btMultiBody::getJointVel(int i) const
{
	return m_realBuf[6 + m_links[i].m_dofOffset];
}

btScalar *btMultiBody::getJointPosMultiDof(int i)
{
	return &m_links[i].m_jointPos[0];
}

btScalar *btMultiBody::getJointVelMultiDof(int i)
{
	return &m_realBuf[6 + m_links[i].m_dofOffset];
}

const btScalar *btMultiBody::getJointPosMultiDof(int i) const
{
	return &m_links[i].m_jointPos[0];
}

const btScalar *btMultiBody::getJointVelMultiDof(int i) const
{
	return &m_realBuf[6 + m_links[i].m_dofOffset];
}

void btMultiBody::setJointPos(int i, btScalar q)
{
	m_links[i].m_jointPos[0] = q;
	m_links[i].updateCacheMultiDof();
}


void btMultiBody::setJointPosMultiDof(int i, const double *q)
{
	for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
		m_links[i].m_jointPos[pos] = (btScalar)q[pos];

	m_links[i].updateCacheMultiDof();
}

void btMultiBody::setJointPosMultiDof(int i, const float *q)
{
	for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
		m_links[i].m_jointPos[pos] = (btScalar)q[pos];

	m_links[i].updateCacheMultiDof();
}



void btMultiBody::setJointVel(int i, btScalar qdot)
{
	m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
}

void btMultiBody::setJointVelMultiDof(int i, const double *qdot)
{
	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
		m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
}

void btMultiBody::setJointVelMultiDof(int i, const float* qdot)
{
	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
		m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
}

const btVector3 &btMultiBody::getRVector(int i) const
{
	return m_links[i].m_cachedRVector;
}

const btQuaternion &btMultiBody::getParentToLocalRot(int i) const
{
	return m_links[i].m_cachedRotParentToThis;
}

btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
{
	btAssert(i >= -1);
	btAssert(i < m_links.size());
	if ((i < -1) || (i >= m_links.size()))
	{
		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
	}

	btVector3 result = local_pos;
	while (i != -1)
	{
		// 'result' is in frame i. transform it to frame parent(i)
		result += getRVector(i);
		result = quatRotate(getParentToLocalRot(i).inverse(), result);
		i = getParent(i);
	}

	// 'result' is now in the base frame. transform it to world frame
	result = quatRotate(getWorldToBaseRot().inverse(), result);
	result += getBasePos();

	return result;
}

btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
{
	btAssert(i >= -1);
	btAssert(i < m_links.size());
	if ((i < -1) || (i >= m_links.size()))
	{
		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
	}

	if (i == -1)
	{
		// world to base
		return quatRotate(getWorldToBaseRot(), (world_pos - getBasePos()));
	}
	else
	{
		// find position in parent frame, then transform to current frame
		return quatRotate(getParentToLocalRot(i), worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
	}
}

btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
{
	btAssert(i >= -1);
	btAssert(i < m_links.size());
	if ((i < -1) || (i >= m_links.size()))
	{
		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
	}

	btVector3 result = local_dir;
	while (i != -1)
	{
		result = quatRotate(getParentToLocalRot(i).inverse(), result);
		i = getParent(i);
	}
	result = quatRotate(getWorldToBaseRot().inverse(), result);
	return result;
}

btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
{
	btAssert(i >= -1);
	btAssert(i < m_links.size());
	if ((i < -1) || (i >= m_links.size()))
	{
		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
	}

	if (i == -1)
	{
		return quatRotate(getWorldToBaseRot(), world_dir);
	}
	else
	{
		return quatRotate(getParentToLocalRot(i), worldDirToLocal(getParent(i), world_dir));
	}
}

btMatrix3x3 btMultiBody::localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
{
	btMatrix3x3 result = local_frame;
	btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0));
	btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1));
	btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2));
	result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
	return result;
}

void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
{
	int num_links = getNumLinks();
	// Calculates the velocities of each link (and the base) in its local frame
	const btQuaternion& base_rot = getWorldToBaseRot();
	omega[0] = quatRotate(base_rot, getBaseOmega());
	vel[0] = quatRotate(base_rot, getBaseVel());

	for (int i = 0; i < num_links; ++i)
	{
		const btMultibodyLink& link = getLink(i);
		const int parent = link.m_parent;

		// transform parent vel into this frame, store in omega[i+1], vel[i+1]
		spatialTransform(btMatrix3x3(link.m_cachedRotParentToThis), link.m_cachedRVector,
			omega[parent + 1], vel[parent + 1],
			omega[i + 1], vel[i + 1]);

		// now add qidot * shat_i
		const btScalar* jointVel = getJointVelMultiDof(i);
		for (int dof = 0; dof < link.m_dofCount; ++dof)
		{
			omega[i + 1] += jointVel[dof] * link.getAxisTop(dof);
			vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof);
		}
	}
}

btScalar btMultiBody::getKineticEnergy() const
{
	int num_links = getNumLinks();
	// TODO: would be better not to allocate memory here
	btAlignedObjectArray<btVector3> omega;
	omega.resize(num_links + 1);
	btAlignedObjectArray<btVector3> vel;
	vel.resize(num_links + 1);
	compTreeLinkVelocities(&omega[0], &vel[0]);

	// we will do the factor of 0.5 at the end
	btScalar result = m_baseMass * vel[0].dot(vel[0]);
	result += omega[0].dot(m_baseInertia * omega[0]);

	for (int i = 0; i < num_links; ++i)
	{
		result += m_links[i].m_mass * vel[i + 1].dot(vel[i + 1]);
		result += omega[i + 1].dot(m_links[i].m_inertiaLocal * omega[i + 1]);
	}

	return 0.5f * result;
}

btVector3 btMultiBody::getAngularMomentum() const
{
	int num_links = getNumLinks();
	// TODO: would be better not to allocate memory here
	btAlignedObjectArray<btVector3> omega;
	omega.resize(num_links + 1);
	btAlignedObjectArray<btVector3> vel;
	vel.resize(num_links + 1);
	btAlignedObjectArray<btQuaternion> rot_from_world;
	rot_from_world.resize(num_links + 1);
	compTreeLinkVelocities(&omega[0], &vel[0]);

	rot_from_world[0] = m_baseQuat;
	btVector3 result = quatRotate(rot_from_world[0].inverse(), (m_baseInertia * omega[0]));

	for (int i = 0; i < num_links; ++i)
	{
		rot_from_world[i + 1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent + 1];
		result += (quatRotate(rot_from_world[i + 1].inverse(), (m_links[i].m_inertiaLocal * omega[i + 1])));
	}

	return result;
}

void btMultiBody::clearConstraintForces()
{
	m_baseConstraintForce.setValue(0, 0, 0);
	m_baseConstraintTorque.setValue(0, 0, 0);

	for (int i = 0; i < getNumLinks(); ++i)
	{
		m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
		m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
	}
}
void btMultiBody::clearForcesAndTorques()
{
	m_baseForce.setValue(0, 0, 0);
	m_baseTorque.setValue(0, 0, 0);

	for (int i = 0; i < getNumLinks(); ++i)
	{
		m_links[i].m_appliedForce.setValue(0, 0, 0);
		m_links[i].m_appliedTorque.setValue(0, 0, 0);
		m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f;
	}
}

void btMultiBody::clearVelocities()
{
	for (int i = 0; i < 6 + getNumDofs(); ++i)
	{
		m_realBuf[i] = 0.f;
	}
}
void btMultiBody::addLinkForce(int i, const btVector3 &f)
{
	m_links[i].m_appliedForce += f;
}

void btMultiBody::addLinkTorque(int i, const btVector3 &t)
{
	m_links[i].m_appliedTorque += t;
}

void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f)
{
	m_links[i].m_appliedConstraintForce += f;
}

void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t)
{
	m_links[i].m_appliedConstraintTorque += t;
}

void btMultiBody::addJointTorque(int i, btScalar Q)
{
	m_links[i].m_jointTorque[0] += Q;
}

void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q)
{
	m_links[i].m_jointTorque[dof] += Q;
}

void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q)
{
	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
		m_links[i].m_jointTorque[dof] = Q[dof];
}

const btVector3 &btMultiBody::getLinkForce(int i) const
{
	return m_links[i].m_appliedForce;
}

const btVector3 &btMultiBody::getLinkTorque(int i) const
{
	return m_links[i].m_appliedTorque;
}

btScalar btMultiBody::getJointTorque(int i) const
{
	return m_links[i].m_jointTorque[0];
}

btScalar *btMultiBody::getJointTorqueMultiDof(int i)
{
	return &m_links[i].m_jointTorque[0];
}

inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)  //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?
{
	btVector3 row0 = btVector3(
		v0.x() * v1.x(),
		v0.x() * v1.y(),
		v0.x() * v1.z());
	btVector3 row1 = btVector3(
		v0.y() * v1.x(),
		v0.y() * v1.y(),
		v0.y() * v1.z());
	btVector3 row2 = btVector3(
		v0.z() * v1.x(),
		v0.z() * v1.y(),
		v0.z() * v1.z());

	btMatrix3x3 m(row0[0], row0[1], row0[2],
				  row1[0], row1[1], row1[2],
				  row2[0], row2[1], row2[2]);
	return m;
}

#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
//

void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
    btAlignedObjectArray<btScalar> &scratch_r,
    btAlignedObjectArray<btVector3> &scratch_v,
    btAlignedObjectArray<btMatrix3x3> &scratch_m,
	bool isConstraintPass,
	bool jointFeedbackInWorldSpace,
	bool jointFeedbackInJointFrame)
{
	// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
	// and the base linear & angular accelerations.

	// We apply damping forces in this routine as well as any external forces specified by the
	// caller (via addBaseForce etc).

	// output should point to an array of 6 + num_links reals.
	// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
	// num_links joint acceleration values.

	// We added support for multi degree of freedom (multi dof) joints.
	// In addition we also can compute the joint reaction forces. This is performed in a second pass,
	// so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver)

	m_internalNeedsJointFeedback = false;

	int num_links = getNumLinks();

	const btScalar DAMPING_K1_LINEAR = m_linearDamping;
	const btScalar DAMPING_K2_LINEAR = m_linearDamping;

	const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
	const btScalar DAMPING_K2_ANGULAR = m_angularDamping;

	const btVector3 base_vel = getBaseVel();
	const btVector3 base_omega = getBaseOmega();

	// Temporary matrices/vectors -- use scratch space from caller
	// so that we don't have to keep reallocating every frame

	scratch_r.resize(2 * m_dofCount + 7);  //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
	scratch_v.resize(8 * num_links + 6);
	scratch_m.resize(4 * num_links + 4);

	//btScalar * r_ptr = &scratch_r[0];
	btScalar *output = &scratch_r[m_dofCount];  // "output" holds the q_double_dot results
	btVector3 *v_ptr = &scratch_v[0];

	// vhat_i  (top = angular, bottom = linear part)
	btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;
	v_ptr += num_links * 2 + 2;
	//
	// zhat_i^A
	btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
	v_ptr += num_links * 2 + 2;
	//
	// chat_i  (note NOT defined for the base)
	btSpatialMotionVector *spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
	v_ptr += num_links * 2;
	//
	// Ihat_i^A.
	btSymmetricSpatialDyad *spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];

	// Cached 3x3 rotation matrices from parent frame to this frame.
	btMatrix3x3 *rot_from_parent = &m_matrixBuf[0];
	btMatrix3x3 *rot_from_world = &scratch_m[0];

	// hhat_i, ahat_i
	// hhat is NOT stored for the base (but ahat is)
	btSpatialForceVector *h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
	btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr;
	v_ptr += num_links * 2 + 2;
	//
	// Y_i, invD_i
	btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
	btScalar *Y = &scratch_r[0];
	//
	//aux variables
	btSpatialMotionVector spatJointVel;         //spatial velocity due to the joint motion (i.e. without predecessors' influence)
	btScalar D[36];                             //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
	btScalar invD_times_Y[6];                   //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
	btSpatialMotionVector result;               //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
	btScalar Y_minus_hT_a[6];                   //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
	btSpatialForceVector spatForceVecTemps[6];  //6 temporary spatial force vectors
	btSpatialTransformationMatrix fromParent;   //spatial transform from parent to child
	btSymmetricSpatialDyad dyadTemp;            //inertia matrix temp
	btSpatialTransformationMatrix fromWorld;
	fromWorld.m_trnVec.setZero();
	/////////////////

	// ptr to the joint accel part of the output
	btScalar *joint_accel = output + 6;

	// Start of the algorithm proper.

	// First 'upward' loop.
	// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.

	rot_from_parent[0] = btMatrix3x3(m_baseQuat);  //m_baseQuat assumed to be alias!?

	//create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
	spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);

	if (m_fixedBase)
	{
		zeroAccSpatFrc[0].setZero();
	}
	else
	{
		const btVector3 &baseForce = isConstraintPass ? m_baseConstraintForce : m_baseForce;
		const btVector3 &baseTorque = isConstraintPass ? m_baseConstraintTorque : m_baseTorque;
		//external forces
		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));

		//adding damping terms (only)
		const btScalar linDampMult = 1., angDampMult = 1.;
		zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
									linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));

		//
		//p += vhat x Ihat vhat - done in a simpler way
		if (m_useGyroTerm)
			zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));
		//
		zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
	}

	//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
	spatInertia[0].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0),
							 //
							 btMatrix3x3(m_baseMass, 0, 0,
										 0, m_baseMass, 0,
										 0, 0, m_baseMass),
							 //
							 btMatrix3x3(m_baseInertia[0], 0, 0,
										 0, m_baseInertia[1], 0,
										 0, 0, m_baseInertia[2]));

	rot_from_world[0] = rot_from_parent[0];

	//
	for (int i = 0; i < num_links; ++i)
	{
		const int parent = m_links[i].m_parent;
		rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
		rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1];

		fromParent.m_rotMat = rot_from_parent[i + 1];
		fromParent.m_trnVec = m_links[i].m_cachedRVector;
		fromWorld.m_rotMat = rot_from_world[i + 1];
		fromParent.transform(spatVel[parent + 1], spatVel[i + 1]);

		// now set vhat_i to its true value by doing
		// vhat_i += qidot * shat_i
		if (!m_useGlobalVelocities)
		{
			spatJointVel.setZero();

			for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];

			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
			spatVel[i + 1] += spatJointVel;

			//
			// vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
			//spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
		}
		else
		{
			fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i + 1]);
			fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
		}

		// we can now calculate chat_i
		spatVel[i + 1].cross(spatJointVel, spatCoriolisAcc[i]);

		// calculate zhat_i^A
		//
		//external forces
		btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
		btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;

		zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));

#if 0	
		{

			b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
			i+1,
			zeroAccSpatFrc[i+1].m_topVec[0],
			zeroAccSpatFrc[i+1].m_topVec[1],
			zeroAccSpatFrc[i+1].m_topVec[2],

			zeroAccSpatFrc[i+1].m_bottomVec[0],
			zeroAccSpatFrc[i+1].m_bottomVec[1],
			zeroAccSpatFrc[i+1].m_bottomVec[2]);
		}
#endif
		//
		//adding damping terms (only)
		btScalar linDampMult = 1., angDampMult = 1.;
		zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()),
										linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));

		// calculate Ihat_i^A
		//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
		spatInertia[i + 1].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0),
									 //
									 btMatrix3x3(m_links[i].m_mass, 0, 0,
												 0, m_links[i].m_mass, 0,
												 0, 0, m_links[i].m_mass),
									 //
									 btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
												 0, m_links[i].m_inertiaLocal[1], 0,
												 0, 0, m_links[i].m_inertiaLocal[2]));
		//
		//p += vhat x Ihat vhat - done in a simpler way
		if (m_useGyroTerm)
			zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
		//
		zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
		//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
		////clamp parent's omega
		//btScalar parOmegaMod = temp.length();
		//btScalar parOmegaModMax = 1000;
		//if(parOmegaMod > parOmegaModMax)
		//	temp *= parOmegaModMax / parOmegaMod;
		//zeroAccSpatFrc[i+1].addLinear(temp);
		//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
		//temp = spatCoriolisAcc[i].getLinear();
		//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());

		//printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
		//printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());
		//printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
	}

	// 'Downward' loop.
	// (part of TreeForwardDynamics in Mirtich.)
	for (int i = num_links - 1; i >= 0; --i)
	{
		const int parent = m_links[i].m_parent;
		fromParent.m_rotMat = rot_from_parent[i + 1];
		fromParent.m_trnVec = m_links[i].m_cachedRVector;

		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
		{
			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
			//
			hDof = spatInertia[i + 1] * m_links[i].m_axes[dof];
			//
			Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]) - spatCoriolisAcc[i].dot(hDof);
		}
		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
		{
			btScalar *D_row = &D[dof * m_links[i].m_dofCount];
			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
			{
				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
				D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
			}
		}

		btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
		switch (m_links[i].m_jointType)
		{
			case btMultibodyLink::ePrismatic:
			case btMultibodyLink::eRevolute:
			{
				if (D[0] >= SIMD_EPSILON)
				{
					invDi[0] = 1.0f / D[0];
				}
				else
				{
					invDi[0] = 0;
				}
				break;
			}
			case btMultibodyLink::eSpherical:
			case btMultibodyLink::ePlanar:
			{
				const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
				const btMatrix3x3 invD3x3(D3x3.inverse());

				//unroll the loop?
				for (int row = 0; row < 3; ++row)
				{
					for (int col = 0; col < 3; ++col)
					{
						invDi[row * 3 + col] = invD3x3[row][col];
					}
				}

				break;
			}
			default:
			{
			}
		}

		//determine h*D^{-1}
		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
		{
			spatForceVecTemps[dof].setZero();

			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
			{
				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
				//
				spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
			}
		}

		dyadTemp = spatInertia[i + 1];

		//determine (h*D^{-1}) * h^{T}
		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
		{
			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
			//
			dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
		}

		fromParent.transformInverse(dyadTemp, spatInertia[parent + 1], btSpatialTransformationMatrix::Add);

		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
		{
			invD_times_Y[dof] = 0.f;

			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
			{
				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
			}
		}

		spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];

		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
		{
			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
			//
			spatForceVecTemps[0] += hDof * invD_times_Y[dof];
		}

		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);

		zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
	}

	// Second 'upward' loop
	// (part of TreeForwardDynamics in Mirtich)

	if (m_fixedBase)
	{
		spatAcc[0].setZero();
	}
	else
	{
		if (num_links > 0)
		{
			m_cachedInertiaValid = true;
			m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
			m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
			m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
			m_cachedInertiaLowerRight = spatInertia[0].m_topLeftMat.transpose();
		}

		solveImatrix(zeroAccSpatFrc[0], result);
		spatAcc[0] = -result;
	}

	// now do the loop over the m_links
	for (int i = 0; i < num_links; ++i)
	{
		//	qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)
		//	a = apar + cor + Sqdd
		//or
		//	qdd = D^{-1} * (Y - h^{T}*(apar+cor))
		//	a = apar + Sqdd

		const int parent = m_links[i].m_parent;
		fromParent.m_rotMat = rot_from_parent[i + 1];
		fromParent.m_trnVec = m_links[i].m_cachedRVector;

		fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);

		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
		{
			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
			//
			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
		}

		btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
		//D^{-1} * (Y - h^{T}*apar)
		mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);

		spatAcc[i + 1] += spatCoriolisAcc[i];

		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
			spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];

		if (m_links[i].m_jointFeedback)
		{
			m_internalNeedsJointFeedback = true;

			btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
			btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;

			if (jointFeedbackInJointFrame)
			{
				//shift the reaction forces to the joint frame
				//linear (force) component is the same
				//shift the angular (torque, moment) component using the relative position,  m_links[i].m_dVector
				angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
			}

			if (jointFeedbackInWorldSpace)
			{
				if (isConstraintPass)
				{
					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
					m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
				}
				else
				{
					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
				}
			}
			else
			{
				if (isConstraintPass)
				{
					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
					m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
				}
				else
				{
					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
				}
			}
		}
	}

	// transform base accelerations back to the world frame.
	const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
	output[0] = omegadot_out[0];
	output[1] = omegadot_out[1];
	output[2] = omegadot_out[2];

	const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
	output[3] = vdot_out[0];
	output[4] = vdot_out[1];
	output[5] = vdot_out[2];

	/////////////////
	//printf("q = [");
	//printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
	//for(int link = 0; link < getNumLinks(); ++link)
	//	for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
	//		printf("%.6f ", m_links[link].m_jointPos[dof]);
	//printf("]\n");
	////
	//printf("qd = [");
	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
	//	printf("%.6f ", m_realBuf[dof]);
	//printf("]\n");
	//printf("qdd = [");
	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
	//	printf("%.6f ", output[dof]);
	//printf("]\n");
	/////////////////

	// Final step: add the accelerations (times dt) to the velocities.

	if (!isConstraintPass)
	{
		if (dt > 0.)
			applyDeltaVeeMultiDof(output, dt);
	}
	/////
	//btScalar angularThres = 1;
	//btScalar maxAngVel = 0.;
	//bool scaleDown = 1.;
	//for(int link = 0; link < m_links.size(); ++link)
	//{
	//	if(spatVel[link+1].getAngular().length() > maxAngVel)
	//	{
	//		maxAngVel = spatVel[link+1].getAngular().length();
	//		scaleDown = angularThres / spatVel[link+1].getAngular().length();
	//		break;
	//	}
	//}

	//if(scaleDown != 1.)
	//{
	//	for(int link = 0; link < m_links.size(); ++link)
	//	{
	//		if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)
	//		{
	//			for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
	//				getJointVelMultiDof(link)[dof] *= scaleDown;
	//		}
	//	}
	//}
	/////

	/////////////////////
	if (m_useGlobalVelocities)
	{
		for (int i = 0; i < num_links; ++i)
		{
			const int parent = m_links[i].m_parent;
			//rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);    /// <- done
			//rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];		/// <- done

			fromParent.m_rotMat = rot_from_parent[i + 1];
			fromParent.m_trnVec = m_links[i].m_cachedRVector;
			fromWorld.m_rotMat = rot_from_world[i + 1];

			// vhat_i = i_xhat_p(i) * vhat_p(i)
			fromParent.transform(spatVel[parent + 1], spatVel[i + 1]);
			//nice alternative below (using operator *) but it generates temps
			/////////////////////////////////////////////////////////////

			// now set vhat_i to its true value by doing
			// vhat_i += qidot * shat_i
			spatJointVel.setZero();

			for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];

			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
			spatVel[i + 1] += spatJointVel;

			fromWorld.transformInverseRotationOnly(spatVel[i + 1], m_links[i].m_absFrameTotVelocity);
			fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
		}
	}
}

void btMultiBody::solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const
{
	int num_links = getNumLinks();
	///solve I * x = rhs, so the result = invI * rhs
	if (num_links == 0)
	{
		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier

		if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
		{
			result[0] = rhs_bot[0] / m_baseInertia[0];
			result[1] = rhs_bot[1] / m_baseInertia[1];
			result[2] = rhs_bot[2] / m_baseInertia[2];
		}
		else
		{
			result[0] = 0;
			result[1] = 0;
			result[2] = 0;
		}
		if (m_baseMass >= SIMD_EPSILON)
		{
			result[3] = rhs_top[0] / m_baseMass;
			result[4] = rhs_top[1] / m_baseMass;
			result[5] = rhs_top[2] / m_baseMass;
		}
		else
		{
			result[3] = 0;
			result[4] = 0;
			result[5] = 0;
		}
	}
	else
	{
		if (!m_cachedInertiaValid)
		{
			for (int i = 0; i < 6; i++)
			{
				result[i] = 0.f;
			}
			return;
		}
		/// Special routine for calculating the inverse of a spatial inertia matrix
		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f;
		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
		tmp = invIupper_right * m_cachedInertiaLowerRight;
		btMatrix3x3 invI_upper_left = (tmp * Binv);
		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
		tmp = m_cachedInertiaTopLeft * invI_upper_left;
		tmp[0][0] -= 1.0;
		tmp[1][1] -= 1.0;
		tmp[2][2] -= 1.0;
		btMatrix3x3 invI_lower_left = (Binv * tmp);

		//multiply result = invI * rhs
		{
			btVector3 vtop = invI_upper_left * rhs_top;
			btVector3 tmp;
			tmp = invIupper_right * rhs_bot;
			vtop += tmp;
			btVector3 vbot = invI_lower_left * rhs_top;
			tmp = invI_lower_right * rhs_bot;
			vbot += tmp;
			result[0] = vtop[0];
			result[1] = vtop[1];
			result[2] = vtop[2];
			result[3] = vbot[0];
			result[4] = vbot[1];
			result[5] = vbot[2];
		}
	}
}
void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
{
	int num_links = getNumLinks();
	///solve I * x = rhs, so the result = invI * rhs
	if (num_links == 0)
	{
		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
		if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
		{
			result.setAngular(rhs.getAngular() / m_baseInertia);
		}
		else
		{
			result.setAngular(btVector3(0, 0, 0));
		}
		if (m_baseMass >= SIMD_EPSILON)
		{
			result.setLinear(rhs.getLinear() / m_baseMass);
		}
		else
		{
			result.setLinear(btVector3(0, 0, 0));
		}
	}
	else
	{
		/// Special routine for calculating the inverse of a spatial inertia matrix
		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
		if (!m_cachedInertiaValid)
		{
			result.setLinear(btVector3(0, 0, 0));
			result.setAngular(btVector3(0, 0, 0));
			result.setVector(btVector3(0, 0, 0), btVector3(0, 0, 0));
			return;
		}
		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f;
		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
		tmp = invIupper_right * m_cachedInertiaLowerRight;
		btMatrix3x3 invI_upper_left = (tmp * Binv);
		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
		tmp = m_cachedInertiaTopLeft * invI_upper_left;
		tmp[0][0] -= 1.0;
		tmp[1][1] -= 1.0;
		tmp[2][2] -= 1.0;
		btMatrix3x3 invI_lower_left = (Binv * tmp);

		//multiply result = invI * rhs
		{
			btVector3 vtop = invI_upper_left * rhs.getLinear();
			btVector3 tmp;
			tmp = invIupper_right * rhs.getAngular();
			vtop += tmp;
			btVector3 vbot = invI_lower_left * rhs.getLinear();
			tmp = invI_lower_right * rhs.getAngular();
			vbot += tmp;
			result.setVector(vtop, vbot);
		}
	}
}

void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
{
	for (int row = 0; row < rowsA; row++)
	{
		for (int col = 0; col < colsB; col++)
		{
			pC[row * colsB + col] = 0.f;
			for (int inner = 0; inner < rowsB; inner++)
			{
				pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
			}
		}
	}
}

void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
												 btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
{
	// Temporary matrices/vectors -- use scratch space from caller
	// so that we don't have to keep reallocating every frame

	int num_links = getNumLinks();
	scratch_r.resize(m_dofCount);
	scratch_v.resize(4 * num_links + 4);

	btScalar *r_ptr = m_dofCount ? &scratch_r[0] : 0;
	btVector3 *v_ptr = &scratch_v[0];

	// zhat_i^A (scratch space)
	btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
	v_ptr += num_links * 2 + 2;

	// rot_from_parent (cached from calcAccelerations)
	const btMatrix3x3 *rot_from_parent = &m_matrixBuf[0];

	// hhat (cached), accel (scratch)
	// hhat is NOT stored for the base (but ahat is)
	const btSpatialForceVector *h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
	btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr;
	v_ptr += num_links * 2 + 2;

	// Y_i (scratch), invD_i (cached)
	const btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
	btScalar *Y = r_ptr;
	////////////////
	//aux variables
	btScalar invD_times_Y[6];                   //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
	btSpatialMotionVector result;               //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
	btScalar Y_minus_hT_a[6];                   //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
	btSpatialForceVector spatForceVecTemps[6];  //6 temporary spatial force vectors
	btSpatialTransformationMatrix fromParent;
	/////////////////

	// First 'upward' loop.
	// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.

	// Fill in zero_acc
	// -- set to force/torque on the base, zero otherwise
	if (m_fixedBase)
	{
		zeroAccSpatFrc[0].setZero();
	}
	else
	{
		//test forces
		fromParent.m_rotMat = rot_from_parent[0];
		fromParent.transformRotationOnly(btSpatialForceVector(-force[0], -force[1], -force[2], -force[3], -force[4], -force[5]), zeroAccSpatFrc[0]);
	}
	for (int i = 0; i < num_links; ++i)
	{
		zeroAccSpatFrc[i + 1].setZero();
	}

	// 'Downward' loop.
	// (part of TreeForwardDynamics in Mirtich.)
	for (int i = num_links - 1; i >= 0; --i)
	{
		const int parent = m_links[i].m_parent;
		fromParent.m_rotMat = rot_from_parent[i + 1];
		fromParent.m_trnVec = m_links[i].m_cachedRVector;

		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
		{
			Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]);
		}

		btVector3 in_top, in_bottom, out_top, out_bottom;
		const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];

		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
		{
			invD_times_Y[dof] = 0.f;

			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
			{
				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
			}
		}

		// Zp += pXi * (Zi + hi*Yi/Di)
		spatForceVecTemps[0] = zeroAccSpatFrc[i + 1];

		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
		{
			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
			//
			spatForceVecTemps[0] += hDof * invD_times_Y[dof];
		}

		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);

		zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
	}

	// ptr to the joint accel part of the output
	btScalar *joint_accel = output + 6;

	// Second 'upward' loop
	// (part of TreeForwardDynamics in Mirtich)

	if (m_fixedBase)
	{
		spatAcc[0].setZero();
	}
	else
	{
		solveImatrix(zeroAccSpatFrc[0], result);
		spatAcc[0] = -result;
	}

	// now do the loop over the m_links
	for (int i = 0; i < num_links; ++i)
	{
		const int parent = m_links[i].m_parent;
		fromParent.m_rotMat = rot_from_parent[i + 1];
		fromParent.m_trnVec = m_links[i].m_cachedRVector;

		fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);

		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
		{
			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
			//
			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
		}

		const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
		mulMatrix(const_cast<btScalar *>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);

		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
			spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
	}

	// transform base accelerations back to the world frame.
	btVector3 omegadot_out;
	omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
	output[0] = omegadot_out[0];
	output[1] = omegadot_out[1];
	output[2] = omegadot_out[2];

	btVector3 vdot_out;
	vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();
	output[3] = vdot_out[0];
	output[4] = vdot_out[1];
	output[5] = vdot_out[2];

	/////////////////
	//printf("delta = [");
	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
	//	printf("%.2f ", output[dof]);
	//printf("]\n");
	/////////////////
}

void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
{
	int num_links = getNumLinks();
	// step position by adding dt * velocity
	//btVector3 v = getBaseVel();
	//m_basePos += dt * v;
	//
	btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
	btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
	//
	pBasePos[0] += dt * pBaseVel[0];
	pBasePos[1] += dt * pBaseVel[1];
	pBasePos[2] += dt * pBaseVel[2];

	///////////////////////////////
	//local functor for quaternion integration (to avoid error prone redundancy)
	struct
	{
		//"exponential map" based on btTransformUtil::integrateTransform(..)
		void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
		{
			//baseBody	=>	quat is alias and omega is global coor
			//!baseBody	=>	quat is alibi and omega is local coor

			btVector3 axis;
			btVector3 angvel;

			if (!baseBody)
				angvel = quatRotate(quat, omega);  //if quat is not m_baseQuat, it is alibi => ok
			else
				angvel = omega;

			btScalar fAngle = angvel.length();
			//limit the angular motion
			if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
			{
				fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;
			}

			if (fAngle < btScalar(0.001))
			{
				// use Taylor's expansions of sync function
				axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle);
			}
			else
			{
				// sync(fAngle) = sin(c*fAngle)/t
				axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle);
			}

			if (!baseBody)
				quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat;
			else
				quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5)));
			//equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();

			quat.normalize();
		}
	} pQuatUpdateFun;
	///////////////////////////////

	//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
	//
	btScalar *pBaseQuat = pq ? pq : m_baseQuat;
	btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
	//
	btQuaternion baseQuat;
	baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
	btVector3 baseOmega;
	baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
	pQuatUpdateFun(baseOmega, baseQuat, true, dt);
	pBaseQuat[0] = baseQuat.x();
	pBaseQuat[1] = baseQuat.y();
	pBaseQuat[2] = baseQuat.z();
	pBaseQuat[3] = baseQuat.w();

	//printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
	//printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
	//printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());

	if (pq)
		pq += 7;
	if (pqd)
		pqd += 6;

	// Finally we can update m_jointPos for each of the m_links
	for (int i = 0; i < num_links; ++i)
	{
		btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);
		btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));

		switch (m_links[i].m_jointType)
		{
			case btMultibodyLink::ePrismatic:
			case btMultibodyLink::eRevolute:
			{
				btScalar jointVel = pJointVel[0];
				pJointPos[0] += dt * jointVel;
				break;
			}
			case btMultibodyLink::eSpherical:
			{
				btVector3 jointVel;
				jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
				btQuaternion jointOri;
				jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
				pQuatUpdateFun(jointVel, jointOri, false, dt);
				pJointPos[0] = jointOri.x();
				pJointPos[1] = jointOri.y();
				pJointPos[2] = jointOri.z();
				pJointPos[3] = jointOri.w();
				break;
			}
			case btMultibodyLink::ePlanar:
			{
				pJointPos[0] += dt * getJointVelMultiDof(i)[0];

				btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
				btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
				pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
				pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;

				break;
			}
			default:
			{
			}
		}

		m_links[i].updateCacheMultiDof(pq);

		if (pq)
			pq += m_links[i].m_posVarCount;
		if (pqd)
			pqd += m_links[i].m_dofCount;
	}
}

void btMultiBody::fillConstraintJacobianMultiDof(int link,
												 const btVector3 &contact_point,
												 const btVector3 &normal_ang,
												 const btVector3 &normal_lin,
												 btScalar *jac,
												 btAlignedObjectArray<btScalar> &scratch_r1,
												 btAlignedObjectArray<btVector3> &scratch_v,
												 btAlignedObjectArray<btMatrix3x3> &scratch_m) const
{
	// temporary space
	int num_links = getNumLinks();
	int m_dofCount = getNumDofs();
	scratch_v.resize(3 * num_links + 3);  //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
	scratch_m.resize(num_links + 1);

	btVector3 *v_ptr = &scratch_v[0];
	btVector3 *p_minus_com_local = v_ptr;
	v_ptr += num_links + 1;
	btVector3 *n_local_lin = v_ptr;
	v_ptr += num_links + 1;
	btVector3 *n_local_ang = v_ptr;
	v_ptr += num_links + 1;
	btAssert(v_ptr - &scratch_v[0] == scratch_v.size());

	//scratch_r.resize(m_dofCount);
	//btScalar *results = m_dofCount > 0 ? &scratch_r[0] : 0;

    scratch_r1.resize(m_dofCount+num_links);
    btScalar * results = m_dofCount > 0 ? &scratch_r1[0] : 0;
    btScalar* links = num_links? &scratch_r1[m_dofCount] : 0;
    int numLinksChildToRoot=0;
    int l = link;
    while (l != -1)
    {
        links[numLinksChildToRoot++]=l;
        l = m_links[l].m_parent;
    }
    
	btMatrix3x3 *rot_from_world = &scratch_m[0];

	const btVector3 p_minus_com_world = contact_point - m_basePos;
	const btVector3 &normal_lin_world = normal_lin;  //convenience
	const btVector3 &normal_ang_world = normal_ang;

	rot_from_world[0] = btMatrix3x3(m_baseQuat);

	// omega coeffients first.
	btVector3 omega_coeffs_world;
	omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
	jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
	jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
	jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
	// then v coefficients
	jac[3] = normal_lin_world[0];
	jac[4] = normal_lin_world[1];
	jac[5] = normal_lin_world[2];

	//create link-local versions of p_minus_com and normal
	p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
	n_local_lin[0] = rot_from_world[0] * normal_lin_world;
	n_local_ang[0] = rot_from_world[0] * normal_ang_world;

	// Set remaining jac values to zero for now.
	for (int i = 6; i < 6 + m_dofCount; ++i)
	{
		jac[i] = 0;
	}

	// Qdot coefficients, if necessary.
	if (num_links > 0 && link > -1)
	{
        // TODO: (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
		// which is resulting in repeated work being done...)

		// calculate required normals & positions in the local frames.
        for (int a = 0; a < numLinksChildToRoot; a++)
        {
            int i = links[numLinksChildToRoot-1-a];
        	// transform to local frame
			const int parent = m_links[i].m_parent;
			const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
			rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];

			n_local_lin[i + 1] = mtx * n_local_lin[parent + 1];
			n_local_ang[i + 1] = mtx * n_local_ang[parent + 1];
			p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] - m_links[i].m_cachedRVector;

			// calculate the jacobian entry
			switch (m_links[i].m_jointType)
			{
				case btMultibodyLink::eRevolute:
				{
					results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));
					results[m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
					break;
				}
				case btMultibodyLink::ePrismatic:
				{
					results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(0));
					break;
				}
				case btMultibodyLink::eSpherical:
				{
					results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));
					results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(1));
					results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(2));

					results[m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
					results[m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(1));
					results[m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(2));

					break;
				}
				case btMultibodyLink::ePlanar:
				{
					results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]));  // + m_links[i].getAxisBottom(0));
					results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(1));
					results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(2));

					break;
				}
				default:
				{
				}
			}
		}

		// Now copy through to output.
		//printf("jac[%d] = ", link);
		while (link != -1)
		{
			for (int dof = 0; dof < m_links[link].m_dofCount; ++dof)
			{
				jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
				//printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
			}

			link = m_links[link].m_parent;
		}
		//printf("]\n");
	}
}

void btMultiBody::wakeUp()
{
	m_sleepTimer = 0;
	m_awake = true;
}

void btMultiBody::goToSleep()
{
	m_awake = false;
}

void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
{
	extern bool gDisableDeactivation;
	if (!m_canSleep || gDisableDeactivation)
	{
		m_awake = true;
		m_sleepTimer = 0;
		return;
	}

	

	// motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
	btScalar motion = 0;
	{
		for (int i = 0; i < 6 + m_dofCount; ++i)
			motion += m_realBuf[i] * m_realBuf[i];
	}

	if (motion < SLEEP_EPSILON)
	{
		m_sleepTimer += timestep;
		if (m_sleepTimer > SLEEP_TIMEOUT)
		{
			goToSleep();
		}
	}
	else
	{
		m_sleepTimer = 0;
		if (m_canWakeup)
		{
			if (!m_awake)
				wakeUp();
		}
	}
}

void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
{
	int num_links = getNumLinks();

	// Cached 3x3 rotation matrices from parent frame to this frame.
	btMatrix3x3 *rot_from_parent = (btMatrix3x3 *)&m_matrixBuf[0];

	rot_from_parent[0] = btMatrix3x3(m_baseQuat);  //m_baseQuat assumed to be alias!?

	for (int i = 0; i < num_links; ++i)
	{
		rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
	}

	int nLinks = getNumLinks();
	///base + num m_links
	world_to_local.resize(nLinks + 1);
	local_origin.resize(nLinks + 1);

	world_to_local[0] = getWorldToBaseRot();
	local_origin[0] = getBasePos();

	for (int k = 0; k < getNumLinks(); k++)
	{
		const int parent = getParent(k);
		world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];
		local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));
	}

	for (int link = 0; link < getNumLinks(); link++)
	{
		int index = link + 1;

		btVector3 posr = local_origin[index];
		btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
		btTransform tr;
		tr.setIdentity();
		tr.setOrigin(posr);
		tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
		getLink(link).m_cachedWorldTransform = tr;
	}
}

void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
{
	world_to_local.resize(getNumLinks() + 1);
	local_origin.resize(getNumLinks() + 1);

	world_to_local[0] = getWorldToBaseRot();
	local_origin[0] = getBasePos();

	if (getBaseCollider())
	{
		btVector3 posr = local_origin[0];
		//	float pos[4]={posr.x(),posr.y(),posr.z(),1};
		btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
		btTransform tr;
		tr.setIdentity();
		tr.setOrigin(posr);
		tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));

		getBaseCollider()->setWorldTransform(tr);
	}

	for (int k = 0; k < getNumLinks(); k++)
	{
		const int parent = getParent(k);
		world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];
		local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));
	}

	for (int m = 0; m < getNumLinks(); m++)
	{
		btMultiBodyLinkCollider *col = getLink(m).m_collider;
		if (col)
		{
			int link = col->m_link;
			btAssert(link == m);

			int index = link + 1;

			btVector3 posr = local_origin[index];
			//			float pos[4]={posr.x(),posr.y(),posr.z(),1};
			btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
			btTransform tr;
			tr.setIdentity();
			tr.setOrigin(posr);
			tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));

			col->setWorldTransform(tr);
		}
	}
}

int btMultiBody::calculateSerializeBufferSize() const
{
	int sz = sizeof(btMultiBodyData);
	return sz;
}

///fills the dataBuffer and returns the struct name (and 0 on failure)
const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *serializer) const
{
	btMultiBodyData *mbd = (btMultiBodyData *)dataBuffer;
	getBasePos().serialize(mbd->m_baseWorldPosition);
	getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation);
	getBaseVel().serialize(mbd->m_baseLinearVelocity);
	getBaseOmega().serialize(mbd->m_baseAngularVelocity);

	mbd->m_baseMass = this->getBaseMass();
	getBaseInertia().serialize(mbd->m_baseInertia);
	{
		char *name = (char *)serializer->findNameForPointer(m_baseName);
		mbd->m_baseName = (char *)serializer->getUniquePointer(name);
		if (mbd->m_baseName)
		{
			serializer->serializeName(name);
		}
	}
	mbd->m_numLinks = this->getNumLinks();
	if (mbd->m_numLinks)
	{
		int sz = sizeof(btMultiBodyLinkData);
		int numElem = mbd->m_numLinks;
		btChunk *chunk = serializer->allocate(sz, numElem);
		btMultiBodyLinkData *memPtr = (btMultiBodyLinkData *)chunk->m_oldPtr;
		for (int i = 0; i < numElem; i++, memPtr++)
		{
			memPtr->m_jointType = getLink(i).m_jointType;
			memPtr->m_dofCount = getLink(i).m_dofCount;
			memPtr->m_posVarCount = getLink(i).m_posVarCount;

			getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);

			getLink(i).m_absFrameTotVelocity.m_topVec.serialize(memPtr->m_absFrameTotVelocityTop);
			getLink(i).m_absFrameTotVelocity.m_bottomVec.serialize(memPtr->m_absFrameTotVelocityBottom);
			getLink(i).m_absFrameLocVelocity.m_topVec.serialize(memPtr->m_absFrameLocVelocityTop);
			getLink(i).m_absFrameLocVelocity.m_bottomVec.serialize(memPtr->m_absFrameLocVelocityBottom);

			memPtr->m_linkMass = getLink(i).m_mass;
			memPtr->m_parentIndex = getLink(i).m_parent;
			memPtr->m_jointDamping = getLink(i).m_jointDamping;
			memPtr->m_jointFriction = getLink(i).m_jointFriction;
			memPtr->m_jointLowerLimit = getLink(i).m_jointLowerLimit;
			memPtr->m_jointUpperLimit = getLink(i).m_jointUpperLimit;
			memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce;
			memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity;

			getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);
			getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
			getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
			btAssert(memPtr->m_dofCount <= 3);
			for (int dof = 0; dof < getLink(i).m_dofCount; dof++)
			{
				getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
				getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);

				memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
				memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
			}
			int numPosVar = getLink(i).m_posVarCount;
			for (int posvar = 0; posvar < numPosVar; posvar++)
			{
				memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
			}

			{
				char *name = (char *)serializer->findNameForPointer(m_links[i].m_linkName);
				memPtr->m_linkName = (char *)serializer->getUniquePointer(name);
				if (memPtr->m_linkName)
				{
					serializer->serializeName(name);
				}
			}
			{
				char *name = (char *)serializer->findNameForPointer(m_links[i].m_jointName);
				memPtr->m_jointName = (char *)serializer->getUniquePointer(name);
				if (memPtr->m_jointName)
				{
					serializer->serializeName(name);
				}
			}
			memPtr->m_linkCollider = (btCollisionObjectData *)serializer->getUniquePointer(getLink(i).m_collider);
		}
		serializer->finalizeChunk(chunk, btMultiBodyLinkDataName, BT_ARRAY_CODE, (void *)&m_links[0]);
	}
	mbd->m_links = mbd->m_numLinks ? (btMultiBodyLinkData *)serializer->getUniquePointer((void *)&m_links[0]) : 0;

	// Fill padding with zeros to appease msan.
#ifdef BT_USE_DOUBLE_PRECISION
	memset(mbd->m_padding, 0, sizeof(mbd->m_padding));
#endif

	return btMultiBodyDataName;
}