51 , m_frameInA(frameInA)
52 , m_frameInB(frameInB)
53 , m_rotateOrder(rotOrder)
456 for(i = 0; i < 3; i++)
485 int row =
setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
486 setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
494 for (
int i=0;i<3 ;i++ )
522 int indx1 = (i + 1) % 3;
523 int indx2 = (i + 2) % 3;
525 #define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3 534 if( indx1Violated && indx2Violated )
538 row +=
get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
549 int row = row_offset;
552 int cIdx[] = {0, 1, 2};
555 case RO_XYZ : cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2;
break;
556 case RO_XZY : cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1;
break;
557 case RO_YXZ : cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2;
break;
558 case RO_YZX : cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0;
break;
559 case RO_ZXY : cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1;
break;
560 case RO_ZYX : cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0;
break;
564 for (
int ii = 0; ii < 3 ; ii++ )
587 row +=
get_limit_motor_info2(&
m_angularLimits[i],transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
608 for(
int i = 0; i < 3; i++)
624 J2[srow+0] = -ax1[0];
625 J2[srow+1] = -ax1[1];
626 J2[srow+2] = -ax1[2];
635 tmpA = relA.
cross(ax1);
636 tmpB = relB.
cross(ax1);
655 int srow = row * info->
rowskip;
659 btScalar vel = rotational ? angVelA.
dot(ax1) - angVelB.
dot(ax1) : linVelA.
dot(ax1) - linVelB.
dot(ax1);
661 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
671 if (bounceerror < info->m_constraintError[srow]) info->
m_constraintError[srow] = bounceerror;
680 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
685 if (bounceerror < info->m_constraintError[srow]) info->
m_constraintError[srow] = bounceerror;
701 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
712 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
730 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
754 info->
m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
765 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
778 btScalar vel = rotational ? angVelA.
dot(ax1) - angVelB.
dot(ax1) : linVelA.
dot(ax1) - linVelB.
dot(ax1);
784 btScalar angularfreq = sqrt(ks / m);
788 if( 0.25 < angularfreq * dt)
798 btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
816 info->
cfm[srow] = cfm;
829 if((axis >= 0) && (axis < 3))
853 else if((axis >=3) && (axis < 6))
887 if((axis >= 0) && (axis < 3))
911 else if((axis >=3) && (axis < 6))
953 xAxis[1], yAxis[1], zAxis[1],
954 xAxis[2], yAxis[2], zAxis[2]);
965 btAssert((index >= 0) && (index < 6));
974 btAssert((index >= 0) && (index < 6));
983 btAssert((index >= 0) && (index < 6));
992 btAssert((index >= 0) && (index < 6));
1001 btAssert((index >= 0) && (index < 6));
1010 btAssert((index >= 0) && (index < 6));
1019 btAssert((index >= 0) && (index < 6));
1028 btAssert((index >= 0) && (index < 6));
1037 btAssert((index >= 0) && (index < 6));
1048 for( i = 0; i < 3; i++)
1050 for(i = 0; i < 3; i++)
1056 btAssert((index >= 0) && (index < 6));
1066 btAssert((index >= 0) && (index < 6));
1079 if(m_loLimit > m_hiLimit) {
1081 m_currentLimitError =
btScalar(0.f);
1083 else if(m_loLimit == m_hiLimit) {
1084 m_currentLimitError = test_value - m_loLimit;
1087 m_currentLimitError = test_value - m_loLimit;
1088 m_currentLimitErrorHi = test_value - m_hiLimit;
1097 btScalar loLimit = m_lowerLimit[limitIndex];
1098 btScalar hiLimit = m_upperLimit[limitIndex];
1099 if(loLimit > hiLimit) {
1100 m_currentLimitError[limitIndex] = 0;
1101 m_currentLimit[limitIndex] = 0;
1103 else if(loLimit == hiLimit) {
1104 m_currentLimitError[limitIndex] = test_value - loLimit;
1105 m_currentLimit[limitIndex] = 3;
1107 m_currentLimitError[limitIndex] = test_value - loLimit;
1108 m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
1109 m_currentLimit[limitIndex] = 4;
btScalar * m_constraintError
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
void setTargetVelocity(int index, btScalar velocity)
btScalar * m_J2angularAxis
static bool matrixToEulerZXY(const btMatrix3x3 &mat, btVector3 &xyz)
btVector3 m_maxMotorForce
const btVector3 & getAngularVelocity() const
#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION
btVector3 m_springStiffness
void setMaxMotorForce(int index, btScalar force)
#define BT_6DOF_FLAGS_AXIS_SHIFT2
void testLimitValue(int limitIndex, btScalar test_value)
static bool matrixToEulerYXZ(const btMatrix3x3 &mat, btVector3 &xyz)
int get_limit_motor_info2(btRotationalLimitMotor2 *limot, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB, btConstraintInfo2 *info, int row, btVector3 &ax1, int rotational, int rotAllowed=false)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
void setDamping(int index, btScalar damping)
const btTransform & getCenterOfMassTransform() const
void setServo(int index, bool onOff)
const btRigidBody & getRigidBodyA() const
btVector3 m_springDamping
static btScalar btGetMatrixElem(const btMatrix3x3 &mat, int index)
void setAxis(const btVector3 &axis1, const btVector3 &axis2)
void calculateLinearInfo()
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
btScalar * m_J1angularAxis
const btRigidBody & getRigidBodyB() const
static bool matrixToEulerZYX(const btMatrix3x3 &mat, btVector3 &xyz)
btTranslationalLimitMotor2 m_linearLimits
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
void calculateTransforms()
btVector3 normalized() const
Return a normalized version of this vector.
btVector3 m_currentLinearDiff
btVector3 getAxis(int axis_index) const
btVector3 m_calculatedLinearDiff
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
btScalar m_currentLimitError
btScalar * m_J1linearAxis
const btVector3 & getLinearVelocity() const
btScalar getInvMass() const
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
btScalar dot(const btVector3 &v) const
Return the dot product.
btScalar btAtan2(btScalar x, btScalar y)
void setStiffness(int index, btScalar stiffness)
btVector3 m_currentLimitErrorHi
void setValue(const btScalar &xx, const btScalar &xy, const btScalar &xz, const btScalar &yx, const btScalar &yy, const btScalar &yz, const btScalar &zx, const btScalar &zy, const btScalar &zz)
Set the values of the matrix explicitly (row major)
btTransform m_calculatedTransformB
btScalar m_currentLimitErrorHi
The btRigidBody is the main class for rigid body objects.
btTransform m_calculatedTransformA
void setEquilibriumPoint()
btVector3 m_equilibriumPoint
btScalar m_targetVelocity
btGeneric6DofSpring2Constraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &frameInA, const btTransform &frameInB, RotateOrder rotOrder=RO_XYZ)
2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev Added support for generic constrain...
virtual void setParam(int num, btScalar value, int axis=-1)
override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0...
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
btVector3 m_currentLimitError
btVector3 can be used to represent 3D points and vectors.
static bool matrixToEulerXZY(const btMatrix3x3 &mat, btVector3 &xyz)
static btRigidBody & getFixedBody()
RotateOrder m_rotateOrder
void calculateJacobi(btRotationalLimitMotor2 *limot, const btTransform &transA, const btTransform &transB, btConstraintInfo2 *info, int srow, btVector3 &ax1, int rotational, int rotAllowed)
btVector3 m_calculatedAxis[3]
btScalar * m_J2linearAxis
TypedConstraint is the baseclass for Bullet constraints and vehicles.
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
internal method used by the constraint solver, don't use them directly
btVector3 m_targetVelocity
btScalar m_equilibriumPoint
btRotationalLimitMotor2 m_angularLimits[3]
static bool matrixToEulerYZX(const btMatrix3x3 &mat, btVector3 &xyz)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
void testLimitValue(btScalar test_value)
btScalar m_currentPosition
void enableSpring(int index, bool onOff)
btScalar btAsin(btScalar x)
void setFrames(const btTransform &frameA, const btTransform &frameB)
btMatrix3x3 inverse() const
Return the inverse of the matrix.
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
#define btAssertConstrParams(_par)
btScalar m_springStiffness
void calculateAngleInfo()
btVector3 m_calculatedAxisAngleDiff
void testAngularLimitMotor(int axis_index)
void setServoTarget(int index, btScalar target)
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
int setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
void enableMotor(int index, bool onOff)
void setBounce(int index, btScalar bounce)