forked from LeenkxTeam/LNXSDK
Update Files
This commit is contained in:
2040
lib/haxebullet/bullet/BulletDynamics/Featherstone/btMultiBody.cpp
Normal file
2040
lib/haxebullet/bullet/BulletDynamics/Featherstone/btMultiBody.cpp
Normal file
File diff suppressed because it is too large
Load Diff
814
lib/haxebullet/bullet/BulletDynamics/Featherstone/btMultiBody.h
Normal file
814
lib/haxebullet/bullet/BulletDynamics/Featherstone/btMultiBody.h
Normal 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
|
@ -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;
|
||||
|
||||
}
|
@ -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
@ -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
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
@ -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
|
@ -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);
|
||||
}
|
||||
}
|
@ -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
|
@ -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);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
@ -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
|
@ -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
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -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
|
@ -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
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
@ -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
|
@ -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);
|
||||
}
|
||||
}
|
@ -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
|
@ -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
|
Reference in New Issue
Block a user