Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletDynamics / Featherstone / btMultiBodyLink.h
index fbd54c45db3260dc9b995db68fcf25ee916cc56f..668e44439045ffaadb47d9855c04b91b6fa04198 100644 (file)
@@ -24,84 +24,193 @@ enum       btMultiBodyLinkFlags
 {
        BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
 };
+
+//both defines are now permanently enabled
+#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+#define TEST_SPATIAL_ALGEBRA_LAYER
+
 //
-// Link struct
+// Various spatial helper functions
 //
 
-struct btMultibodyLink 
-{
+//namespace {
 
-       BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    btScalar joint_pos;    // qi
+#include "LinearMath/btSpatialAlgebra.h"
 
-    btScalar mass;         // mass of link
-    btVector3 inertia;   // inertia of link (local frame; diagonal)
+//}
 
-    int parent;         // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
+//
+// Link struct
+//
 
-    btQuaternion zero_rot_parent_to_this;    // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
+struct btMultibodyLink 
+{
 
-    // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
-    // for prismatic: axis_top = zero;
-    //                axis_bottom = unit vector along the joint axis.
-    // for revolute: axis_top = unit vector along the rotation axis (u);
-    //               axis_bottom = u cross d_vector.
-    btVector3 axis_top;
-    btVector3 axis_bottom;
+       BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    btVector3 d_vector;   // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only.
+    btScalar m_mass;         // mass of link
+    btVector3 m_inertiaLocal;   // inertia of link (local frame; diagonal)
 
-    // e_vector is constant, but depends on the joint type
-    // prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
-    // revolute: vector from parent's COM to the pivot point, in PARENT's frame.
-    btVector3 e_vector;
+    int m_parent;         // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
 
-    bool is_revolute;   // true = revolute, false = prismatic
+    btQuaternion m_zeroRotParentToThis;    // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
 
-    btQuaternion cached_rot_parent_to_this;   // rotates vectors in parent frame to vectors in local frame
-    btVector3 cached_r_vector;                // vector from COM of parent to COM of this link, in local frame.
+    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;
 
-    btVector3 applied_force;    // In WORLD frame
-    btVector3 applied_torque;   // In WORLD frame
-    btScalar joint_torque;
+       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!
 
     // ctor: set some sensible defaults
        btMultibodyLink()
-               : joint_pos(0),
-                       mass(1),
-                       parent(-1),
-                       zero_rot_parent_to_this(1, 0, 0, 0),
-                       is_revolute(false),
-                       cached_rot_parent_to_this(1, 0, 0, 0),
-                       joint_torque(0),
+               :       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_flags(0),
+                       m_dofCount(0),
+                       m_posVarCount(0),
+                       m_jointType(btMultibodyLink::eInvalid),
+                       m_jointFeedback(0),
+                       m_linkName(0),
+                       m_jointName(0)
        {
-               inertia.setValue(1, 1, 1);
-               axis_top.setValue(0, 0, 0);
-               axis_bottom.setValue(1, 0, 0);
-               d_vector.setValue(0, 0, 0);
-               e_vector.setValue(0, 0, 0);
-               cached_r_vector.setValue(0, 0, 0);
-               applied_force.setValue( 0, 0, 0);
-               applied_torque.setValue(0, 0, 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 cached_rot_parent_to_this and cached_r_vector
-    void updateCache()
+    // routine to update m_cachedRotParentToThis and m_cachedRVector
+       void updateCacheMultiDof(btScalar *pq = 0)
        {
-               if (is_revolute) 
-               {
-                       cached_rot_parent_to_this = btQuaternion(axis_top,-joint_pos) * zero_rot_parent_to_this;
-                       cached_r_vector = d_vector + quatRotate(cached_rot_parent_to_this,e_vector);
-               } else 
+               btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
+
+               switch(m_jointType)
                {
-                       // cached_rot_parent_to_this never changes, so no need to update
-                       cached_r_vector = e_vector + joint_pos * axis_bottom;
+                       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);
+                       }
                }
        }
 };