Update Files

This commit is contained in:
2025-01-22 16:18:30 +01:00
parent ed4603cf95
commit a36294b518
16718 changed files with 2960346 additions and 0 deletions

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,814 @@
/*
* 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 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
{
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;
}
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;
} // 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 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!?
}
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, btScalar *q);
void setJointVelMultiDof(int i, btScalar *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];
}
/* 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.
//
// 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 &mat) 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=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);
}
// 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);
//
// 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)
{
m_canSleep = canSleep;
}
bool getCanSleep()const
{
return m_canSleep;
}
bool isAwake() const { return m_awake; }
void wakeUp();
void goToSleep();
void checkMotionAndSleepIfRequired(btScalar timestep);
bool hasFixedBase() const
{
return m_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>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
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;
}
///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;
}
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)
btQuaternion m_baseQuat; // rotates world points into base frame
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;
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;
///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];
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];
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];
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;
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
{
btTransformDoubleData m_baseWorldTransform;
btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal)
double m_baseMass;
char *m_baseName;
btMultiBodyLinkDoubleData *m_links;
btCollisionObjectDoubleData *m_baseCollider;
char *m_paddingPtr;
int m_numLinks;
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

View File

@ -0,0 +1,417 @@
#include "btMultiBodyConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
:m_bodyA(bodyA),
m_bodyB(bodyB),
m_linkA(linkA),
m_linkB(linkB),
m_numRows(numRows),
m_jacSizeA(0),
m_jacSizeBoth(0),
m_isUnilateral(isUnilateral),
m_numDofsFinalized(-1),
m_maxAppliedImpulse(100)
{
}
void btMultiBodyConstraint::updateJacobianSizes()
{
if(m_bodyA)
{
m_jacSizeA = (6 + m_bodyA->getNumDofs());
}
if(m_bodyB)
{
m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
}
else
m_jacSizeBoth = m_jacSizeA;
}
void btMultiBodyConstraint::allocateJacobiansMultiDof()
{
updateJacobianSizes();
m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
m_data.resize((2 + m_jacSizeBoth) * m_numRows);
}
btMultiBodyConstraint::~btMultiBodyConstraint()
{
}
void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
{
for (int i = 0; i < ndof; ++i)
data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
}
btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
btScalar* jacOrgA, btScalar* jacOrgB,
const btVector3& constraintNormalAng,
const btVector3& constraintNormalLin,
const btVector3& posAworld, const btVector3& posBworld,
btScalar posError,
const btContactSolverInfo& infoGlobal,
btScalar lowerLimit, btScalar upperLimit,
bool angConstraint,
btScalar relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
solverConstraint.m_multiBodyA = m_bodyA;
solverConstraint.m_multiBodyB = m_bodyB;
solverConstraint.m_linkA = m_linkA;
solverConstraint.m_linkB = m_linkB;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
if (bodyA)
rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
if (bodyB)
rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
if (multiBodyA)
{
if (solverConstraint.m_linkA<0)
{
rel_pos1 = posAworld - multiBodyA->getBasePos();
} else
{
rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
}
const int ndofA = multiBodyA->getNumDofs() + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
if (solverConstraint.m_deltaVelAindex <0)
{
solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
} else
{
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
}
//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
//resize..
solverConstraint.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
//copy/determine
if(jacOrgA)
{
for (int i=0;i<ndofA;i++)
data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
}
else
{
btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
//multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
}
//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
//resize..
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
//determine..
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
btVector3 torqueAxis0;
if (angConstraint) {
torqueAxis0 = constraintNormalAng;
}
else {
torqueAxis0 = rel_pos1.cross(constraintNormalLin);
}
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = constraintNormalLin;
}
else //if(rb0)
{
btVector3 torqueAxis0;
if (angConstraint) {
torqueAxis0 = constraintNormalAng;
}
else {
torqueAxis0 = rel_pos1.cross(constraintNormalLin);
}
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = constraintNormalLin;
}
if (multiBodyB)
{
if (solverConstraint.m_linkB<0)
{
rel_pos2 = posBworld - multiBodyB->getBasePos();
} else
{
rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
}
const int ndofB = multiBodyB->getNumDofs() + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
{
solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
//resize..
solverConstraint.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
//copy/determine..
if(jacOrgB)
{
for (int i=0;i<ndofB;i++)
data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
}
else
{
//multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
}
//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
//resize..
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
//determine..
multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
btVector3 torqueAxis1;
if (angConstraint) {
torqueAxis1 = constraintNormalAng;
}
else {
torqueAxis1 = rel_pos2.cross(constraintNormalLin);
}
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -constraintNormalLin;
}
else //if(rb1)
{
btVector3 torqueAxis1;
if (angConstraint) {
torqueAxis1 = constraintNormalAng;
}
else {
torqueAxis1 = rel_pos2.cross(constraintNormalLin);
}
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -constraintNormalLin;
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
btScalar* deltaVelA = 0;
btScalar* deltaVelB = 0;
int ndofA = 0;
//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
if (multiBodyA)
{
ndofA = multiBodyA->getNumDofs() + 6;
jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
btScalar l = deltaVelA[i];
denom0 += j*l;
}
}
else if(rb0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
if (angConstraint) {
denom0 = rb0->getInvMass() + constraintNormalAng.dot(vec);
}
else {
denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
}
}
//
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumDofs() + 6;
jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
btScalar j = jacB[i] ;
btScalar l = deltaVelB[i];
denom1 += j*l;
}
}
else if(rb1)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
if (angConstraint) {
denom1 = rb1->getInvMass() + constraintNormalAng.dot(vec);
}
else {
denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
}
}
//
btScalar d = denom0+denom1;
if (d>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
}
else
{
//disable the constraint row to handle singularity/redundant constraint
solverConstraint.m_jacDiagABInv = 0.f;
}
}
//compute rhs and remaining solverConstraint fields
btScalar penetration = isFriction? 0 : posError;
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumDofs() + 6;
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
}
else if(rb0)
{
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumDofs() + 6;
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
}
else if(rb1)
{
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
}
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
}
///warm starting (or zero if disabled)
/*
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
} else
{
if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
}
if (multiBodyB)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
} else
{
if (rb1)
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
}
}
} else
*/
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar positionalError = 0.f;
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
//split impulse is not implemented yet for btMultiBody*
//if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
positionalError = -penetration * erp/infoGlobal.m_timeStep;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
//split impulse is not implemented yet for btMultiBody*
// if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
}
/*else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
*/
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = lowerLimit;
solverConstraint.m_upperLimit = upperLimit;
}
return rel_vel;
}

View File

