1 files changed, 239 insertions, 239 deletions
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
index ee0fc193b..92d41dfac 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
@@ -1,239 +1,239 @@
-/*
-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_appliedConstraintForce.setValue(0, 0, 0);
-		m_appliedConstraintTorque.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
+/*
+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_appliedConstraintForce.setValue(0, 0, 0);
+		m_appliedConstraintTorque.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