870 lines
26 KiB
C++
870 lines
26 KiB
C++
/*
|
|
* PURPOSE:
|
|
* Class representing an articulated rigid body. Stores the body's
|
|
* current state, allows forces and torques to be set, handles
|
|
* timestepping and implements Featherstone's algorithm.
|
|
*
|
|
* COPYRIGHT:
|
|
* Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
|
|
* Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
|
|
* Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
|
|
*/
|
|
|
|
#ifndef BT_MULTIBODY_H
|
|
#define BT_MULTIBODY_H
|
|
|
|
#include "LinearMath/btScalar.h"
|
|
#include "LinearMath/btVector3.h"
|
|
#include "LinearMath/btQuaternion.h"
|
|
#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"
|
|
#else
|
|
#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
|
|
{
|
|
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);
|
|
|
|
virtual ~btMultiBody();
|
|
|
|
//note: fixed link collision with parent is always disabled
|
|
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 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)
|
|
{
|
|
return m_links[index];
|
|
}
|
|
|
|
void setBaseCollider(btMultiBodyLinkCollider * collider) //collider can be NULL to disable collision for the base
|
|
{
|
|
m_baseCollider = collider;
|
|
}
|
|
const btMultiBodyLinkCollider *getBaseCollider() const
|
|
{
|
|
return m_baseCollider;
|
|
}
|
|
btMultiBodyLinkCollider *getBaseCollider()
|
|
{
|
|
return m_baseCollider;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
//
|
|
// 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)
|
|
//
|
|
|
|
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
|
|
//
|
|
|
|
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
|
|
{
|
|
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;
|
|
m_basePos_interpolate = 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)
|
|
{
|
|
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!?
|
|
m_baseQuat_interpolate = rot;
|
|
}
|
|
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)
|
|
//
|
|
|
|
btScalar getJointPos(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 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];
|
|
}
|
|
|
|
const btScalar *getDeltaVelocityVector() const
|
|
{
|
|
return &m_deltaV[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)
|
|
//
|
|
|
|
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 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.
|
|
//
|
|
|
|
void clearForcesAndTorques();
|
|
void clearConstraintForces();
|
|
|
|
void clearVelocities();
|
|
|
|
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 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 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,
|
|
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)
|
|
{
|
|
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
|
|
{
|
|
m_deltaV[dof] += delta_vee[dof] * multiplier;
|
|
}
|
|
}
|
|
void processDeltaVeeMultiDof2()
|
|
{
|
|
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)
|
|
{
|
|
//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
|
|
// printf("%.4f ", delta_vee[dof]*multiplier);
|
|
//printf("\n");
|
|
|
|
//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)
|
|
{
|
|
m_realBuf[dof] += delta_vee[dof] * multiplier;
|
|
btClamp(m_realBuf[dof], -m_maxCoordinateVelocity, m_maxCoordinateVelocity);
|
|
}
|
|
}
|
|
|
|
// timestep the positions (given current velocities).
|
|
void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
|
|
|
|
// predict the positions
|
|
void predictPositionsMultiDof(btScalar dt);
|
|
|
|
//
|
|
// 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.
|
|
|
|
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); }
|
|
|
|
//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;
|
|
|
|
//
|
|
// sleeping
|
|
//
|
|
void setCanSleep(bool canSleep)
|
|
{
|
|
if (m_canWakeup)
|
|
{
|
|
m_canSleep = canSleep;
|
|
}
|
|
}
|
|
|
|
bool getCanSleep() const
|
|
{
|
|
return m_canSleep;
|
|
}
|
|
|
|
bool getCanWakeup() const
|
|
{
|
|
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 hasFixedBase() const
|
|
{
|
|
return m_fixedBase;
|
|
}
|
|
|
|
void setFixedBase(bool fixedBase)
|
|
{
|
|
m_fixedBase = fixedBase;
|
|
}
|
|
|
|
int getCompanionId() const
|
|
{
|
|
return m_companionId;
|
|
}
|
|
void setCompanionId(int id)
|
|
{
|
|
//printf("for %p setCompanionId(%d)\n",this, id);
|
|
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
|
|
{
|
|
m_links.resize(numLinks);
|
|
}
|
|
|
|
btScalar getLinearDamping() const
|
|
{
|
|
return m_linearDamping;
|
|
}
|
|
void setLinearDamping(btScalar damp)
|
|
{
|
|
m_linearDamping = damp;
|
|
}
|
|
btScalar getAngularDamping() const
|
|
{
|
|
return m_angularDamping;
|
|
}
|
|
void setAngularDamping(btScalar damp)
|
|
{
|
|
m_angularDamping = damp;
|
|
}
|
|
|
|
bool getUseGyroTerm() const
|
|
{
|
|
return m_useGyroTerm;
|
|
}
|
|
void setUseGyroTerm(bool useGyro)
|
|
{
|
|
m_useGyroTerm = useGyro;
|
|
}
|
|
btScalar getMaxCoordinateVelocity() const
|
|
{
|
|
return m_maxCoordinateVelocity;
|
|
}
|
|
void setMaxCoordinateVelocity(btScalar maxVel)
|
|
{
|
|
m_maxCoordinateVelocity = maxVel;
|
|
}
|
|
|
|
btScalar getMaxAppliedImpulse() const
|
|
{
|
|
return m_maxAppliedImpulse;
|
|
}
|
|
void setMaxAppliedImpulse(btScalar maxImp)
|
|
{
|
|
m_maxAppliedImpulse = maxImp;
|
|
}
|
|
void setHasSelfCollision(bool hasSelfCollision)
|
|
{
|
|
m_hasSelfCollision = hasSelfCollision;
|
|
}
|
|
bool hasSelfCollision() const
|
|
{
|
|
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>& world_to_local, btAlignedObjectArray<btVector3> & local_origin);
|
|
|
|
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;
|
|
|
|
const char *getBaseName() const
|
|
{
|
|
return m_baseName;
|
|
}
|
|
///memory of setBaseName needs to be manager by user
|
|
void setBaseName(const char *name)
|
|
{
|
|
m_baseName = name;
|
|
}
|
|
|
|
///users can point to their objects, userPointer is not used by Bullet
|
|
void *getUserPointer() const
|
|
{
|
|
return m_userObjectPointer;
|
|
}
|
|
|
|
int getUserIndex() const
|
|
{
|
|
return m_userIndex;
|
|
}
|
|
|
|
int getUserIndex2() const
|
|
{
|
|
return m_userIndex2;
|
|
}
|
|
///users can point to their objects, userPointer is not used by Bullet
|
|
void setUserPointer(void *userPointer)
|
|
{
|
|
m_userObjectPointer = userPointer;
|
|
}
|
|
|
|
///users can point to their objects, userPointer is not used by Bullet
|
|
void setUserIndex(int index)
|
|
{
|
|
m_userIndex = index;
|
|
}
|
|
|
|
void setUserIndex2(int index)
|
|
{
|
|
m_userIndex2 = index;
|
|
}
|
|
|
|
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
|
|
|
|
|
|
|
|
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_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;
|
|
|
|
// 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;
|
|
|
|
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_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_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
|
|
{
|
|
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
|
|
{
|
|
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
|