@ -0,0 +1,195 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_MULTIBODY_CONSTRAINT_H
#define BT_MULTIBODY_CONSTRAINT_H
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "btMultiBody.h"
class btMultiBody;
struct btSolverInfo;
#include "btMultiBodySolverConstraint.h"
struct btMultiBodyJacobianData
{
btAlignedObjectArray<btScalar> m_jacobians;
btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
btAlignedObjectArray<btScalar> m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;
btAlignedObjectArray<btSolverBody>* m_solverBodyPool;
int m_fixedBodyId;
};
ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraint
{
protected:
btMultiBody* m_bodyA;
btMultiBody* m_bodyB;
int m_linkA;
int m_linkB;
int m_numRows;
int m_jacSizeA;
int m_jacSizeBoth;
int m_posOffset;
bool m_isUnilateral;
int m_numDofsFinalized;
btScalar m_maxAppliedImpulse;
// warning: the data block lay out is not consistent for all constraints
// data block laid out as follows:
// cached impulses. (one per row.)
// jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
// positions. (one per row.)
btAlignedObjectArray<btScalar> m_data;
void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
btScalar* jacOrgA, btScalar* jacOrgB,
const btVector3& constraintNormalAng,
const btVector3& constraintNormalLin,
const btVector3& posAworld, const btVector3& posBworld,
btScalar posError,
const btContactSolverInfo& infoGlobal,
btScalar lowerLimit, btScalar upperLimit,
bool angConstraint = false,
btScalar relaxation = 1.f,
bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
virtual ~btMultiBodyConstraint();
void updateJacobianSizes();
void allocateJacobiansMultiDof();
//many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later.
virtual void setFrameInB(const btMatrix3x3& frameInB) {}
virtual void setPivotInB(const btVector3& pivotInB){}
virtual void finalizeMultiDof()=0;
virtual int getIslandIdA() const =0;
virtual int getIslandIdB() const =0;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)=0;
int getNumRows() const
{
return m_numRows;
}
btMultiBody* getMultiBodyA()
{
return m_bodyA;
}
btMultiBody* getMultiBodyB()
{
return m_bodyB;
}
void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
{
btAssert(dof>=0);
btAssert(dof < getNumRows());
m_data[dof] = appliedImpulse;
}
btScalar getAppliedImpulse(int dof)
{
btAssert(dof>=0);
btAssert(dof < getNumRows());
return m_data[dof];
}
// current constraint position
// constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
// NOTE: ignored position for friction rows.
btScalar getPosition(int row) const
{
return m_data[m_posOffset + row];
}
void setPosition(int row, btScalar pos)
{
m_data[m_posOffset + row] = pos;
}
bool isUnilateral() const
{
return m_isUnilateral;
}
// jacobian blocks.
// each of size 6 + num_links. (jacobian2 is null if no body2.)
// format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
btScalar* jacobianA(int row)
{
return &m_data[m_numRows + row * m_jacSizeBoth];
}
const btScalar* jacobianA(int row) const
{
return &m_data[m_numRows + (row * m_jacSizeBoth)];
}
btScalar* jacobianB(int row)
{
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
}
const btScalar* jacobianB(int row) const
{
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
}
btScalar getMaxAppliedImpulse() const
{
return m_maxAppliedImpulse;
}
void setMaxAppliedImpulse(btScalar maxImp)
{
m_maxAppliedImpulse = maxImp;
}
virtual void debugDraw(class btIDebugDraw* drawer)=0;
virtual void setGearRatio(btScalar ratio) {}
virtual void setGearAuxLink(int gearAuxLink) {}
virtual void setRelativePositionTarget(btScalar relPosTarget){}
virtual void setErp(btScalar erp){}
};
#endif //BT_MULTIBODY_CONSTRAINT_H

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,100 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H
#define BT_MULTIBODY_CONSTRAINT_SOLVER_H
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "btMultiBodySolverConstraint.h"
#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
class btMultiBody;
#include "btMultiBodyConstraint.h"
ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
{
protected:
btMultiBodyConstraintArray m_multiBodyNonContactConstraints;
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
btMultiBodyJacobianData m_data;
//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
btMultiBodyConstraint** m_tmpMultiBodyConstraints;
int m_tmpNumMultiBodyConstraints;
btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,
btScalar combinedTorsionalFriction,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow,
btScalar* jacA,btScalar* jacB,
btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
const btContactSolverInfo& infoGlobal);
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
const btVector3& contactNormal,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
//either rolling or spinning friction
void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
const btVector3& contactNormal,
btManifoldPoint& cp,
btScalar combinedTorsionalFriction,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
};
#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H

View File

