Page MenuHome
Differential D1739 Diff 5899 extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h

Changeset View

Standalone View

View Options

extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h

/* /*
* PURPOSE: * PURPOSE:
* Class representing an articulated rigid body. Stores the body's * Class representing an articulated rigid body. Stores the body's
* current state, allows forces and torques to be set, handles * current state, allows forces and torques to be set, handles
* timestepping and implements Featherstone's algorithm. * timestepping and implements Featherstone's algorithm.
* *
* COPYRIGHT: * COPYRIGHT:
* Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013 * Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
* Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) * 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. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef BT_MULTIBODY_H #ifndef BT_MULTIBODY_H
#define BT_MULTIBODY_H #define BT_MULTIBODY_H
#include "LinearMath/btScalar.h" #include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
#include "LinearMath/btQuaternion.h" #include "LinearMath/btQuaternion.h"
#include "LinearMath/btMatrix3x3.h" #include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms
#ifdef BT_USE_DOUBLE_PRECISION
#define btMultiBodyData btMultiBodyDoubleData
#define btMultiBodyDataName "btMultiBodyDoubleData"
#define btMultiBodyLinkData btMultiBodyLinkDoubleData
#define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData"
#else
#define btMultiBodyData btMultiBodyFloatData
#define btMultiBodyDataName "btMultiBodyFloatData"
#define btMultiBodyLinkData btMultiBodyLinkFloatData
#define btMultiBodyLinkDataName "btMultiBodyLinkFloatData"
#endif //BT_USE_DOUBLE_PRECISION
#include "btMultiBodyLink.h" #include "btMultiBodyLink.h"
class btMultiBodyLinkCollider; class btMultiBodyLinkCollider;
class btMultiBody ATTRIBUTE_ALIGNED16(class) btMultiBody
{ {
public: public:
BT_DECLARE_ALIGNED_ALLOCATOR(); BT_DECLARE_ALIGNED_ALLOCATOR();
// //
// initialization // initialization
// //
btMultiBody(int n_links, // NOT including the base btMultiBody(int n_links, // NOT including the base
btScalar mass, // mass of base btScalar mass, // mass of base
const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal
bool fixed_base_, // whether the base is fixed (true) or can move (false) bool fixedBase, // whether the base is fixed (true) or can move (false)
bool can_sleep_); bool canSleep, bool deprecatedMultiDof=true);
~btMultiBody(); virtual ~btMultiBody();
void setupPrismatic(int i, // 0 to num_links-1 //note: fixed link collision with parent is always disabled
void setupFixed(int linkIndex,
btScalar mass, btScalar mass,
const btVector3 &inertia, // in my frame; assumed diagonal const btVector3 &inertia,
int parent, int parent,
const btQuaternion &rot_parent_to_this, // rotate points in parent frame to my frame. const btQuaternion &rotParentToThis,
const btVector3 &joint_axis, // in my frame const btVector3 &parentComToThisPivotOffset,
const btVector3 &r_vector_when_q_zero, // vector from parent COM to my COM, in my frame, when q = 0. const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true);
bool disableParentCollision=false
);
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 setupRevolute(int i, // 0 to num_links-1 void setupPlanar(int i, // 0 to num_links-1
btScalar mass, btScalar mass,
const btVector3 &inertia, const btVector3 &inertia,
int parent, int parent,
const btQuaternion &zero_rot_parent_to_this, // rotate points in parent frame to this frame, when q = 0 const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
const btVector3 &joint_axis, // in my frame const btVector3 &rotationAxis,
const btVector3 &parent_axis_position, // vector from parent COM to joint axis, in PARENT frame const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame
const btVector3 &my_axis_position, // vector from joint axis to my COM, in MY frame
bool disableParentCollision=false); bool disableParentCollision=false);
const btMultibodyLink& getLink(int index) const const btMultibodyLink& getLink(int index) const
{ {
return links[index]; return m_links[index];
} }
btMultibodyLink& getLink(int index) btMultibodyLink& getLink(int index)
{ {
return links[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; m_baseCollider = collider;
} }
const btMultiBodyLinkCollider* getBaseCollider() const const btMultiBodyLinkCollider* getBaseCollider() const
Show All 9 Lines btMultiBodyLinkCollider* getBaseCollider()
// get parent // get parent
// input: link num from 0 to num_links-1 // input: link num from 0 to num_links-1
// output: link num from 0 to num_links-1, OR -1 to mean the base. // output: link num from 0 to num_links-1, OR -1 to mean the base.
// //
int getParent(int link_num) const; int getParent(int link_num) const;
// //
// get number of links, masses, moments of inertia // get number of m_links, masses, moments of inertia
// //
int getNumLinks() const { return links.size(); } int getNumLinks() const { return m_links.size(); }
btScalar getBaseMass() const { return base_mass; } int getNumDofs() const { return m_dofCount; }
const btVector3 & getBaseInertia() const { return base_inertia; } int getNumPosVars() const { return m_posVarCnt; }
btScalar getBaseMass() const { return m_baseMass; }
const btVector3 & getBaseInertia() const { return m_baseInertia; }
btScalar getLinkMass(int i) const; btScalar getLinkMass(int i) const;
const btVector3 & getLinkInertia(int i) const; const btVector3 & getLinkInertia(int i) const;
// //
// change mass (incomplete: can only change base mass and inertia at present) // change mass (incomplete: can only change base mass and inertia at present)
// //
void setBaseMass(btScalar mass) { base_mass = mass; } void setBaseMass(btScalar mass) { m_baseMass = mass; }
void setBaseInertia(const btVector3 &inertia) { base_inertia = inertia; } 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 base_pos; } // in world frame const btVector3 & getBasePos() const { return m_basePos; } // in world frame
const btVector3 getBaseVel() const const btVector3 getBaseVel() const
{ {
return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]); return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]);
} // in world frame } // in world frame
const btQuaternion & getWorldToBaseRot() const const btQuaternion & getWorldToBaseRot() const
{ {
return base_quat; return m_baseQuat;
} // rotates world vectors into base frame } // rotates world vectors into base frame
btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); } // in world frame btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); } // in world frame
void setBasePos(const btVector3 &pos) void setBasePos(const btVector3 &pos)
{ {
base_pos = pos; m_basePos = pos;
}
void setBaseWorldTransform(const btTransform& tr)
{
setBasePos(tr.getOrigin());
setWorldToBaseRot(tr.getRotation().inverse());
} }
btTransform getBaseWorldTransform() const
{
btTransform tr;
tr.setOrigin(getBasePos());
tr.setRotation(getWorldToBaseRot().inverse());
return tr;
}
void setBaseVel(const btVector3 &vel) void setBaseVel(const btVector3 &vel)
{ {
m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2]; m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2];
} }
void setWorldToBaseRot(const btQuaternion &rot) void setWorldToBaseRot(const btQuaternion &rot)
{ {
base_quat = rot; m_baseQuat = rot; //m_baseQuat asumed to ba alias!?
} }
void setBaseOmega(const btVector3 &omega) void setBaseOmega(const btVector3 &omega)
{ {
m_real_buf[0]=omega[0]; m_realBuf[0]=omega[0];
m_real_buf[1]=omega[1]; m_realBuf[1]=omega[1];
m_real_buf[2]=omega[2]; m_realBuf[2]=omega[2];
} }
// //
// get/set pos/vel for child 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 getJointPos(int i) const;
btScalar getJointVel(int i) const; btScalar getJointVel(int i) const;
btScalar * getJointVelMultiDof(int i);
btScalar * getJointPosMultiDof(int i);
const btScalar * getJointVelMultiDof(int i) const ;
const btScalar * getJointPosMultiDof(int i) const ;
void setJointPos(int i, btScalar q); void setJointPos(int i, btScalar q);
void setJointVel(int i, btScalar qdot); void setJointVel(int i, btScalar qdot);
void setJointPosMultiDof(int i, btScalar *q);
void setJointVelMultiDof(int i, btScalar *qdot);
// //
// direct access to velocities as a vector of 6 + num_links elements. // direct access to velocities as a vector of 6 + num_links elements.
// (omega first, then v, then joint velocities.) // (omega first, then v, then joint velocities.)
// //
const btScalar * getVelocityVector() const const btScalar * getVelocityVector() const
{ {
return &m_real_buf[0]; return &m_realBuf[0];
} }
/* btScalar * getVelocityVector() /* btScalar * getVelocityVector()
{ {
return &real_buf[0]; return &real_buf[0];
} }
*/ */
// //
// get the frames of reference (positions and orientations) of the child links // get the frames of reference (positions and orientations) of the child m_links
// (i = 0 to num_links-1) // (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 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 btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.
// //
Show All 14 Lines */
btVector3 getAngularMomentum() 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 clearForcesAndTorques();
void clearConstraintForces();
void clearVelocities(); void clearVelocities();
void addBaseForce(const btVector3 &f) void addBaseForce(const btVector3 &f)
{ {
base_force += f; m_baseForce += f;
} }
void addBaseTorque(const btVector3 &t) { base_torque += t; } void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
void addLinkForce(int i, const btVector3 &f); void addLinkForce(int i, const btVector3 &f);
void addLinkTorque(int i, const btVector3 &t); void addLinkTorque(int i, const btVector3 &t);
void addBaseConstraintForce(const btVector3 &f)
{
m_baseConstraintForce += f;
}
void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; }
void addLinkConstraintForce(int i, const btVector3 &f);
void addLinkConstraintTorque(int i, const btVector3 &t);
void addJointTorque(int i, btScalar Q); void 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 base_force; } const btVector3 & getBaseForce() const { return m_baseForce; }
const btVector3 & getBaseTorque() const { return base_torque; } const btVector3 & getBaseTorque() const { return m_baseTorque; }
const btVector3 & getLinkForce(int i) const; const btVector3 & getLinkForce(int i) const;
const btVector3 & getLinkTorque(int i) const; const btVector3 & getLinkTorque(int i) const;
btScalar getJointTorque(int i) const; btScalar getJointTorque(int i) const;
btScalar * getJointTorqueMultiDof(int i);
// //
// dynamics routines. // dynamics routines.
// //
// timestep the velocities (given the external forces/torques set using addBaseForce etc). // timestep the velocities (given the external forces/torques set using addBaseForce etc).
// also sets up caches for calcAccelerationDeltas. // also sets up caches for calcAccelerationDeltas.
// //
// Note: the caller must provide three vectors which are used as // Note: the caller must provide three vectors which are used as
// temporary scratch space. The idea here is to reduce dynamic // temporary scratch space. The idea here is to reduce dynamic
// memory allocation: the same scratch vectors can be re-used // memory allocation: the same scratch vectors can be re-used
// again and again for different Multibodies, instead of each // again and again for different Multibodies, instead of each
// btMultiBody allocating (and then deallocating) their own // btMultiBody allocating (and then deallocating) their own
// individual scratch buffers. This gives a considerable speed // individual scratch buffers. This gives a considerable speed
// improvement, at least on Windows (where dynamic memory // improvement, at least on Windows (where dynamic memory
// allocation appears to be fairly slow). // allocation appears to be fairly slow).
// //
void stepVelocities(btScalar dt,
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v, btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m); 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);
}
// calcAccelerationDeltas // calcAccelerationDeltasMultiDof
// input: force vector (in same format as jacobian, i.e.: // input: force vector (in same format as jacobian, i.e.:
// 3 torque values, 3 force values, num_links joint torque values) // 3 torque values, 3 force values, num_links joint torque values)
// output: 3 omegadot values, 3 vdot values, num_links q_double_dot values // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
// (existing contents of output array are replaced) // (existing contents of output array are replaced)
// stepVelocities must have been called first. // calcAccelerationDeltasMultiDof must have been called first.
void calcAccelerationDeltas(const btScalar *force, btScalar *output, void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v) const; btAlignedObjectArray<btVector3> &scratch_v) const;
// apply a delta-vee directly. used in sequential impulses code.
void applyDeltaVee(const btScalar * delta_vee)
{
for (int i = 0; i < 6 + getNumLinks(); ++i) void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier)
{
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{ {
m_real_buf[i] += delta_vee[i]; m_deltaV[dof] += delta_vee[dof] * multiplier;
} }
} }
void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) void processDeltaVeeMultiDof2()
{ {
btScalar sum = 0; applyDeltaVeeMultiDof(&m_deltaV[0],1);
for (int i = 0; i < 6 + getNumLinks(); ++i)
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{ {
sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; m_deltaV[dof] = 0.f;
} }
btScalar l = btSqrt(sum);
/*
static btScalar maxl = -1e30f;
if (l>maxl)
{
maxl=l;
// printf("maxl=%f\n",maxl);
} }
*/
if (l>m_maxAppliedImpulse) void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier)
{ {
// printf("exceeds 100: l=%f\n",maxl); //for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
multiplier *= m_maxAppliedImpulse/l; // printf("%.4f ", delta_vee[dof]*multiplier);
} //printf("\n");
for (int i = 0; i < 6 + getNumLinks(); ++i) //btScalar sum = 0;
//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
//{
// sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
//}
//btScalar l = btSqrt(sum);
//if (l>m_maxAppliedImpulse)
//{
// multiplier *= m_maxAppliedImpulse/l;
//}
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{ {
sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; m_realBuf[dof] += delta_vee[dof] * multiplier;
m_real_buf[i] += delta_vee[i] * multiplier; btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity);
} }
} }
// timestep the positions (given current velocities). // timestep the positions (given current velocities).
void stepPositions(btScalar dt); void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
// //
// contacts // contacts
// //
// This routine fills out a contact constraint jacobian for this body. // 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. // the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
// 'normal' & 'contact_point' are both given in world coordinates. // 'normal' & 'contact_point' are both given in world coordinates.
void fillContactJacobian(int link,
void fillContactJacobianMultiDof(int link,
const btVector3 &contact_point, const btVector3 &contact_point,
const btVector3 &normal, const btVector3 &normal,
btScalar *jac, btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v, 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; btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
// //
// sleeping // sleeping
// //
void setCanSleep(bool canSleep) void setCanSleep(bool canSleep)
{ {
can_sleep = canSleep; m_canSleep = canSleep;
}
bool getCanSleep()const
{
return m_canSleep;
} }
bool isAwake() const { return awake; } bool isAwake() const { return m_awake; }
void wakeUp(); void wakeUp();
void goToSleep(); void goToSleep();
void checkMotionAndSleepIfRequired(btScalar timestep); void checkMotionAndSleepIfRequired(btScalar timestep);
bool hasFixedBase() const bool hasFixedBase() const
{ {
return fixed_base; return m_fixedBase;
} }
int getCompanionId() const int getCompanionId() const
{ {
return m_companionId; return m_companionId;
} }
void setCompanionId(int id) void setCompanionId(int id)
{ {
//printf("for %p setCompanionId(%d)\n",this, id); //printf("for %p setCompanionId(%d)\n",this, id);
m_companionId = id; m_companionId = id;
} }
void setNumLinks(int numLinks)//careful: when changing the number of links, make sure to re-initialize or update existing links void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
{ {
links.resize(numLinks); m_links.resize(numLinks);
} }
btScalar getLinearDamping() const btScalar getLinearDamping() const
{ {
return m_linearDamping; return m_linearDamping;
} }
void setLinearDamping( btScalar damp) void setLinearDamping( btScalar damp)
{ {
m_linearDamping = damp; m_linearDamping = damp;
} }
btScalar getAngularDamping() const btScalar getAngularDamping() const
{ {
return m_angularDamping; return m_angularDamping;
} }
void setAngularDamping( btScalar damp)
{
m_angularDamping = damp;
}
bool getUseGyroTerm() const bool getUseGyroTerm() const
{ {
return m_useGyroTerm; return m_useGyroTerm;
} }
void setUseGyroTerm(bool useGyro) void setUseGyroTerm(bool useGyro)
{ {
m_useGyroTerm = useGyro; m_useGyroTerm = useGyro;
} }
btScalar getMaxCoordinateVelocity() const
{
return m_maxCoordinateVelocity ;
}
void setMaxCoordinateVelocity(btScalar maxVel)
{
m_maxCoordinateVelocity = maxVel;
}
btScalar getMaxAppliedImpulse() const btScalar getMaxAppliedImpulse() const
{ {
return m_maxAppliedImpulse; return m_maxAppliedImpulse;
} }
void setMaxAppliedImpulse(btScalar maxImp) void setMaxAppliedImpulse(btScalar maxImp)
{ {
m_maxAppliedImpulse = maxImp; m_maxAppliedImpulse = maxImp;
} }
void setHasSelfCollision(bool hasSelfCollision) void setHasSelfCollision(bool hasSelfCollision)
{ {
m_hasSelfCollision = hasSelfCollision; m_hasSelfCollision = hasSelfCollision;
} }
bool hasSelfCollision() const bool hasSelfCollision() const
{ {
return m_hasSelfCollision; return m_hasSelfCollision;
} }
void finalizeMultiDof();
void useRK4Integration(bool use) { m_useRK4 = use; }
bool isUsingRK4Integration() const { return m_useRK4; }
void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; }
bool isPosUpdated() const
{
return __posUpdated;
}
void setPosUpdated(bool updated)
{
__posUpdated = updated;
}
//internalNeedsJointFeedback is for internal use only
bool internalNeedsJointFeedback() const
{
return m_internalNeedsJointFeedback;
}
void forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
const char* getBaseName() const
{
return m_baseName;
}
///memory of setBaseName needs to be manager by user
void setBaseName(const char* name)
{
m_baseName = name;
}
private: private:
btMultiBody(const btMultiBody &); // not implemented btMultiBody(const btMultiBody &); // not implemented
void operator=(const btMultiBody &); // not implemented void operator=(const btMultiBody &); // not implemented
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const; 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 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: private:
btMultiBodyLinkCollider* m_baseCollider;//can be NULL btMultiBodyLinkCollider* m_baseCollider;//can be NULL
const char* m_baseName;//memory needs to be manager by user!
btVector3 m_basePos; // position of COM of base (world frame)
btQuaternion m_baseQuat; // rotates world points into base frame
btVector3 base_pos; // position of COM of base (world frame) btScalar m_baseMass; // mass of the base
btQuaternion base_quat; // rotates world points into base frame btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal)
btScalar base_mass; // mass of the base btVector3 m_baseForce; // external force applied to base. World frame.
btVector3 base_inertia; // inertia of the base (in local frame; diagonal) btVector3 m_baseTorque; // external torque applied to base. World frame.
btVector3 base_force; // external force applied to base. World frame. btVector3 m_baseConstraintForce; // external force applied to base. World frame.
btVector3 base_torque; // external torque applied to base. World frame. btVector3 m_baseConstraintTorque; // external torque applied to base. World frame.
btAlignedObjectArray<btMultibodyLink> links; // array of links, excluding the base. index from 0 to num_links-1. btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1.
btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders; btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
// //
// real_buf: // realBuf:
// offset size array // offset size array
// 0 6 + num_links v (base_omega; base_vel; joint_vels) // 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 // 6+num_links num_links D
// //
// vector_buf: // vectorBuf:
// offset size array // offset size array
// 0 num_links h_top // 0 num_links h_top
// num_links num_links h_bottom // num_links num_links h_bottom
// //
// matrix_buf: // matrixBuf:
// offset size array // offset size array
// 0 num_links+1 rot_from_parent // 0 num_links+1 rot_from_parent
// //
btAlignedObjectArray<btScalar> m_deltaV;
btAlignedObjectArray<btScalar> m_realBuf;
btAlignedObjectArray<btVector3> m_vectorBuf;
btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
btAlignedObjectArray<btScalar> m_real_buf; btMatrix3x3 m_cachedInertiaTopLeft;
btAlignedObjectArray<btVector3> vector_buf; btMatrix3x3 m_cachedInertiaTopRight;
btAlignedObjectArray<btMatrix3x3> matrix_buf; btMatrix3x3 m_cachedInertiaLowerLeft;
btMatrix3x3 m_cachedInertiaLowerRight;
//std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
btMatrix3x3 cached_inertia_top_left;
btMatrix3x3 cached_inertia_top_right;
btMatrix3x3 cached_inertia_lower_left;
btMatrix3x3 cached_inertia_lower_right;
bool fixed_base; bool m_fixedBase;
// Sleep parameters. // Sleep parameters.
bool awake; bool m_awake;
bool can_sleep; bool m_canSleep;
btScalar sleep_timer; btScalar m_sleepTimer;
int m_companionId; int m_companionId;
btScalar m_linearDamping; btScalar m_linearDamping;
btScalar m_angularDamping; btScalar m_angularDamping;
bool m_useGyroTerm; bool m_useGyroTerm;
btScalar m_maxAppliedImpulse; btScalar m_maxAppliedImpulse;
btScalar m_maxCoordinateVelocity;
bool m_hasSelfCollision; bool m_hasSelfCollision;
bool __posUpdated;
int m_dofCount, m_posVarCnt;
bool m_useRK4, m_useGlobalVelocities;
///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
bool m_internalNeedsJointFeedback;
}; };
struct btMultiBodyLinkDoubleData
{
btQuaternionDoubleData m_zeroRotParentToThis;
btVector3DoubleData m_parentComToThisComOffset;
btVector3DoubleData m_thisPivotToThisComOffset;
btVector3DoubleData m_jointAxisTop[6];
btVector3DoubleData m_jointAxisBottom[6];
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];
};
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;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
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;
char m_padding[4];
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btMultiBodyFloatData
{
char *m_baseName;
btMultiBodyLinkFloatData *m_links;
btCollisionObjectFloatData *m_baseCollider;
btTransformFloatData m_baseWorldTransform;
btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal)
float m_baseMass;
int m_numLinks;
};
#endif #endif