diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h | 1042 |
1 files changed, 585 insertions, 457 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h index 8fbe6cda827..f2acfab9b24 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h @@ -21,7 +21,6 @@ */ - #ifndef BT_MULTIBODY_H #define BT_MULTIBODY_H @@ -31,170 +30,193 @@ #include "LinearMath/btMatrix3x3.h" #include "LinearMath/btAlignedObjectArray.h" - ///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms #ifdef BT_USE_DOUBLE_PRECISION - #define btMultiBodyData btMultiBodyDoubleData - #define btMultiBodyDataName "btMultiBodyDoubleData" - #define btMultiBodyLinkData btMultiBodyLinkDoubleData - #define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData" +#define btMultiBodyData btMultiBodyDoubleData +#define btMultiBodyDataName "btMultiBodyDoubleData" +#define btMultiBodyLinkData btMultiBodyLinkDoubleData +#define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData" #else - #define btMultiBodyData btMultiBodyFloatData - #define btMultiBodyDataName "btMultiBodyFloatData" - #define btMultiBodyLinkData btMultiBodyLinkFloatData - #define btMultiBodyLinkDataName "btMultiBodyLinkFloatData" -#endif //BT_USE_DOUBLE_PRECISION +#define btMultiBodyData btMultiBodyFloatData +#define btMultiBodyDataName "btMultiBodyFloatData" +#define btMultiBodyLinkData btMultiBodyLinkFloatData +#define btMultiBodyLinkDataName "btMultiBodyLinkFloatData" +#endif //BT_USE_DOUBLE_PRECISION #include "btMultiBodyLink.h" class btMultiBodyLinkCollider; -ATTRIBUTE_ALIGNED16(class) btMultiBody +ATTRIBUTE_ALIGNED16(class) +btMultiBody { public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - // - // initialization - // - - btMultiBody(int n_links, // NOT including the base - btScalar mass, // mass of base - const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal - bool fixedBase, // whether the base is fixed (true) or can move (false) - bool canSleep, bool deprecatedMultiDof=true); + // + // initialization + // + btMultiBody(int n_links, // NOT including the base + btScalar mass, // mass of base + const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal + bool fixedBase, // whether the base is fixed (true) or can move (false) + bool canSleep, bool deprecatedMultiDof = true); virtual ~btMultiBody(); - + //note: fixed link collision with parent is always disabled - void setupFixed(int linkIndex, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true); - - + void setupFixed(int i, //linkIndex + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision = true); + void setupPrismatic(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &jointAxis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, - bool disableParentCollision); - - void setupRevolute(int linkIndex, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parentIndex, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &jointAxis, // in my frame - const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame - const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame - bool disableParentCollision=false); - - void setupSpherical(int linkIndex, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame - const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame - bool disableParentCollision=false); - - void setupPlanar(int i, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &rotationAxis, - const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame - bool disableParentCollision=false); - - const btMultibodyLink& getLink(int index) const + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &jointAxis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, + bool disableParentCollision); + + void setupRevolute(int i, // 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, + int parentIndex, + const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 + const btVector3 &jointAxis, // in my frame + const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame + const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame + bool disableParentCollision = false); + + void setupSpherical(int i, // linkIndex, 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 + const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame + const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame + bool disableParentCollision = false); + + void setupPlanar(int i, // 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 + const btVector3 &rotationAxis, + const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame + bool disableParentCollision = false); + + const btMultibodyLink &getLink(int index) const { return m_links[index]; } - btMultibodyLink& getLink(int index) + btMultibodyLink &getLink(int index) { return m_links[index]; } - - void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base + void setBaseCollider(btMultiBodyLinkCollider * collider) //collider can be NULL to disable collision for the base { m_baseCollider = collider; } - const btMultiBodyLinkCollider* getBaseCollider() const + const btMultiBodyLinkCollider *getBaseCollider() const { return m_baseCollider; } - btMultiBodyLinkCollider* getBaseCollider() + btMultiBodyLinkCollider *getBaseCollider() { return m_baseCollider; } - // - // get parent - // input: link num from 0 to num_links-1 - // output: link num from 0 to num_links-1, OR -1 to mean the base. - // - int getParent(int link_num) const; - - - // - // get number of m_links, masses, moments of inertia - // + const btMultiBodyLinkCollider *getLinkCollider(int index) const + { + if (index >= 0 && index < getNumLinks()) + { + return getLink(index).m_collider; + } + return 0; + } + + btMultiBodyLinkCollider *getLinkCollider(int index) + { + if (index >= 0 && index < getNumLinks()) + { + return getLink(index).m_collider; + } + return 0; + } - int getNumLinks() const { return m_links.size(); } + // + // get parent + // input: link num from 0 to num_links-1 + // output: link num from 0 to num_links-1, OR -1 to mean the base. + // + int getParent(int link_num) const; + + // + // get number of m_links, masses, moments of inertia + // + + int getNumLinks() const { return m_links.size(); } int getNumDofs() const { return m_dofCount; } int getNumPosVars() const { return m_posVarCnt; } - btScalar getBaseMass() const { return m_baseMass; } - const btVector3 & getBaseInertia() const { return m_baseInertia; } - btScalar getLinkMass(int i) const; - const btVector3 & getLinkInertia(int i) const; - - - - // - // change mass (incomplete: can only change base mass and inertia at present) - // + btScalar getBaseMass() const { return m_baseMass; } + const btVector3 &getBaseInertia() const { return m_baseInertia; } + btScalar getLinkMass(int i) const; + const btVector3 &getLinkInertia(int i) const; - void setBaseMass(btScalar mass) { m_baseMass = mass; } - void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; } + // + // change mass (incomplete: can only change base mass and inertia at present) + // + void setBaseMass(btScalar mass) { m_baseMass = mass; } + void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; } - // - // get/set pos/vel/rot/omega for the base link - // + // + // get/set pos/vel/rot/omega for the base link + // - const btVector3 & getBasePos() const { return m_basePos; } // in world frame - const btVector3 getBaseVel() const - { - return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]); - } // in world frame - const btQuaternion & getWorldToBaseRot() const + const btVector3 &getBasePos() const { - return m_baseQuat; - } // rotates world vectors into base frame - btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); } // in world frame + return m_basePos; + } // in world frame + const btVector3 getBaseVel() const + { + return btVector3(m_realBuf[3], m_realBuf[4], m_realBuf[5]); + } // in world frame + const btQuaternion &getWorldToBaseRot() const + { + return m_baseQuat; + } + + const btVector3 &getInterpolateBasePos() const + { + return m_basePos_interpolate; + } // in world frame + const btQuaternion &getInterpolateWorldToBaseRot() const + { + return m_baseQuat_interpolate; + } + + // rotates world vectors into base frame + btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); } // in world frame - void setBasePos(const btVector3 &pos) - { - m_basePos = pos; + void setBasePos(const btVector3 &pos) + { + m_basePos = pos; + m_basePos_interpolate = pos; } - void setBaseWorldTransform(const btTransform& tr) + void setBaseWorldTransform(const btTransform &tr) { setBasePos(tr.getOrigin()); setWorldToBaseRot(tr.getRotation().inverse()); - } btTransform getBaseWorldTransform() const @@ -205,186 +227,212 @@ public: return tr; } - void setBaseVel(const btVector3 &vel) - { - - m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2]; + void setBaseVel(const btVector3 &vel) + { + m_realBuf[3] = vel[0]; + m_realBuf[4] = vel[1]; + m_realBuf[5] = vel[2]; } - void setWorldToBaseRot(const btQuaternion &rot) - { - m_baseQuat = rot; //m_baseQuat asumed to ba alias!? + void setWorldToBaseRot(const btQuaternion &rot) + { + m_baseQuat = rot; //m_baseQuat asumed to ba alias!? + m_baseQuat_interpolate = rot; } - void setBaseOmega(const btVector3 &omega) - { - m_realBuf[0]=omega[0]; - m_realBuf[1]=omega[1]; - m_realBuf[2]=omega[2]; + void setBaseOmega(const btVector3 &omega) + { + m_realBuf[0] = omega[0]; + m_realBuf[1] = omega[1]; + m_realBuf[2] = omega[2]; } + // + // get/set pos/vel for child m_links (i = 0 to num_links-1) + // - // - // get/set pos/vel for child m_links (i = 0 to num_links-1) - // - - btScalar getJointPos(int i) const; - btScalar getJointVel(int i) const; - - btScalar * getJointVelMultiDof(int i); - btScalar * getJointPosMultiDof(int i); + btScalar getJointPos(int i) const; + btScalar getJointVel(int i) const; - const btScalar * getJointVelMultiDof(int i) const ; - const btScalar * getJointPosMultiDof(int i) const ; + btScalar *getJointVelMultiDof(int i); + btScalar *getJointPosMultiDof(int i); - void setJointPos(int i, btScalar q); - void setJointVel(int i, btScalar qdot); - void setJointPosMultiDof(int i, btScalar *q); - void setJointVelMultiDof(int i, btScalar *qdot); + const btScalar *getJointVelMultiDof(int i) const; + const btScalar *getJointPosMultiDof(int i) const; + void setJointPos(int i, btScalar q); + void setJointVel(int i, btScalar qdot); + void setJointPosMultiDof(int i, const double *q); + void setJointVelMultiDof(int i, const double *qdot); + void setJointPosMultiDof(int i, const float *q); + void setJointVelMultiDof(int i, const float *qdot); - - // - // direct access to velocities as a vector of 6 + num_links elements. - // (omega first, then v, then joint velocities.) - // - const btScalar * getVelocityVector() const - { - return &m_realBuf[0]; + // + // direct access to velocities as a vector of 6 + num_links elements. + // (omega first, then v, then joint velocities.) + // + const btScalar *getVelocityVector() const + { + return &m_realBuf[0]; } -/* btScalar * getVelocityVector() + + const btScalar *getDeltaVelocityVector() const + { + return &m_deltaV[0]; + } + + const btScalar *getSplitVelocityVector() const + { + return &m_splitV[0]; + } + /* btScalar * getVelocityVector() { return &real_buf[0]; } - */ + */ - // - // get the frames of reference (positions and orientations) of the child m_links - // (i = 0 to num_links-1) - // + // + // get the frames of reference (positions and orientations) of the child m_links + // (i = 0 to num_links-1) + // - const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords - const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. + const btVector3 &getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords + const btQuaternion &getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. + const btVector3 &getInterpolateRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords + const btQuaternion &getInterpolateParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. + // + // transform vectors in local frame of link i to world frame (or vice versa) + // + btVector3 localPosToWorld(int i, const btVector3 &local_pos) const; + btVector3 localDirToWorld(int i, const btVector3 &local_dir) const; + btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const; + btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const; - // - // transform vectors in local frame of link i to world frame (or vice versa) - // - btVector3 localPosToWorld(int i, const btVector3 &vec) const; - btVector3 localDirToWorld(int i, const btVector3 &vec) const; - btVector3 worldPosToLocal(int i, const btVector3 &vec) const; - btVector3 worldDirToLocal(int i, const btVector3 &vec) const; - + // + // transform a frame in local coordinate to a frame in world coordinate + // + btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const; - // - // calculate kinetic energy and angular momentum - // useful for debugging. - // - btScalar getKineticEnergy() const; - btVector3 getAngularMomentum() const; - - - // - // set external forces and torques. Note all external forces/torques are given in the WORLD frame. - // + // + // set external forces and torques. Note all external forces/torques are given in the WORLD frame. + // - void clearForcesAndTorques(); - void clearConstraintForces(); + void clearForcesAndTorques(); + void clearConstraintForces(); void clearVelocities(); - void addBaseForce(const btVector3 &f) - { - m_baseForce += f; + void addBaseForce(const btVector3 &f) + { + m_baseForce += f; } - void addBaseTorque(const btVector3 &t) { m_baseTorque += t; } - void addLinkForce(int i, const btVector3 &f); - void addLinkTorque(int i, const btVector3 &t); + void addBaseTorque(const btVector3 &t) { m_baseTorque += t; } + void addLinkForce(int i, const btVector3 &f); + void addLinkTorque(int i, const btVector3 &t); - void addBaseConstraintForce(const btVector3 &f) - { - m_baseConstraintForce += f; - } - void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; } - void addLinkConstraintForce(int i, const btVector3 &f); - void addLinkConstraintTorque(int i, const btVector3 &t); - + void addBaseConstraintForce(const btVector3 &f) + { + m_baseConstraintForce += f; + } + void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; } + void addLinkConstraintForce(int i, const btVector3 &f); + void addLinkConstraintTorque(int i, const btVector3 &t); -void addJointTorque(int i, btScalar Q); + void addJointTorque(int i, btScalar Q); void addJointTorqueMultiDof(int i, int dof, btScalar Q); void addJointTorqueMultiDof(int i, const btScalar *Q); - const btVector3 & getBaseForce() const { return m_baseForce; } - const btVector3 & getBaseTorque() const { return m_baseTorque; } - const btVector3 & getLinkForce(int i) const; - const btVector3 & getLinkTorque(int i) const; - btScalar getJointTorque(int i) const; - btScalar * getJointTorqueMultiDof(int i); - - - // - // dynamics routines. - // - - // timestep the velocities (given the external forces/torques set using addBaseForce etc). - // also sets up caches for calcAccelerationDeltas. - // - // Note: the caller must provide three vectors which are used as - // temporary scratch space. The idea here is to reduce dynamic - // memory allocation: the same scratch vectors can be re-used - // again and again for different Multibodies, instead of each - // btMultiBody allocating (and then deallocating) their own - // individual scratch buffers. This gives a considerable speed - // improvement, at least on Windows (where dynamic memory - // allocation appears to be fairly slow). - // - - - void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, - btAlignedObjectArray<btScalar> &scratch_r, - btAlignedObjectArray<btVector3> &scratch_v, - btAlignedObjectArray<btMatrix3x3> &scratch_m, - bool isConstraintPass=false - ); - -///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead - void stepVelocitiesMultiDof(btScalar dt, - btAlignedObjectArray<btScalar> &scratch_r, - btAlignedObjectArray<btVector3> &scratch_v, - btAlignedObjectArray<btMatrix3x3> &scratch_m, - bool isConstraintPass=false) - { - computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt,scratch_r,scratch_v,scratch_m,isConstraintPass); - } + const btVector3 &getBaseForce() const { return m_baseForce; } + const btVector3 &getBaseTorque() const { return m_baseTorque; } + const btVector3 &getLinkForce(int i) const; + const btVector3 &getLinkTorque(int i) const; + btScalar getJointTorque(int i) const; + btScalar *getJointTorqueMultiDof(int i); + + // + // dynamics routines. + // + + // timestep the velocities (given the external forces/torques set using addBaseForce etc). + // also sets up caches for calcAccelerationDeltas. + // + // Note: the caller must provide three vectors which are used as + // temporary scratch space. The idea here is to reduce dynamic + // memory allocation: the same scratch vectors can be re-used + // again and again for different Multibodies, instead of each + // btMultiBody allocating (and then deallocating) their own + // individual scratch buffers. This gives a considerable speed + // improvement, at least on Windows (where dynamic memory + // allocation appears to be fairly slow). + // - // calcAccelerationDeltasMultiDof - // input: force vector (in same format as jacobian, i.e.: - // 3 torque values, 3 force values, num_links joint torque values) - // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values - // (existing contents of output array are replaced) - // calcAccelerationDeltasMultiDof must have been called first. + void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, + btAlignedObjectArray<btScalar> & scratch_r, + btAlignedObjectArray<btVector3> & scratch_v, + btAlignedObjectArray<btMatrix3x3> & scratch_m, + bool isConstraintPass, + bool jointFeedbackInWorldSpace, + bool jointFeedbackInJointFrame + ); + + ///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead + //void stepVelocitiesMultiDof(btScalar dt, + // btAlignedObjectArray<btScalar> & scratch_r, + // btAlignedObjectArray<btVector3> & scratch_v, + // btAlignedObjectArray<btMatrix3x3> & scratch_m, + // bool isConstraintPass = false) + //{ + // computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false); + //} + + // calcAccelerationDeltasMultiDof + // input: force vector (in same format as jacobian, i.e.: + // 3 torque values, 3 force values, num_links joint torque values) + // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values + // (existing contents of output array are replaced) + // calcAccelerationDeltasMultiDof must have been called first. void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, - btAlignedObjectArray<btScalar> &scratch_r, - btAlignedObjectArray<btVector3> &scratch_v) const; - - - void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier) + btAlignedObjectArray<btScalar> &scratch_r, + btAlignedObjectArray<btVector3> &scratch_v) const; + + void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier) { for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { - m_deltaV[dof] += delta_vee[dof] * multiplier; - } + { + m_deltaV[dof] += delta_vee[dof] * multiplier; + } } + void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier) + { + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_splitV[dof] += delta_vee[dof] * multiplier; + } + } + void addSplitV() + { + applyDeltaVeeMultiDof(&m_splitV[0], 1); + } + void substractSplitV() + { + applyDeltaVeeMultiDof(&m_splitV[0], -1); + + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_splitV[dof] = 0.f; + } + } void processDeltaVeeMultiDof2() { - applyDeltaVeeMultiDof(&m_deltaV[0],1); + applyDeltaVeeMultiDof(&m_deltaV[0], 1); for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { + { m_deltaV[dof] = 0.f; } } - void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier) + void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier) { //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) // printf("%.4f ", delta_vee[dof]*multiplier); @@ -405,65 +453,81 @@ void addJointTorque(int i, btScalar Q); for (int dof = 0; dof < 6 + getNumDofs(); ++dof) { m_realBuf[dof] += delta_vee[dof] * multiplier; - btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity); + btClamp(m_realBuf[dof], -m_maxCoordinateVelocity, m_maxCoordinateVelocity); } - } + } - - - // timestep the positions (given current velocities). + // timestep the positions (given current velocities). void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); + + // predict the positions + void predictPositionsMultiDof(btScalar dt); + // + // contacts + // - // - // contacts - // + // This routine fills out a contact constraint jacobian for this body. + // the 'normal' supplied must be -n for body1 or +n for body2 of the contact. + // 'normal' & 'contact_point' are both given in world coordinates. - // This routine fills out a contact constraint jacobian for this body. - // the 'normal' supplied must be -n for body1 or +n for body2 of the contact. - // 'normal' & 'contact_point' are both given in world coordinates. - void fillContactJacobianMultiDof(int link, - const btVector3 &contact_point, - const btVector3 &normal, - btScalar *jac, - btAlignedObjectArray<btScalar> &scratch_r, - btAlignedObjectArray<btVector3> &scratch_v, - btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); } + const btVector3 &contact_point, + const btVector3 &normal, + btScalar *jac, + btAlignedObjectArray<btScalar> &scratch_r, + btAlignedObjectArray<btVector3> &scratch_v, + btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); } //a more general version of fillContactJacobianMultiDof which does not assume.. //.. that the constraint in question is contact or, to be more precise, constrains linear velocity only void fillConstraintJacobianMultiDof(int link, - const btVector3 &contact_point, - const btVector3 &normal_ang, - const btVector3 &normal_lin, - btScalar *jac, - btAlignedObjectArray<btScalar> &scratch_r, - btAlignedObjectArray<btVector3> &scratch_v, - btAlignedObjectArray<btMatrix3x3> &scratch_m) const; - + const btVector3 &contact_point, + const btVector3 &normal_ang, + const btVector3 &normal_lin, + btScalar *jac, + btAlignedObjectArray<btScalar> &scratch_r, + btAlignedObjectArray<btVector3> &scratch_v, + btAlignedObjectArray<btMatrix3x3> &scratch_m) const; + + // + // sleeping + // + void setCanSleep(bool canSleep) + { + if (m_canWakeup) + { + m_canSleep = canSleep; + } + } - // - // sleeping - // - void setCanSleep(bool canSleep) + bool getCanSleep() const { - m_canSleep = canSleep; + return m_canSleep; } - bool getCanSleep()const + bool getCanWakeup() const { - return m_canSleep; + return m_canWakeup; } + + void setCanWakeup(bool canWakeup) + { + m_canWakeup = canWakeup; + } + bool isAwake() const { return m_awake; } + void wakeUp(); + void goToSleep(); + void checkMotionAndSleepIfRequired(btScalar timestep); - bool isAwake() const { return m_awake; } - void wakeUp(); - void goToSleep(); - void checkMotionAndSleepIfRequired(btScalar timestep); - bool hasFixedBase() const { - return m_fixedBase; + return m_fixedBase; + } + + void setFixedBase(bool fixedBase) + { + m_fixedBase = fixedBase; } int getCompanionId() const @@ -476,16 +540,16 @@ void addJointTorque(int i, btScalar Q); m_companionId = id; } - void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links + void setNumLinks(int numLinks) //careful: when changing the number of m_links, make sure to re-initialize or update existing m_links { m_links.resize(numLinks); } btScalar getLinearDamping() const { - return m_linearDamping; + return m_linearDamping; } - void setLinearDamping( btScalar damp) + void setLinearDamping(btScalar damp) { m_linearDamping = damp; } @@ -493,11 +557,11 @@ void addJointTorque(int i, btScalar Q); { return m_angularDamping; } - void setAngularDamping( btScalar damp) + void setAngularDamping(btScalar damp) { m_angularDamping = damp; } - + bool getUseGyroTerm() const { return m_useGyroTerm; @@ -506,24 +570,24 @@ void addJointTorque(int i, btScalar Q); { m_useGyroTerm = useGyro; } - btScalar getMaxCoordinateVelocity() const + btScalar getMaxCoordinateVelocity() const { - return m_maxCoordinateVelocity ; + return m_maxCoordinateVelocity; } - void setMaxCoordinateVelocity(btScalar maxVel) + void setMaxCoordinateVelocity(btScalar maxVel) { m_maxCoordinateVelocity = maxVel; } - btScalar getMaxAppliedImpulse() const + btScalar getMaxAppliedImpulse() const { return m_maxAppliedImpulse; } - void setMaxAppliedImpulse(btScalar maxImp) + void setMaxAppliedImpulse(btScalar maxImp) { m_maxAppliedImpulse = maxImp; } - void setHasSelfCollision(bool hasSelfCollision) + void setHasSelfCollision(bool hasSelfCollision) { m_hasSelfCollision = hasSelfCollision; } @@ -532,7 +596,6 @@ void addJointTorque(int i, btScalar Q); return m_hasSelfCollision; } - void finalizeMultiDof(); void useRK4Integration(bool use) { m_useRK4 = use; } @@ -548,213 +611,278 @@ void addJointTorque(int i, btScalar Q); { __posUpdated = updated; } - + //internalNeedsJointFeedback is for internal use only bool internalNeedsJointFeedback() const { return m_internalNeedsJointFeedback; } - void forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m); + void forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local, btAlignedObjectArray<btVector3> & local_origin); - void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m); - - virtual int calculateSerializeBufferSize() const; + void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const; + + void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin); + void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin); + + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; + virtual const char *serialize(void *dataBuffer, class btSerializer *serializer) const; - const char* getBaseName() const + const char *getBaseName() const { return m_baseName; } ///memory of setBaseName needs to be manager by user - void setBaseName(const char* name) + void setBaseName(const char *name) { m_baseName = name; } -private: - btMultiBody(const btMultiBody &); // not implemented - void operator=(const btMultiBody &); // not implemented + ///users can point to their objects, userPointer is not used by Bullet + void *getUserPointer() const + { + return m_userObjectPointer; + } - void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; + int getUserIndex() const + { + return m_userIndex; + } - void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const; - void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const; - - void updateLinksDofOffsets() + int getUserIndex2() const { - int dofOffset = 0, cfgOffset = 0; - for(int bidx = 0; bidx < m_links.size(); ++bidx) - { - m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset; - dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount; - } + return m_userIndex2; + } + ///users can point to their objects, userPointer is not used by Bullet + void setUserPointer(void *userPointer) + { + m_userObjectPointer = userPointer; } - void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; - - -private: + ///users can point to their objects, userPointer is not used by Bullet + void setUserIndex(int index) + { + m_userIndex = index; + } - btMultiBodyLinkCollider* m_baseCollider;//can be NULL - const char* m_baseName;//memory needs to be manager by user! + void setUserIndex2(int index) + { + m_userIndex2 = index; + } - btVector3 m_basePos; // position of COM of base (world frame) - btQuaternion m_baseQuat; // rotates world points into base frame + static void 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 - btScalar m_baseMass; // mass of the base - btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) - btVector3 m_baseForce; // external force applied to base. World frame. - btVector3 m_baseTorque; // external torque applied to base. World frame. - - btVector3 m_baseConstraintForce; // external force applied to base. World frame. - btVector3 m_baseConstraintTorque; // external torque applied to base. World frame. - - btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1. - btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders; - - // - // realBuf: - // offset size array - // 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized] - // 6+num_links num_links D - // - // vectorBuf: - // offset size array - // 0 num_links h_top - // num_links num_links h_bottom - // - // matrixBuf: - // offset size array - // 0 num_links+1 rot_from_parent - // - btAlignedObjectArray<btScalar> m_deltaV; - btAlignedObjectArray<btScalar> m_realBuf; - btAlignedObjectArray<btVector3> m_vectorBuf; - btAlignedObjectArray<btMatrix3x3> m_matrixBuf; +private: + btMultiBody(const btMultiBody &); // not implemented + void operator=(const btMultiBody &); // not implemented + void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const; + void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const; + + void updateLinksDofOffsets() + { + int dofOffset = 0, cfgOffset = 0; + for (int bidx = 0; bidx < m_links.size(); ++bidx) + { + m_links[bidx].m_dofOffset = dofOffset; + m_links[bidx].m_cfgOffset = cfgOffset; + dofOffset += m_links[bidx].m_dofCount; + cfgOffset += m_links[bidx].m_posVarCount; + } + } + + void mulMatrix(btScalar * pA, btScalar * pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; + +private: + btMultiBodyLinkCollider *m_baseCollider; //can be NULL + const char *m_baseName; //memory needs to be manager by user! + + btVector3 m_basePos; // position of COM of base (world frame) + btVector3 m_basePos_interpolate; // position of interpolated COM of base (world frame) + btQuaternion m_baseQuat; // rotates world points into base frame + btQuaternion m_baseQuat_interpolate; + + btScalar m_baseMass; // mass of the base + btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) + + btVector3 m_baseForce; // external force applied to base. World frame. + btVector3 m_baseTorque; // external torque applied to base. World frame. + + btVector3 m_baseConstraintForce; // external force applied to base. World frame. + btVector3 m_baseConstraintTorque; // external torque applied to base. World frame. + + btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1. + + // + // realBuf: + // offset size array + // 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized] + // 6+num_links num_links D + // + // vectorBuf: + // offset size array + // 0 num_links h_top + // num_links num_links h_bottom + // + // matrixBuf: + // offset size array + // 0 num_links+1 rot_from_parent + // + btAlignedObjectArray<btScalar> m_splitV; + btAlignedObjectArray<btScalar> m_deltaV; + btAlignedObjectArray<btScalar> m_realBuf; + btAlignedObjectArray<btVector3> m_vectorBuf; + btAlignedObjectArray<btMatrix3x3> m_matrixBuf; btMatrix3x3 m_cachedInertiaTopLeft; btMatrix3x3 m_cachedInertiaTopRight; btMatrix3x3 m_cachedInertiaLowerLeft; btMatrix3x3 m_cachedInertiaLowerRight; + bool m_cachedInertiaValid; - bool m_fixedBase; + bool m_fixedBase; - // Sleep parameters. - bool m_awake; - bool m_canSleep; - btScalar m_sleepTimer; + // Sleep parameters. + bool m_awake; + bool m_canSleep; + bool m_canWakeup; + btScalar m_sleepTimer; + + void *m_userObjectPointer; + int m_userIndex2; + int m_userIndex; + + int m_companionId; + btScalar m_linearDamping; + btScalar m_angularDamping; + bool m_useGyroTerm; + btScalar m_maxAppliedImpulse; + btScalar m_maxCoordinateVelocity; + bool m_hasSelfCollision; + + bool __posUpdated; + int m_dofCount, m_posVarCnt; - int m_companionId; - btScalar m_linearDamping; - btScalar m_angularDamping; - bool m_useGyroTerm; - btScalar m_maxAppliedImpulse; - btScalar m_maxCoordinateVelocity; - bool m_hasSelfCollision; - - bool __posUpdated; - int m_dofCount, m_posVarCnt; bool m_useRK4, m_useGlobalVelocities; - + //for global velocities, see 8.3.2B Proposed resolution in Jakub Stepien PhD Thesis + //https://drive.google.com/file/d/0Bz3vEa19XOYGNWdZWGpMdUdqVmZ5ZVBOaEh4ZnpNaUxxZFNV/view?usp=sharing + ///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only bool m_internalNeedsJointFeedback; }; struct btMultiBodyLinkDoubleData { - btQuaternionDoubleData m_zeroRotParentToThis; - btVector3DoubleData m_parentComToThisComOffset; - btVector3DoubleData m_thisPivotToThisComOffset; - btVector3DoubleData m_jointAxisTop[6]; - btVector3DoubleData m_jointAxisBottom[6]; - - - char *m_linkName; - char *m_jointName; - btCollisionObjectDoubleData *m_linkCollider; - - btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal) - double m_linkMass; - int m_parentIndex; - int m_jointType; - - - - - int m_dofCount; - int m_posVarCount; - double m_jointPos[7]; - double m_jointVel[6]; - double m_jointTorque[6]; - - - + btQuaternionDoubleData m_zeroRotParentToThis; + btVector3DoubleData m_parentComToThisPivotOffset; + btVector3DoubleData m_thisPivotToThisComOffset; + btVector3DoubleData m_jointAxisTop[6]; + btVector3DoubleData m_jointAxisBottom[6]; + + btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal) + btVector3DoubleData m_absFrameTotVelocityTop; + btVector3DoubleData m_absFrameTotVelocityBottom; + btVector3DoubleData m_absFrameLocVelocityTop; + btVector3DoubleData m_absFrameLocVelocityBottom; + + double m_linkMass; + int m_parentIndex; + int m_jointType; + + int m_dofCount; + int m_posVarCount; + double m_jointPos[7]; + double m_jointVel[6]; + double m_jointTorque[6]; + + double m_jointDamping; + double m_jointFriction; + double m_jointLowerLimit; + double m_jointUpperLimit; + double m_jointMaxForce; + double m_jointMaxVelocity; + + char *m_linkName; + char *m_jointName; + btCollisionObjectDoubleData *m_linkCollider; + char *m_paddingPtr; }; struct btMultiBodyLinkFloatData { - btQuaternionFloatData m_zeroRotParentToThis; - btVector3FloatData m_parentComToThisComOffset; - btVector3FloatData m_thisPivotToThisComOffset; - btVector3FloatData m_jointAxisTop[6]; - btVector3FloatData m_jointAxisBottom[6]; - - - char *m_linkName; - char *m_jointName; - btCollisionObjectFloatData *m_linkCollider; - - btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal) - int m_dofCount; - float m_linkMass; - int m_parentIndex; - int m_jointType; - - - - float m_jointPos[7]; - float m_jointVel[6]; - float m_jointTorque[6]; - int m_posVarCount; - - + btQuaternionFloatData m_zeroRotParentToThis; + btVector3FloatData m_parentComToThisPivotOffset; + btVector3FloatData m_thisPivotToThisComOffset; + btVector3FloatData m_jointAxisTop[6]; + btVector3FloatData m_jointAxisBottom[6]; + btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal) + btVector3FloatData m_absFrameTotVelocityTop; + btVector3FloatData m_absFrameTotVelocityBottom; + btVector3FloatData m_absFrameLocVelocityTop; + btVector3FloatData m_absFrameLocVelocityBottom; + + int m_dofCount; + float m_linkMass; + int m_parentIndex; + int m_jointType; + + float m_jointPos[7]; + float m_jointVel[6]; + float m_jointTorque[6]; + int m_posVarCount; + float m_jointDamping; + float m_jointFriction; + float m_jointLowerLimit; + float m_jointUpperLimit; + float m_jointMaxForce; + float m_jointMaxVelocity; + + char *m_linkName; + char *m_jointName; + btCollisionObjectFloatData *m_linkCollider; + char *m_paddingPtr; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btMultiBodyDoubleData +struct btMultiBodyDoubleData { - char *m_baseName; - btMultiBodyLinkDoubleData *m_links; - btCollisionObjectDoubleData *m_baseCollider; - - btTransformDoubleData m_baseWorldTransform; - btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal) - - int m_numLinks; - double m_baseMass; - + btVector3DoubleData m_baseWorldPosition; + btQuaternionDoubleData m_baseWorldOrientation; + btVector3DoubleData m_baseLinearVelocity; + btVector3DoubleData m_baseAngularVelocity; + btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal) + double m_baseMass; + int m_numLinks; char m_padding[4]; - + + char *m_baseName; + btMultiBodyLinkDoubleData *m_links; + btCollisionObjectDoubleData *m_baseCollider; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btMultiBodyFloatData +struct btMultiBodyFloatData { - char *m_baseName; - btMultiBodyLinkFloatData *m_links; - btCollisionObjectFloatData *m_baseCollider; - btTransformFloatData m_baseWorldTransform; - btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal) - - float m_baseMass; - int m_numLinks; + btVector3FloatData m_baseWorldPosition; + btQuaternionFloatData m_baseWorldOrientation; + btVector3FloatData m_baseLinearVelocity; + btVector3FloatData m_baseAngularVelocity; + + btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal) + float m_baseMass; + int m_numLinks; + + char *m_baseName; + btMultiBodyLinkFloatData *m_links; + btCollisionObjectFloatData *m_baseCollider; }; - - #endif |