@ -0,0 +1,991 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btMultiBodyDynamicsWorld.h"
#include "btMultiBodyConstraintSolver.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
#include "LinearMath/btQuickprof.h"
#include "btMultiBodyConstraint.h"
#include "LinearMath/btIDebugDraw.h"
#include "LinearMath/btSerializer.h"
void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int mask)
{
m_multiBodies.push_back(body);
}
void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
{
m_multiBodies.remove(body);
}
void btMultiBodyDynamicsWorld::calculateSimulationIslands()
{
BT_PROFILE("calculateSimulationIslands");
getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
{
//merge islands based on speculative contact manifolds too
for (int i=0;i<this->m_predictiveManifolds.size();i++)
{
btPersistentManifold* manifold = m_predictiveManifolds[i];
const btCollisionObject* colObj0 = manifold->getBody0();
const btCollisionObject* colObj1 = manifold->getBody1();
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
{
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
}
}
}
{
int i;
int numConstraints = int(m_constraints.size());
for (i=0;i< numConstraints ; i++ )
{
btTypedConstraint* constraint = m_constraints[i];
if (constraint->isEnabled())
{
const btRigidBody* colObj0 = &constraint->getRigidBodyA();
const btRigidBody* colObj1 = &constraint->getRigidBodyB();
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
{
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
}
}
}
}
//merge islands linked by Featherstone link colliders
for (int i=0;i<m_multiBodies.size();i++)
{
btMultiBody* body = m_multiBodies[i];
{
btMultiBodyLinkCollider* prev = body->getBaseCollider();
for (int b=0;b<body->getNumLinks();b++)
{
btMultiBodyLinkCollider* cur = body->getLink(b).m_collider;
if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
((prev) && (!(prev)->isStaticOrKinematicObject())))
{
int tagPrev = prev->getIslandTag();
int tagCur = cur->getIslandTag();
getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur);
}
if (cur && !cur->isStaticOrKinematicObject())
prev = cur;
}
}
}
//merge islands linked by multibody constraints
{
for (int i=0;i<this->m_multiBodyConstraints.size();i++)
{
btMultiBodyConstraint* c = m_multiBodyConstraints[i];
int tagA = c->getIslandIdA();
int tagB = c->getIslandIdB();
if (tagA>=0 && tagB>=0)
getSimulationIslandManager()->getUnionFind().unite(tagA, tagB);
}
}
//Store the island id in each body
getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
}
void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
{
BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState");
for ( int i=0;i<m_multiBodies.size();i++)
{
btMultiBody* body = m_multiBodies[i];
if (body)
{
body->checkMotionAndSleepIfRequired(timeStep);
if (!body->isAwake())
{
btMultiBodyLinkCollider* col = body->getBaseCollider();
if (col && col->getActivationState() == ACTIVE_TAG)
{
col->setActivationState( WANTS_DEACTIVATION);
col->setDeactivationTime(0.f);
}
for (int b=0;b<body->getNumLinks();b++)
{
btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
if (col && col->getActivationState() == ACTIVE_TAG)
{
col->setActivationState( WANTS_DEACTIVATION);
col->setDeactivationTime(0.f);
}
}
} else
{
btMultiBodyLinkCollider* col = body->getBaseCollider();
if (col && col->getActivationState() != DISABLE_DEACTIVATION)
col->setActivationState( ACTIVE_TAG );
for (int b=0;b<body->getNumLinks();b++)
{
btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
if (col && col->getActivationState() != DISABLE_DEACTIVATION)
col->setActivationState( ACTIVE_TAG );
}
}
}
}
btDiscreteDynamicsWorld::updateActivationState(timeStep);
}
SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs)
{
int islandId;
const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
return islandId;
}
class btSortConstraintOnIslandPredicate2
{
public:
bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const
{
int rIslandId0,lIslandId0;
rIslandId0 = btGetConstraintIslandId2(rhs);
lIslandId0 = btGetConstraintIslandId2(lhs);
return lIslandId0 < rIslandId0;
}
};
SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
{
int islandId;
int islandTagA = lhs->getIslandIdA();
int islandTagB = lhs->getIslandIdB();
islandId= islandTagA>=0?islandTagA:islandTagB;
return islandId;
}
class btSortMultiBodyConstraintOnIslandPredicate
{
public:
bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const
{
int rIslandId0,lIslandId0;
rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
return lIslandId0 < rIslandId0;
}
};
struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
{
btContactSolverInfo* m_solverInfo;
btMultiBodyConstraintSolver* m_solver;
btMultiBodyConstraint** m_multiBodySortedConstraints;
int m_numMultiBodyConstraints;
btTypedConstraint** m_sortedConstraints;
int m_numConstraints;
btIDebugDraw* m_debugDrawer;
btDispatcher* m_dispatcher;
btAlignedObjectArray<btCollisionObject*> m_bodies;
btAlignedObjectArray<btPersistentManifold*> m_manifolds;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver,
btDispatcher* dispatcher)
:m_solverInfo(NULL),
m_solver(solver),
m_multiBodySortedConstraints(NULL),
m_numConstraints(0),
m_debugDrawer(NULL),
m_dispatcher(dispatcher)
{
}
MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other)
{
btAssert(0);
(void)other;
return *this;
}
SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
{
btAssert(solverInfo);
m_solverInfo = solverInfo;
m_multiBodySortedConstraints = sortedMultiBodyConstraints;
m_numMultiBodyConstraints = numMultiBodyConstraints;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
m_bodies.resize (0);
m_manifolds.resize (0);
m_constraints.resize (0);
m_multiBodyConstraints.resize(0);
}
virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
{
if (islandId<0)
{
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
} else
{
//also add all non-contact constraints/joints for this island
btTypedConstraint** startConstraint = 0;
btMultiBodyConstraint** startMultiBodyConstraint = 0;
int numCurConstraints = 0;
int numCurMultiBodyConstraints = 0;
int i;
//find the first constraint for this island
for (i=0;i<m_numConstraints;i++)
{
if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
{
startConstraint = &m_sortedConstraints[i];
break;
}
}
//count the number of constraints in this island
for (;i<m_numConstraints;i++)
{
if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
{
numCurConstraints++;
}
}
for (i=0;i<m_numMultiBodyConstraints;i++)
{
if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
{
startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
break;
}
}
//count the number of multi body constraints in this island
for (;i<m_numMultiBodyConstraints;i++)
{
if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
{
numCurMultiBodyConstraints++;
}
}
//if (m_solverInfo->m_minimumSolverBatchSize<=1)
//{
// m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
//} else
{
for (i=0;i<numBodies;i++)
m_bodies.push_back(bodies[i]);
for (i=0;i<numManifolds;i++)
m_manifolds.push_back(manifolds[i]);
for (i=0;i<numCurConstraints;i++)
m_constraints.push_back(startConstraint[i]);
for (i=0;i<numCurMultiBodyConstraints;i++)
m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
{
processConstraints();
} else
{
//printf("deferred\n");
}
}
}
}
void processConstraints()
{
btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
m_bodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);
}
};
btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
m_multiBodyConstraintSolver(constraintSolver)
{
//split impulse is not yet supported for Featherstone hierarchies
// getSolverInfo().m_splitImpulse = false;
getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS;
m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher);
}
btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
{
delete m_solverMultiBodyIslandCallback;
}
void btMultiBodyDynamicsWorld::forwardKinematics()
{
for (int b=0;b<m_multiBodies.size();b++)
{
btMultiBody* bod = m_multiBodies[b];
bod->forwardKinematics(m_scratch_world_to_local,m_scratch_local_origin);
}
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
forwardKinematics();
BT_PROFILE("solveConstraints");
m_sortedConstraints.resize( m_constraints.size());
int i;
for (i=0;i<getNumConstraints();i++)
{
m_sortedConstraints[i] = m_constraints[i];
}
m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
for (i=0;i<m_multiBodyConstraints.size();i++)
{
m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
}
m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
{
BT_PROFILE("btMultiBody addForce");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks()+1);
m_scratch_m.resize(bod->getNumLinks()+1);
bod->addBaseForce(m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
}//if (!isSleeping)
}
}
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks()+1);
m_scratch_m.resize(bod->getNumLinks()+1);
bool doNotUpdatePos = false;
{
if(!bod->isUsingRK4Integration())
{
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m);
}
else
{
//
int numDofs = bod->getNumDofs() + 6;
int numPosVars = bod->getNumPosVars() + 7;
btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
//convenience
btScalar *pMem = &scratch_r2[0];
btScalar *scratch_q0 = pMem; pMem += numPosVars;
btScalar *scratch_qx = pMem; pMem += numPosVars;
btScalar *scratch_qd0 = pMem; pMem += numDofs;
btScalar *scratch_qd1 = pMem; pMem += numDofs;
btScalar *scratch_qd2 = pMem; pMem += numDofs;
btScalar *scratch_qd3 = pMem; pMem += numDofs;
btScalar *scratch_qdd0 = pMem; pMem += numDofs;
btScalar *scratch_qdd1 = pMem; pMem += numDofs;
btScalar *scratch_qdd2 = pMem; pMem += numDofs;
btScalar *scratch_qdd3 = pMem; pMem += numDofs;
btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
/////
//copy q0 to scratch_q0 and qd0 to scratch_qd0
scratch_q0[0] = bod->getWorldToBaseRot().x();
scratch_q0[1] = bod->getWorldToBaseRot().y();
scratch_q0[2] = bod->getWorldToBaseRot().z();
scratch_q0[3] = bod->getWorldToBaseRot().w();
scratch_q0[4] = bod->getBasePos().x();
scratch_q0[5] = bod->getBasePos().y();
scratch_q0[6] = bod->getBasePos().z();
//
for(int link = 0; link < bod->getNumLinks(); ++link)
{
for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
}
//
for(int dof = 0; dof < numDofs; ++dof)
scratch_qd0[dof] = bod->getVelocityVector()[dof];
////
struct
{
btMultiBody *bod;
btScalar *scratch_qx, *scratch_q0;
void operator()()
{
for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
scratch_qx[dof] = scratch_q0[dof];
}
} pResetQx = {bod, scratch_qx, scratch_q0};
//
struct
{
void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
{
for(int i = 0; i < size; ++i)
pVal[i] = pCurVal[i] + dt * pDer[i];
}
} pEulerIntegrate;
//
struct
{
void operator()(btMultiBody *pBody, const btScalar *pData)
{
btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
pVel[i] = pData[i];
}
} pCopyToVelocityVector;
//
struct
{
void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
{
for(int i = 0; i < size; ++i)
pDst[i] = pSrc[start + i];
}
} pCopy;
//
btScalar h = solverInfo.m_timeStep;
#define output &m_scratch_r[bod->getNumDofs()]
//calc qdd0 from: q0 & qd0
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd0, 0, numDofs);
//calc q1 = q0 + h/2 * qd0
pResetQx();
bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
//calc qd1 = qd0 + h/2 * qdd0
pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
//
//calc qdd1 from: q1 & qd1
pCopyToVelocityVector(bod, scratch_qd1);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd1, 0, numDofs);
//calc q2 = q0 + h/2 * qd1
pResetQx();
bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
//calc qd2 = qd0 + h/2 * qdd1
pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
//
//calc qdd2 from: q2 & qd2
pCopyToVelocityVector(bod, scratch_qd2);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd2, 0, numDofs);
//calc q3 = q0 + h * qd2
pResetQx();
bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
//calc qd3 = qd0 + h * qdd2
pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
//
//calc qdd3 from: q3 & qd3
pCopyToVelocityVector(bod, scratch_qd3);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd3, 0, numDofs);
//
//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
for(int i = 0; i < numDofs; ++i)
{
delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);
//delta_q[i] = h*scratch_qd0[i];
//delta_qd[i] = h*scratch_qdd0[i];
}
//
pCopyToVelocityVector(bod, scratch_qd0);
bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
//
if(!doNotUpdatePos)
{
btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
for(int i = 0; i < numDofs; ++i)
pRealBuf[i] = delta_q[i];
//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
bod->setPosUpdated(true);
}
//ugly hack which resets the cached data to t0 (needed for constraint solver)
{
for(int link = 0; link < bod->getNumLinks(); ++link)
bod->getLink(link).updateCacheMultiDof();
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m);
}
}
}
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
bod->clearForcesAndTorques();
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
}//if (!isSleeping)
}
}
clearMultiBodyConstraintForces();
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks()+1);
m_scratch_m.resize(bod->getNumLinks()+1);
{
if(!bod->isUsingRK4Integration())
{
bool isConstraintPass = true;
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass);
}
}
}
}
}
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->processDeltaVeeMultiDof2();
}
}
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
btDiscreteDynamicsWorld::integrateTransforms(timeStep);
{
BT_PROFILE("btMultiBody stepPositions");
//integrate and update the Featherstone hierarchies
for (int b=0;b<m_multiBodies.size();b++)
{
btMultiBody* bod = m_multiBodies[b];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
int nLinks = bod->getNumLinks();
///base + num m_links
{
if(!bod->isPosUpdated())
bod->stepPositionsMultiDof(timeStep);
else
{
btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
bod->stepPositionsMultiDof(1, 0, pRealBuf);
bod->setPosUpdated(false);
}
}
m_scratch_world_to_local.resize(nLinks+1);
m_scratch_local_origin.resize(nLinks+1);
bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local,m_scratch_local_origin);
} else
{
bod->clearVelocities();
}
}
}
}
void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
{
m_multiBodyConstraints.push_back(constraint);
}
void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
{
m_multiBodyConstraints.remove(constraint);
}
void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
{
constraint->debugDraw(getDebugDrawer());
}
void btMultiBodyDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
btDiscreteDynamicsWorld::debugDrawWorld();
bool drawConstraints = false;
if (getDebugDrawer())
{
int mode = getDebugDrawer()->getDebugMode();
if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
{
drawConstraints = true;
}
if (drawConstraints)
{
BT_PROFILE("btMultiBody debugDrawWorld");
for (int c=0;c<m_multiBodyConstraints.size();c++)
{
btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
debugDrawMultiBodyConstraint(constraint);
}
for (int b = 0; b<m_multiBodies.size(); b++)
{
btMultiBody* bod = m_multiBodies[b];
bod->forwardKinematics(m_scratch_world_to_local1,m_scratch_local_origin1);
getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
for (int m = 0; m<bod->getNumLinks(); m++)
{
const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
getDebugDrawer()->drawTransform(tr, 0.1);
//draw the joint axis
if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
}
}
}
}
}
void btMultiBodyDynamicsWorld::applyGravity()
{
btDiscreteDynamicsWorld::applyGravity();
#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
BT_PROFILE("btMultiBody addGravity");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
bod->addBaseForce(m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
}//if (!isSleeping)
}
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
}
void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
{
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->clearConstraintForces();
}
}
void btMultiBodyDynamicsWorld::clearMultiBodyForces()
{
{
// BT_PROFILE("clearMultiBodyForces");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
btMultiBody* bod = m_multiBodies[i];
bod->clearForcesAndTorques();
}
}
}
}
void btMultiBodyDynamicsWorld::clearForces()
{
btDiscreteDynamicsWorld::clearForces();
#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
clearMultiBodyForces();
#endif
}
void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
{
serializer->startSerialization();
serializeDynamicsWorldInfo( serializer);
serializeMultiBodies(serializer);
serializeRigidBodies(serializer);
serializeCollisionObjects(serializer);
serializer->finishSerialization();
}
void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
{
int i;
//serialize all collision objects
for (i=0;i<m_multiBodies.size();i++)
{
btMultiBody* mb = m_multiBodies[i];
{
int len = mb->calculateSerializeBufferSize();
btChunk* chunk = serializer->allocate(len,1);
const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
}
}
}

