58 unsigned int offset = 6 + (
m_bodyA->getLink(
m_linkA).m_dofOffset + linkDoF);
165 qMinTwist = -(qABTwist);
175 angleDiff[2] = twistAngle;
180 btScalar allowed = limitRanges[row];
182 if((angleDiff[row]>-allowed)&&(angleDiff[row]<allowed))
189 if (angleDiff[row]>allowed)
191 angleDiff[row]-=allowed;
194 if (angleDiff[row]<-allowed)
196 angleDiff[row]+=allowed;
208 btScalar velocityError = (desiredVelocity - currentVelocity) * kd;
222 posError = kp*angleDiff[row % 3];
227 double min_applied_impulse = -max_applied_impulse;
231 max_applied_impulse=0;
233 min_applied_impulse=0;
242 min_applied_impulse, max_applied_impulse,
true,
static btVector3 vTwist(1, 0, 0)
@ MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT
btAlignedObjectArray< btMultiBodySolverConstraint > btMultiBodyConstraintArray
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btFabs(btScalar x)
T & expandNonInitializing()
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
void setIdentity()
Set the matrix to the identity.
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
btScalar * jacobianA(int row)
btMultiBodyConstraint(btMultiBody *bodyA, btMultiBody *bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type)
btScalar m_maxAppliedImpulse
btScalar * jacobianB(int row)
void allocateJacobiansMultiDof()
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, btScalar damping=1.0)
virtual void debugDraw(class btIDebugDraw *drawer)
virtual ~btMultiBodySphericalJointLimit()
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
btMultiBodySphericalJointLimit(btMultiBody *body, int link, btScalar swingxRange, btScalar swingyRange, btScalar twistRange, btScalar maxAppliedImpulse)
This file was written by Erwin Coumans.
btVector3 m_desiredVelocity
virtual void finalizeMultiDof()
virtual int getIslandIdA() const
btVector3 m_maxAppliedImpulseMultiDof
bool m_use_multi_dof_params
btQuaternion m_desiredPosition
virtual int getIslandIdB() const
const btScalar & z() const
Return the z value.
const btScalar & y() const
Return the y value.
const btScalar & x() const
Return the x value.
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
btQuaternion inverse() const
Return the inverse of this quaternion.
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
btVector3 can be used to represent 3D points and vectors.
btScalar dot(const btVector3 &v) const
Return the dot product.
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btMultiBodyConstraint * m_orgConstraint