454 for (i = 0; i < 3; i++)
464 for (i = 0; i < 3; i++)
486 int row =
setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
487 setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
494 for (
int i = 0; i < 3; i++)
524 int indx1 = (i + 1) % 3;
525 int indx2 = (i + 2) % 3;
527 #define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3
536 if (indx1Violated && indx2Violated)
540 row +=
get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
548 int row = row_offset;
551 int cIdx[] = {0, 1, 2};
588 for (
int ii = 0; ii < 3; ii++)
611 row +=
get_limit_motor_info2(&
m_angularLimits[i], transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
630 for (
int i = 0; i < 3; i++)
642 J1[srow + 0] = ax1[0];
643 J1[srow + 1] = ax1[1];
644 J1[srow + 2] = ax1[2];
646 J2[srow + 0] = -ax1[0];
647 J2[srow + 1] = -ax1[1];
648 J2[srow + 2] = -ax1[2];
657 tmpA = relA.cross(ax1);
658 tmpB = relB.cross(ax1);
676 int srow = row * info->
rowskip;
680 btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
682 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
697 if (bounceerror < info->m_constraintError[srow]) info->
m_constraintError[srow] = bounceerror;
706 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
713 if (bounceerror < info->m_constraintError[srow]) info->
m_constraintError[srow] = bounceerror;
732 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
743 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
776 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
800 info->
m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
811 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
827 vel = angVelA.dot(ax1) - angVelB.dot(ax1);
833 vel = (linVelA + tanVelA).
dot(ax1) - (linVelB + tanVelB).
dot(ax1);
848 m = mA*mB / (mA + mB);
862 btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
897 info->
cfm[srow] = cfm;
909 if ((axis >= 0) && (axis < 3))
933 else if ((axis >= 3) && (axis < 6))
967 if ((axis >= 0) && (axis < 3))
991 else if ((axis >= 3) && (axis < 6))
1029 frameInW.setIdentity();
1030 frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
1031 xAxis[1], yAxis[1], zAxis[1],
1032 xAxis[2], yAxis[2], zAxis[2]);
1043 btAssert((index >= 0) && (index < 6));
1052 btAssert((index >= 0) && (index < 6));
1061 btAssert((index >= 0) && (index < 6));
1070 btAssert((index >= 0) && (index < 6));
1079 btAssert((index >= 0) && (index < 6));
1123 btAssert((index >= 0) && (index < 6));
1132 btAssert((index >= 0) && (index < 6));
1141 btAssert((index >= 0) && (index < 6));
1156 btAssert((index >= 0) && (index < 6));
1173 for (i = 0; i < 3; i++)
1175 for (i = 0; i < 3; i++)
1181 btAssert((index >= 0) && (index < 6));
1191 btAssert((index >= 0) && (index < 6));
1227 if (loLimit > hiLimit)
1232 else if (loLimit == hiLimit)
virtual void getInfo2(btConstraintInfo2 *info)
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
virtual void setParam(int num, btScalar value, int axis=-1)
void setDamping(btScalar damping)
virtual void setFrames(const btTransform &frameA, const btTransform &frameB)
virtual void getInfo1(btConstraintInfo1 *info)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
const btRigidBody & getRigidBodyA() const
const btRigidBody & getRigidBodyB() const
btFixedConstraint btRigidBody & rbB
btFixedConstraint btRigidBody const btTransform & frameInA
btFixedConstraint btRigidBody const btTransform const btTransform & frameInB
bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3....
btScalar btGetMatrixElem(const btMatrix3x3 &mat, int index)
btTransform m_calculatedTransformB
btVector3 m_calculatedAxis[3]
void calculateLinearInfo()
btVector3 m_calculatedLinearDiff
btRotationalLimitMotor m_angularLimits[3]
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
void setAxis(const btVector3 &axis1, const btVector3 &axis2)
int get_limit_motor_info2(btRotationalLimitMotor *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)
btTranslationalLimitMotor m_linearLimits
void calculateAngleInfo()
calcs the euler angles between the two bodies.
void calculateTransforms(const btTransform &transA, const btTransform &transB)
Calcs global transform of the offsets.
btVector3 m_calculatedAxisAngleDiff
btVector3 getAxis(int axis_index) const
Get the rotation axis in global coordinates.
bool testAngularLimitMotor(int axis_index)
Test angular limit.
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)
btTransform m_calculatedTransformA
#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION
static bool matrixToEulerYZX(const btMatrix3x3 &mat, btVector3 &xyz)
void setStiffness(int index, btScalar stiffness, bool limitIfNeeded=true)
static bool matrixToEulerYXZ(const btMatrix3x3 &mat, btVector3 &xyz)
#define BT_6DOF_FLAGS_AXIS_SHIFT2
void enableSpring(int index, bool onOff)
void setBounce(int index, btScalar bounce)
static bool matrixToEulerZXY(const btMatrix3x3 &mat, btVector3 &xyz)
void calculateJacobi(btRotationalLimitMotor2 *limot, const btTransform &transA, const btTransform &transB, btConstraintInfo2 *info, int srow, btVector3 &ax1, int rotational, int rotAllowed)
void setServoTarget(int index, btScalar target)
void setMaxMotorForce(int index, btScalar force)
static bool matrixToEulerZYX(const btMatrix3x3 &mat, btVector3 &xyz)
void setEquilibriumPoint()
static bool matrixToEulerXZY(const btMatrix3x3 &mat, btVector3 &xyz)
void setTargetVelocity(int index, btScalar velocity)
void setServo(int index, bool onOff)
btGeneric6DofSpring2Constraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &frameInA, const btTransform &frameInB, RotateOrder rotOrder=RO_XYZ)
@ BT_6DOF_FLAGS_ERP_MOTO2
@ BT_6DOF_FLAGS_CFM_MOTO2
@ BT_6DOF_FLAGS_ERP_STOP2
@ BT_6DOF_FLAGS_CFM_STOP2
@ BT_6DOF_FLAGS_USE_INFINITE_ERROR
RotateOrder m_rotateOrder
btScalar m_equilibriumPoint[6]
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
SIMD_FORCE_INLINE btScalar btAsin(btScalar x)
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y)
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
internal method used by the constraint solver, don't use them directly
SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
#define btAssertConstrParams(_par)
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
static btRigidBody & getFixedBody()
@ D6_SPRING_2_CONSTRAINT_TYPE
SIMD_FORCE_INLINE btScalar length2() const
Return the length of the vector squared.
SIMD_FORCE_INLINE btScalar angle(const btVector3 &v) const
Return the angle between this and another vector.
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
const btVector3 & getAngularVelocity() const
btScalar getInvMass() const
const btTransform & getCenterOfMassTransform() const
const btVector3 & getLinearVelocity() const
const btMatrix3x3 & getInvInertiaTensorWorld() const
bool m_springDampingLimited
btScalar m_targetVelocity
void testLimitValue(btScalar test_value)
btScalar m_currentLimitErrorHi
btScalar m_currentPosition
bool m_springStiffnessLimited
btScalar m_currentLimitError
btScalar m_springStiffness
btScalar m_equilibriumPoint
btScalar m_currentPosition
How much is violated this limit.
btScalar m_targetVelocity
target motor velocity
btScalar m_maxMotorForce
max force on motor
btScalar m_bounce
restitution factor
btScalar m_currentLimitError
btScalar m_stopERP
Error tolerance factor when joint is at limit.
int testLimitValue(btScalar test_value)
calculates error
int m_currentLimit
current value of angle
btScalar m_stopCFM
Constraint force mixing factor when joint is at limit.
btVector3 m_currentLimitError
void testLimitValue(int limitIndex, btScalar test_value)
btVector3 m_currentLimitErrorHi
btVector3 m_stopERP
Error tolerance factor when joint is at limit.
int m_currentLimit[3]
Current relative offset of constraint frames.
btVector3 m_maxMotorForce
max force on motor
int testLimitValue(int limitIndex, btScalar test_value)
btVector3 m_currentLinearDiff
How much is violated this limit.
btVector3 m_currentLimitError
btVector3 m_targetVelocity
target motor velocity
btVector3 m_lowerLimit
the constraint lower limits
btVector3 m_upperLimit
the constraint upper limits
static void error(const char *str)
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)
T length(const vec_base< T, Size > &a)
btScalar * m_J2linearAxis
btScalar * m_J2angularAxis
btScalar * m_J1angularAxis
btScalar * m_constraintError
btScalar * m_J1linearAxis