View File

@ -0,0 +1,114 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H
#define BT_MULTIBODY_DYNAMICS_WORLD_H
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
#define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
class btMultiBody;
class btMultiBodyConstraint;
class btMultiBodyConstraintSolver;
struct MultiBodyInplaceSolverIslandCallback;
///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet
///This implementation is still preliminary/experimental.
class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld
{
protected:
btAlignedObjectArray<btMultiBody*> m_multiBodies;
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
btMultiBodyConstraintSolver* m_multiBodyConstraintSolver;
MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback;
//cached data to avoid memory allocations
btAlignedObjectArray<btQuaternion> m_scratch_world_to_local;
btAlignedObjectArray<btVector3> m_scratch_local_origin;
btAlignedObjectArray<btQuaternion> m_scratch_world_to_local1;
btAlignedObjectArray<btVector3> m_scratch_local_origin1;
btAlignedObjectArray<btScalar> m_scratch_r;
btAlignedObjectArray<btVector3> m_scratch_v;
btAlignedObjectArray<btMatrix3x3> m_scratch_m;
virtual void calculateSimulationIslands();
virtual void updateActivationState(btScalar timeStep);
virtual void solveConstraints(btContactSolverInfo& solverInfo);
virtual void serializeMultiBodies(btSerializer* serializer);
public:
btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
virtual ~btMultiBodyDynamicsWorld ();
virtual void addMultiBody(btMultiBody* body, int group= btBroadphaseProxy::DefaultFilter, int mask=btBroadphaseProxy::AllFilter);
virtual void removeMultiBody(btMultiBody* body);
virtual int getNumMultibodies() const
{
return m_multiBodies.size();
}
btMultiBody* getMultiBody(int mbIndex)
{
return m_multiBodies[mbIndex];
}
const btMultiBody* getMultiBody(int mbIndex) const
{
return m_multiBodies[mbIndex];
}
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual int getNumMultiBodyConstraints() const
{
return m_multiBodyConstraints.size();
}
virtual btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex)
{
return m_multiBodyConstraints[constraintIndex];
}
virtual const btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex) const
{
return m_multiBodyConstraints[constraintIndex];
}
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void integrateTransforms(btScalar timeStep);
virtual void debugDrawWorld();
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
void forwardKinematics();
virtual void clearForces();
virtual void clearMultiBodyConstraintForces();
virtual void clearMultiBodyForces();
virtual void applyGravity();
virtual void serialize(btSerializer* serializer);
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

View File

@ -0,0 +1,211 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyFixedConstraint.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "LinearMath/btIDebugDraw.h"
#define BTMBFIXEDCONSTRAINT_DIM 6
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
:btMultiBodyConstraint(body,0,link,-1,BTMBFIXEDCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB),
m_frameInA(frameInA),
m_frameInB(frameInB)
{
m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses
}
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBFIXEDCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB),
m_frameInA(frameInA),
m_frameInB(frameInB)
{
m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses
}
void btMultiBodyFixedConstraint::finalizeMultiDof()
{
//not implemented yet
btAssert(0);
}
btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
{
}
int btMultiBodyFixedConstraint::getIslandIdA() const
{
if (m_rigidBodyA)
return m_rigidBodyA->getIslandTag();
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyFixedConstraint::getIslandIdB() const
{
if (m_rigidBodyB)
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
{
int numDim = BTMBFIXEDCONSTRAINT_DIM;
for (int i=0;i<numDim;i++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = i;
constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal1.setValue(0,0,0);
constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal2.setValue(0,0,0);
constraintRow.m_angularComponentA.setValue(0,0,0);
constraintRow.m_angularComponentB.setValue(0,0,0);
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
// Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
btMatrix3x3 frameAworld = m_frameInA;
if (m_rigidBodyA)
{
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
frameAworld = frameAworld.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation());
} else
{
if (m_bodyA) {
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
}
}
btVector3 pivotBworld = m_pivotInB;
btMatrix3x3 frameBworld = m_frameInB;
if (m_rigidBodyB)
{
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
frameBworld = frameBworld.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation());
} else
{
if (m_bodyB) {
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
}
}
btMatrix3x3 relRot = frameAworld.inverse()*frameBworld;
btVector3 angleDiff;
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,angleDiff);
btVector3 constraintNormalLin(0,0,0);
btVector3 constraintNormalAng(0,0,0);
btScalar posError = 0.0;
if (i < 3) {
constraintNormalLin[i] = -1;
posError = (pivotAworld-pivotBworld).dot(constraintNormalLin);
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
constraintNormalLin, pivotAworld, pivotBworld,
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
}
else { //i>=3
constraintNormalAng = frameAworld.getColumn(i%3);
posError = angleDiff[i%3];
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
constraintNormalLin, pivotAworld, pivotBworld,
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse, true
);
}
}
}
void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer)
{
btTransform tr;
tr.setIdentity();
if (m_rigidBodyA)
{
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyA)
{
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
tr.setOrigin(pivotAworld);
drawer->drawTransform(tr, 0.1);
}
if (m_rigidBodyB)
{
// that ideally should draw the same frame
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyB)
{
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
tr.setOrigin(pivotBworld);
drawer->drawTransform(tr, 0.1);
}
}

View File

@ -0,0 +1,94 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_FIXED_CONSTRAINT_H
#define BT_MULTIBODY_FIXED_CONSTRAINT_H
#include "btMultiBodyConstraint.h"
class btMultiBodyFixedConstraint : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btMatrix3x3 m_frameInA;
btMatrix3x3 m_frameInB;
public:
btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
virtual ~btMultiBodyFixedConstraint();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInA() const
{
return m_pivotInA;
}
void setPivotInA(const btVector3& pivotInA)
{
m_pivotInA = pivotInA;
}
const btVector3& getPivotInB() const
{
return m_pivotInB;
}
virtual void setPivotInB(const btVector3& pivotInB)
{
m_pivotInB = pivotInB;
}
const btMatrix3x3& getFrameInA() const
{
return m_frameInA;
}
void setFrameInA(const btMatrix3x3& frameInA)
{
m_frameInA = frameInA;
}
const btMatrix3x3& getFrameInB() const
{
return m_frameInB;
}
virtual void setFrameInB(const btMatrix3x3& frameInB)
{
m_frameInB = frameInB;
}
virtual void debugDraw(class btIDebugDraw* drawer);
};
#endif //BT_MULTIBODY_FIXED_CONSTRAINT_H

View File

@ -0,0 +1,184 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyGearConstraint.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false),
m_gearRatio(1),
m_gearAuxLink(-1),
m_erp(0),
m_relativePositionTarget(0)
{
}
void btMultiBodyGearConstraint::finalizeMultiDof()
{
allocateJacobiansMultiDof();
m_numDofsFinalized = m_jacSizeBoth;
}
btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
{
}
int btMultiBodyGearConstraint::getIslandIdA() const
{
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyGearConstraint::getIslandIdB() const
{
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
if (m_maxAppliedImpulse==0.f)
return;
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
int linkDoF = 0;
unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF);
// row 0: the lower bound
jacobianA(0)[offsetA] = 1;
jacobianB(0)[offsetB] = m_gearRatio;
btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
btScalar kp = 1;
btScalar kd = 1;
int numRows = getNumRows();
for (int row=0;row<numRows;row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar auxVel = 0;
if (m_gearAuxLink>=0)
{
auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
}
currentVelocity += auxVel;
if (m_erp!=0)
{
btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof];
btScalar diff = currentPositionB+currentPositionA;
btScalar desiredPositionDiff = this->m_relativePositionTarget;
posError = -m_erp*(desiredPositionDiff - diff);
}
btScalar desiredRelativeVelocity = auxVel;
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
{
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
break;
}
case btMultibodyLink::ePrismatic:
{
btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
constraintRow.m_contactNormal1=prismaticAxisInWorld;
constraintRow.m_contactNormal2=-prismaticAxisInWorld;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
{
btAssert(0);
}
};
}
}
}

View File

@ -0,0 +1,117 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_GEAR_CONSTRAINT_H
#define BT_MULTIBODY_GEAR_CONSTRAINT_H
#include "btMultiBodyConstraint.h"
class btMultiBodyGearConstraint : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btMatrix3x3 m_frameInA;
btMatrix3x3 m_frameInB;
btScalar m_gearRatio;
int m_gearAuxLink;
btScalar m_erp;
btScalar m_relativePositionTarget;
public:
//btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
virtual ~btMultiBodyGearConstraint();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInA() const
{
return m_pivotInA;
}
void setPivotInA(const btVector3& pivotInA)
{
m_pivotInA = pivotInA;
}
const btVector3& getPivotInB() const
{
return m_pivotInB;
}
virtual void setPivotInB(const btVector3& pivotInB)
{
m_pivotInB = pivotInB;
}
const btMatrix3x3& getFrameInA() const
{
return m_frameInA;
}
void setFrameInA(const btMatrix3x3& frameInA)
{
m_frameInA = frameInA;
}
const btMatrix3x3& getFrameInB() const
{
return m_frameInB;
}
virtual void setFrameInB(const btMatrix3x3& frameInB)
{
m_frameInB = frameInB;
}
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)
}
virtual void setGearRatio(btScalar gearRatio)
{
m_gearRatio = gearRatio;
}
virtual void setGearAuxLink(int gearAuxLink)
{
m_gearAuxLink = gearAuxLink;
}
virtual void setRelativePositionTarget(btScalar relPosTarget)
{
m_relativePositionTarget = relPosTarget;
}
virtual void setErp(btScalar erp)
{
m_erp = erp;
}
};
#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H

View File

@ -0,0 +1,27 @@
/*
Copyright (c) 2015 Google Inc.
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_JOINT_FEEDBACK_H
#define BT_MULTIBODY_JOINT_FEEDBACK_H
#include "LinearMath/btSpatialAlgebra.h"
struct btMultiBodyJointFeedback
{
btSpatialForceVector m_reactionForces;
};
#endif //BT_MULTIBODY_JOINT_FEEDBACK_H

View File

@ -0,0 +1,205 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyJointLimitConstraint.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
//:btMultiBodyConstraint(body,0,link,-1,2,true),
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,2,true),
m_lowerBound(lower),
m_upperBound(upper)
{
}
void btMultiBodyJointLimitConstraint::finalizeMultiDof()
{
// the data.m_jacobians never change, so may as well
// initialize them here
allocateJacobiansMultiDof();
unsigned int offset = 6 + m_bodyA->getLink(m_linkA).m_dofOffset;
// row 0: the lower bound
jacobianA(0)[offset] = 1;
// row 1: the upper bound
//jacobianA(1)[offset] = -1;
jacobianB(1)[offset] = -1;
m_numDofsFinalized = m_jacSizeBoth;
}
btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
{
}
int btMultiBodyJointLimitConstraint::getIslandIdA() const
{
if(m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyJointLimitConstraint::getIslandIdB() const
{
if(m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
// row 0: the lower bound
setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent
// row 1: the upper bound
setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
for (int row=0;row<getNumRows();row++)
{
btScalar penetration = getPosition(row);
//todo: consider adding some safety threshold here
if (penetration>0)
{
continue;
}
btScalar direction = row? -1 : 1;
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
constraintRow.m_multiBodyA = m_bodyA;
constraintRow.m_multiBodyB = m_bodyB;
const btScalar posError = 0; //why assume it's zero?
const btVector3 dummy(0, 0, 0);
btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
{
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
btVector3 revoluteAxisInWorld = direction*quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
break;
}
case btMultibodyLink::ePrismatic:
{
btVector3 prismaticAxisInWorld = direction* quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
constraintRow.m_contactNormal1=prismaticAxisInWorld;
constraintRow.m_contactNormal2=-prismaticAxisInWorld;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
{
btAssert(0);
}
};
}
{
btScalar positionalError = 0.f;
btScalar velocityError = - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = penetrationImpulse;
}
}
}
}

View File

@ -0,0 +1,50 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
#include "btMultiBodyConstraint.h"
struct btSolverInfo;
class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
{
protected:
btScalar m_lowerBound;
btScalar m_upperBound;
public:
btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
virtual ~btMultiBodyJointLimitConstraint();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)
}
};
#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H

View File

@ -0,0 +1,186 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyJointMotor.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(1.),
m_kp(0),
m_erp(1),
m_rhsClamp(SIMD_INFINITY)
{
m_maxAppliedImpulse = maxMotorImpulse;
// the data.m_jacobians never change, so may as well
// initialize them here
}
void btMultiBodyJointMotor::finalizeMultiDof()
{
allocateJacobiansMultiDof();
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
int linkDoF = 0;
unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
// row 0: the lower bound
// row 0: the lower bound
jacobianA(0)[offset] = 1;
m_numDofsFinalized = m_jacSizeBoth;
}
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
//:btMultiBodyConstraint(body,0,link,-1,1,true),
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(1.),
m_kp(0),
m_erp(1),
m_rhsClamp(SIMD_INFINITY)
{
btAssert(linkDoF < body->getLink(link).m_dofCount);
m_maxAppliedImpulse = maxMotorImpulse;
}
btMultiBodyJointMotor::~btMultiBodyJointMotor()
{
}
int btMultiBodyJointMotor::getIslandIdA() const
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
return -1;
}
int btMultiBodyJointMotor::getIslandIdB() const
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
return -1;
}
void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
if (m_maxAppliedImpulse==0.f)
return;
const btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
for (int row=0;row<getNumRows();row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;
if (rhs>m_rhsClamp)
{
rhs=m_rhsClamp;
}
if (rhs<-m_rhsClamp)
{
rhs=-m_rhsClamp;
}
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,rhs);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
{
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
break;
}
case btMultibodyLink::ePrismatic:
{
btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
constraintRow.m_contactNormal1=prismaticAxisInWorld;
constraintRow.m_contactNormal2=-prismaticAxisInWorld;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
{
btAssert(0);
}
};
}
}
}

View File

@ -0,0 +1,81 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_JOINT_MOTOR_H
#define BT_MULTIBODY_JOINT_MOTOR_H
#include "btMultiBodyConstraint.h"
struct btSolverInfo;
class btMultiBodyJointMotor : public btMultiBodyConstraint
{
protected:
btScalar m_desiredVelocity;
btScalar m_desiredPosition;
btScalar m_kd;
btScalar m_kp;
btScalar m_erp;
btScalar m_rhsClamp;//maximum error
public:
btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
virtual ~btMultiBodyJointMotor();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
{
m_desiredVelocity = velTarget;
m_kd = kd;
}
virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
{
m_desiredPosition = posTarget;
m_kp = kp;
}
virtual void setErp(btScalar erp)
{
m_erp = erp;
}
virtual btScalar getErp() const
{
return m_erp;
}
virtual void setRhsClamp(btScalar rhsClamp)
{
m_rhsClamp = rhsClamp;
}
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)
}
};
#endif //BT_MULTIBODY_JOINT_MOTOR_H

View File

@ -0,0 +1,244 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_MULTIBODY_LINK_H
#define BT_MULTIBODY_LINK_H
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btVector3.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
enum btMultiBodyLinkFlags
{
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,
BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,
};
//both defines are now permanently enabled
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
#define TEST_SPATIAL_ALGEBRA_LAYER
//
// Various spatial helper functions
//
//namespace {
#include "LinearMath/btSpatialAlgebra.h"
//}
//
// Link struct
//
struct btMultibodyLink
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btScalar m_mass; // mass of link
btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal)
int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant.
//this is set to zero for planar joint (see also m_eVector comment)
// m_eVector is constant, but depends on the joint type:
// revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
// planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
// todo: fix the planar so it is consistent with the other joints
btVector3 m_eVector;
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
enum eFeatherstoneJointType
{
eRevolute = 0,
ePrismatic = 1,
eSpherical = 2,
ePlanar = 3,
eFixed = 4,
eInvalid
};
// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
// for prismatic: m_axesTop[0] = zero;
// m_axesBottom[0] = unit vector along the joint axis.
// for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
// m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
//
// for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
// m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
//
// for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
// m_axesTop[1][2] = zero
// m_axesBottom[0] = zero
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
btSpatialMotionVector m_axes[6];
void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
void setAxisBottom(int dof, const btVector3 &axis)
{
m_axes[dof].m_bottomVec = axis;
}
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
{
m_axes[dof].m_topVec.setValue(x, y, z);
}
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
{
m_axes[dof].m_bottomVec.setValue(x, y, z);
}
const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
int m_dofOffset, m_cfgOffset;
btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame
btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame.
btVector3 m_appliedForce; // In WORLD frame
btVector3 m_appliedTorque; // In WORLD frame
btVector3 m_appliedConstraintForce; // In WORLD frame
btVector3 m_appliedConstraintTorque; // In WORLD frame
btScalar m_jointPos[7];
//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
//It gets set to zero after each internal stepSimulation call
btScalar m_jointTorque[6];
class btMultiBodyLinkCollider* m_collider;
int m_flags;
int m_dofCount, m_posVarCount; //redundant but handy
eFeatherstoneJointType m_jointType;
struct btMultiBodyJointFeedback* m_jointFeedback;
btTransform m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics
const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
const void* m_userPtr;//m_userPtr ptr needs to be managed by the developer/user!
btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
btScalar m_jointLowerLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
btScalar m_jointUpperLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
btScalar m_jointMaxForce; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
btScalar m_jointMaxVelocity;//todo: implement this internally. It is unused for now, it is set by a URDF loader.
// ctor: set some sensible defaults
btMultibodyLink()
: m_mass(1),
m_parent(-1),
m_zeroRotParentToThis(0, 0, 0, 1),
m_cachedRotParentToThis(0, 0, 0, 1),
m_collider(0),
m_flags(0),
m_dofCount(0),
m_posVarCount(0),
m_jointType(btMultibodyLink::eInvalid),
m_jointFeedback(0),
m_linkName(0),
m_jointName(0),
m_userPtr(0),
m_jointDamping(0),
m_jointFriction(0),
m_jointLowerLimit(0),
m_jointUpperLimit(0),
m_jointMaxForce(0),
m_jointMaxVelocity(0)
{
m_inertiaLocal.setValue(1, 1, 1);
setAxisTop(0, 0., 0., 0.);
setAxisBottom(0, 1., 0., 0.);
m_dVector.setValue(0, 0, 0);
m_eVector.setValue(0, 0, 0);
m_cachedRVector.setValue(0, 0, 0);
m_appliedForce.setValue( 0, 0, 0);
m_appliedTorque.setValue(0, 0, 0);
//
m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
m_jointPos[3] = 1.f; //"quat.w"
m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
m_cachedWorldTransform.setIdentity();
}
// routine to update m_cachedRotParentToThis and m_cachedRVector
void updateCacheMultiDof(btScalar *pq = 0)
{
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
switch(m_jointType)
{
case eRevolute:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
case ePrismatic:
{
// m_cachedRotParentToThis never changes, so no need to update
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
break;
}
case eSpherical:
{
m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
case ePlanar:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
case eFixed:
{
m_cachedRotParentToThis = m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
default:
{
//invalid type
btAssert(0);
}
}
}
};
#endif //BT_MULTIBODY_LINK_H

View File

@ -0,0 +1,125 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H
#define BT_FEATHERSTONE_LINK_COLLIDER_H
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "btMultiBody.h"
class btMultiBodyLinkCollider : public btCollisionObject
{
//protected:
public:
btMultiBody* m_multiBody;
int m_link;
btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
:m_multiBody(multiBody),
m_link(link)
{
m_checkCollideWith = true;
//we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands
//this means that some constraints might point to bodies that are not in the islands, causing crashes
//if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
{
m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
}
// else
//{
// m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT);
//}
m_internalType = CO_FEATHERSTONE_LINK;
}
static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
{
if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
return (btMultiBodyLinkCollider*)colObj;
return 0;
}
static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
{
if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
return (btMultiBodyLinkCollider*)colObj;
return 0;
}
virtual bool checkCollideWithOverride(const btCollisionObject* co) const
{
const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
if (!other)
return true;
if (other->m_multiBody != this->m_multiBody)
return true;
if (!m_multiBody->hasSelfCollision())
return false;
//check if 'link' has collision disabled
if (m_link>=0)
{
const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
{
int parent_of_this = m_link;
while (1)
{
if (parent_of_this==-1)
break;
parent_of_this = m_multiBody->getLink(parent_of_this).m_parent;
if (parent_of_this==other->m_link)
{
return false;
}
}
}
else if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
{
if ( link.m_parent == other->m_link)
return false;
}
}
if (other->m_link>=0)
{
const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
{
int parent_of_other = other->m_link;
while (1)
{
if (parent_of_other==-1)
break;
parent_of_other = m_multiBody->getLink(parent_of_other).m_parent;
if (parent_of_other==this->m_link)
return false;
}
}
else if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
{
if (otherLink.m_parent == this->m_link)
return false;
}
}
return true;
}
};
#endif //BT_FEATHERSTONE_LINK_COLLIDER_H

View File

@ -0,0 +1,221 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyPoint2Point.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btIDebugDraw.h"
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
#define BTMBP2PCONSTRAINT_DIM 3
#else
#define BTMBP2PCONSTRAINT_DIM 6
#endif
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB)
{
m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
}
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB)
{
m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
}
void btMultiBodyPoint2Point::finalizeMultiDof()
{
//not implemented yet
btAssert(0);
}
btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
{
}
int btMultiBodyPoint2Point::getIslandIdA() const
{
if (m_rigidBodyA)
return m_rigidBodyA->getIslandTag();
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyPoint2Point::getIslandIdB() const
{
if (m_rigidBodyB)
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// int i=1;
int numDim = BTMBP2PCONSTRAINT_DIM;
for (int i=0;i<numDim;i++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
//memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = i;
constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal1.setValue(0,0,0);
constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal2.setValue(0,0,0);
constraintRow.m_angularComponentA.setValue(0,0,0);
constraintRow.m_angularComponentB.setValue(0,0,0);
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
btVector3 contactNormalOnB(0,0,0);
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
contactNormalOnB[i] = -1;
#else
contactNormalOnB[i%3] = -1;
#endif
// Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
if (m_rigidBodyA)
{
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
} else
{
if (m_bodyA)
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
}
btVector3 pivotBworld = m_pivotInB;
if (m_rigidBodyB)
{
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
} else
{
if (m_bodyB)
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
}
btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0;
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0,0,0),
contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
//@todo: support the case of btMultiBody versus btRigidBody,
//see btPoint2PointConstraint::getInfo2NonVirtual
#else
const btVector3 dummy(0, 0, 0);
btAssert(m_bodyA->isMultiDof());
btScalar* jac1 = jacobianA(i);
const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;
m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
fillMultiBodyConstraint(constraintRow, data, jac1, 0,
dummy, dummy, dummy, //sucks but let it be this way "for the time being"
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
#endif
}
}
void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer)
{
btTransform tr;
tr.setIdentity();
if (m_rigidBodyA)
{
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyA)
{
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
tr.setOrigin(pivotAworld);
drawer->drawTransform(tr, 0.1);
}
if (m_rigidBodyB)
{
// that ideally should draw the same frame
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyB)
{
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
tr.setOrigin(pivotBworld);
drawer->drawTransform(tr, 0.1);
}
}

View File

@ -0,0 +1,68 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_POINT2POINT_H
#define BT_MULTIBODY_POINT2POINT_H
#include "btMultiBodyConstraint.h"
//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
ATTRIBUTE_ALIGNED16(class) btMultiBodyPoint2Point : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);
virtual ~btMultiBodyPoint2Point();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInB() const
{
return m_pivotInB;
}
virtual void setPivotInB(const btVector3& pivotInB)
{
m_pivotInB = pivotInB;
}
virtual void debugDraw(class btIDebugDraw* drawer);
};
#endif //BT_MULTIBODY_POINT2POINT_H

View File

@ -0,0 +1,230 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#include "btMultiBodySliderConstraint.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "LinearMath/btIDebugDraw.h"
#define BTMBSLIDERCONSTRAINT_DIM 5
#define EPSILON 0.000001
btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
:btMultiBodyConstraint(body,0,link,-1,BTMBSLIDERCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB),
m_frameInA(frameInA),
m_frameInB(frameInB),
m_jointAxis(jointAxis)
{
m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses
}
btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBSLIDERCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB),
m_frameInA(frameInA),
m_frameInB(frameInB),
m_jointAxis(jointAxis)
{
m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses
}
void btMultiBodySliderConstraint::finalizeMultiDof()
{
//not implemented yet
btAssert(0);
}
btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
{
}
int btMultiBodySliderConstraint::getIslandIdA() const
{
if (m_rigidBodyA)
return m_rigidBodyA->getIslandTag();
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodySliderConstraint::getIslandIdB() const
{
if (m_rigidBodyB)
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
{
// Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
btMatrix3x3 frameAworld = m_frameInA;
btVector3 jointAxis = m_jointAxis;
if (m_rigidBodyA)
{
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
frameAworld = m_frameInA.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation());
jointAxis = quatRotate(m_rigidBodyA->getOrientation(),m_jointAxis);
} else if (m_bodyA) {
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
}
btVector3 pivotBworld = m_pivotInB;
btMatrix3x3 frameBworld = m_frameInB;
if (m_rigidBodyB)
{
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
frameBworld = m_frameInB.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation());
} else if (m_bodyB) {
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
}
btVector3 constraintAxis[2];
for (int i = 0; i < 3; ++i)
{
constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
if (constraintAxis[0].safeNorm() > EPSILON)
{
constraintAxis[0] = constraintAxis[0].normalized();
constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
constraintAxis[1] = constraintAxis[1].normalized();
break;
}
}
btMatrix3x3 relRot = frameAworld.inverse()*frameBworld;
btVector3 angleDiff;
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,angleDiff);
int numDim = BTMBSLIDERCONSTRAINT_DIM;
for (int i=0;i<numDim;i++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = i;
constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal1.setValue(0,0,0);
constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal2.setValue(0,0,0);
constraintRow.m_angularComponentA.setValue(0,0,0);
constraintRow.m_angularComponentB.setValue(0,0,0);
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
if (m_rigidBodyA)
{
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
}
if (m_rigidBodyB)
{
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
}
btVector3 constraintNormalLin(0,0,0);
btVector3 constraintNormalAng(0,0,0);
btScalar posError = 0.0;
if (i < 2) {
constraintNormalLin = constraintAxis[i];
posError = (pivotAworld-pivotBworld).dot(constraintNormalLin);
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
constraintNormalLin, pivotAworld, pivotBworld,
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
}
else { //i>=2
constraintNormalAng = frameAworld.getColumn(i%3);
posError = angleDiff[i%3];
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
constraintNormalLin, pivotAworld, pivotBworld,
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse, true
);
}
}
}
void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
{
btTransform tr;
tr.setIdentity();
if (m_rigidBodyA)
{
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyA)
{
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
tr.setOrigin(pivotAworld);
drawer->drawTransform(tr, 0.1);
}
if (m_rigidBodyB)
{
// that ideally should draw the same frame
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyB)
{
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
tr.setOrigin(pivotBworld);
drawer->drawTransform(tr, 0.1);
}
}

View File

@ -0,0 +1,105 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_SLIDER_CONSTRAINT_H
#define BT_MULTIBODY_SLIDER_CONSTRAINT_H
#include "btMultiBodyConstraint.h"
class btMultiBodySliderConstraint : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btMatrix3x3 m_frameInA;
btMatrix3x3 m_frameInB;
btVector3 m_jointAxis;
public:
btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
virtual ~btMultiBodySliderConstraint();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInA() const
{
return m_pivotInA;
}
void setPivotInA(const btVector3& pivotInA)
{
m_pivotInA = pivotInA;
}
const btVector3& getPivotInB() const
{
return m_pivotInB;
}
virtual void setPivotInB(const btVector3& pivotInB)
{
m_pivotInB = pivotInB;
}
const btMatrix3x3& getFrameInA() const
{
return m_frameInA;
}
void setFrameInA(const btMatrix3x3& frameInA)
{
m_frameInA = frameInA;
}
const btMatrix3x3& getFrameInB() const
{
return m_frameInB;
}
virtual void setFrameInB(const btMatrix3x3& frameInB)
{
m_frameInB = frameInB;
}
const btVector3& getJointAxis() const
{
return m_jointAxis;
}
void setJointAxis(const btVector3& jointAxis)
{
m_jointAxis = jointAxis;
}
virtual void debugDraw(class btIDebugDraw* drawer);
};
#endif //BT_MULTIBODY_SLIDER_CONSTRAINT_H

View File

@ -0,0 +1,90 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H
#define BT_MULTIBODY_SOLVER_CONSTRAINT_H
#include "LinearMath/btVector3.h"
#include "LinearMath/btAlignedObjectArray.h"
class btMultiBody;
class btMultiBodyConstraint;
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_orgConstraint(0), m_orgDofIndex(-1)
{}
int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
int m_jacAindex;
int m_deltaVelBindex;
int m_jacBindex;
btVector3 m_relpos1CrossNormal;
btVector3 m_contactNormal1;
btVector3 m_relpos2CrossNormal;
btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
btVector3 m_angularComponentA;
btVector3 m_angularComponentB;
mutable btSimdScalar m_appliedPushImpulse;
mutable btSimdScalar m_appliedImpulse;
btScalar m_friction;
btScalar m_jacDiagABInv;
btScalar m_rhs;
btScalar m_cfm;
btScalar m_lowerLimit;
btScalar m_upperLimit;
btScalar m_rhsPenetration;
union
{
void* m_originalContactPoint;
btScalar m_unusedPadding4;
};
int m_overrideNumSolverIterations;
int m_frictionIndex;
int m_solverBodyIdA;
btMultiBody* m_multiBodyA;
int m_linkA;
int m_solverBodyIdB;
btMultiBody* m_multiBodyB;
int m_linkB;
//for writing back applied impulses
btMultiBodyConstraint* m_orgConstraint;
int m_orgDofIndex;
enum btSolverConstraintType
{
BT_SOLVER_CONTACT_1D = 0,
BT_SOLVER_FRICTION_1D
};
};
typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray;
#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H