30 #define D6_USE_OBSOLETE_METHOD false 31 #define D6_USE_FRAME_OFFSET true 40 , m_frameInA(frameInA)
41 , m_frameInB(frameInB),
42 m_useLinearReferenceFrameA(useLinearReferenceFrameA),
68 #define GENERIC_D6_DISABLE_WARMSTARTING 1 124 if(m_loLimit>m_hiLimit)
129 if (test_value < m_loLimit)
132 m_currentLimitError = test_value - m_loLimit;
133 if(m_currentLimitError>
SIMD_PI)
135 else if(m_currentLimitError<-
SIMD_PI)
139 else if (test_value> m_hiLimit)
142 m_currentLimitError = test_value - m_hiLimit;
143 if(m_currentLimitError>
SIMD_PI)
145 else if(m_currentLimitError<-
SIMD_PI)
161 if (needApplyTorques()==
false)
return 0.0f;
163 btScalar target_velocity = m_targetVelocity;
164 btScalar maxMotorForce = m_maxMotorForce;
167 if (m_currentLimit!=0)
169 target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
170 maxMotorForce = m_maxLimitForce;
173 maxMotorForce *= timeStep;
181 vel_diff = angVelA-angVelB;
188 btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
191 if ( motor_relvel < SIMD_EPSILON && motor_relvel > -
SIMD_EPSILON )
198 btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
204 if (unclippedMotorImpulse>0.0f)
206 clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
210 clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
218 btScalar oldaccumImpulse = m_accumulatedImpulse;
219 btScalar sum = oldaccumImpulse + clippedMotorImpulse;
222 clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
224 btVector3 motorImp = clippedMotorImpulse * axis;
229 return clippedMotorImpulse;
244 btScalar loLimit = m_lowerLimit[limitIndex];
245 btScalar hiLimit = m_upperLimit[limitIndex];
246 if(loLimit > hiLimit)
248 m_currentLimit[limitIndex] = 0;
249 m_currentLimitError[limitIndex] =
btScalar(0.f);
253 if (test_value < loLimit)
255 m_currentLimit[limitIndex] = 2;
256 m_currentLimitError[limitIndex] = test_value - loLimit;
259 else if (test_value> hiLimit)
261 m_currentLimit[limitIndex] = 1;
262 m_currentLimitError[limitIndex] = test_value - hiLimit;
266 m_currentLimit[limitIndex] = 0;
267 m_currentLimitError[limitIndex] =
btScalar(0.f);
300 btScalar depth = -(pointInA - pointInB).
dot(axis_normal_on_a);
304 btScalar minLimit = m_lowerLimit[limit_index];
305 btScalar maxLimit = m_upperLimit[limit_index];
308 if (minLimit < maxLimit)
311 if (depth > maxLimit)
319 if (depth < minLimit)
332 btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
337 btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
339 m_accumulatedImpulse[limit_index] = sum > hi ?
btScalar(0.) : sum < lo ?
btScalar(0.) :
sum;
340 normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
342 btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
348 return normalImpulse;
467 for(i = 0; i < 3; i++)
497 pivotAInW,pivotBInW);
508 normalWorld = this->
getAxis(i);
534 for(i = 0; i < 3; i++)
582 int row =
setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
583 setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
587 int row =
setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
609 int row =
setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
610 setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
614 int row =
setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
626 for (
int i=0;i<3 ;i++ )
649 int indx1 = (i + 1) % 3;
650 int indx2 = (i + 2) % 3;
656 row +=
get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
660 row +=
get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
672 int row = row_offset;
674 for (
int i=0;i<3 ;i++ )
693 transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
750 weight = imA / (imA + imB);
764 for(
int i = 0; i < 3; i++)
778 int srow = row * info->
rowskip;
781 if (powered || limit)
789 J2[srow+0] = -ax1[0];
790 J2[srow+1] = -ax1[1];
791 J2[srow+2] = -ax1[2];
811 btVector3 totalDist = projA + ax1 * desiredOffs - projB;
813 relA = orthoA + totalDist *
m_factA;
814 relB = orthoB - totalDist *
m_factB;
815 tmpA = relA.
cross(ax1);
816 tmpB = relB.
cross(ax1);
898 vel = angVelA.
dot(ax1);
901 vel -= angVelB.
dot(ax1);
905 vel = linVelA.
dot(ax1);
908 vel -= linVelB.
dot(ax1);
926 if (newc < info->m_constraintError[srow])
947 if((axis >= 0) && (axis < 3))
967 else if((axis >=3) && (axis < 6))
997 if((axis >= 0) && (axis < 3))
1017 else if((axis >=3) && (axis < 6))
1055 xAxis[1], yAxis[1], zAxis[1],
1056 xAxis[2], yAxis[2], zAxis[2]);
btMatrix3x3 inverse() const
Return the inverse of the matrix.
btScalar * m_constraintError
btScalar m_damping
Damping for linear limit.
btScalar getInvMass() const
static T sum(const btAlignedObjectArray< T > &items)
int m_currentLimit[3]
Current relative offset of constraint frames.
btScalar * m_J2angularAxis
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
void setAxis(const btVector3 &axis1, const btVector3 &axis2)
bool m_useLinearReferenceFrameA
btScalar solveLinearAxis(btScalar timeStep, btScalar jacDiagABInv, btRigidBody &body1, const btVector3 &pointInA, btRigidBody &body2, const btVector3 &pointInB, int limit_index, const btVector3 &axis_normal_on_a, const btVector3 &anchorPos)
void applyTorqueImpulse(const btVector3 &torque)
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
btScalar m_currentPosition
How much is violated this limit.
btScalar m_loLimit
limit_parameters
btScalar getRelativePivotPosition(int axis_index) const
Get the relative position of the constraint pivot.
btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis locatio...
btScalar m_stopERP
Error tolerance factor when joint is at limit.
btScalar m_currentLimitError
temp_variables
btScalar btGetMatrixElem(const btMatrix3x3 &mat, int index)
btVector3 m_targetVelocity
target motor velocity
btTransform m_calculatedTransformA
btScalar m_normalCFM
Relaxation factor.
btScalar m_stopCFM
Constraint force mixing factor when joint is at limit.
#define D6_USE_OBSOLETE_METHOD
btVector3 m_upperLimit
the constraint upper limits
btScalar * m_J1angularAxis
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
void setFrames(const btTransform &frameA, const btTransform &frameB)
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...
btJacobianEntry m_jacAng[3]
3 orthogonal angular constraints
void buildLinearJacobian(btJacobianEntry &jacLinear, const btVector3 &normalWorld, const btVector3 &pivotAInW, const btVector3 &pivotBInW)
btScalar m_limitSoftness
Linear_Limit_parameters.
btScalar dot(const btVector3 &v) const
Return the dot product.
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
void calculateTransforms()
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
bool m_useSolveConstraintObsolete
for backwards compatibility during the transition to 'getInfo/getInfo2'
btVector3 getAxis(int axis_index) const
Get the rotation axis in global coordinates.
btVector3 m_calculatedAxisAngleDiff
void calculateAngleInfo()
calcs the euler angles between the two bodies.
bool m_useOffsetForConstraintFrame
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
bool isLimited(int limitIndex) const
Test limit.
btScalar * m_J1linearAxis
btScalar m_accumulatedImpulse
#define D6_USE_FRAME_OFFSET
const btTransform & getCenterOfMassTransform() const
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 getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
btVector3 m_normalCFM
Bounce parameter for linear limit.
btScalar getAngle(int axis_index) const
Get the relative Euler angle.
btScalar btAtan2(btScalar x, btScalar y)
btTranslationalLimitMotor m_linearLimits
Linear_Limit_parameters.
bool needApplyForce(int limitIndex) const
btVector3 m_stopCFM
Constraint force mixing factor when joint is at limit.
bool needApplyTorques() const
Need apply correction.
const btVector3 & getAngularVelocity() const
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
btScalar m_targetVelocity
target motor velocity
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)
void buildAngularJacobian(btJacobianEntry &jacAngular, const btVector3 &jointAxisW)
const btVector3 & getCenterOfMassPosition() const
The btRigidBody is the main class for rigid body objects.
btVector3 m_lowerLimit
the constraint lower limits
btTransform m_calculatedTransformB
btVector3 m_calculatedLinearDiff
btVector3 m_currentLimitError
int m_currentLimit
current value of angle
btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
btVector3 can be used to represent 3D points and vectors.
btTransform m_frameInA
relative_frames
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
btScalar m_damping
Damping.
static btRigidBody & getFixedBody()
bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html.
bool testAngularLimitMotor(int axis_index)
Test angular limit.
btRotationalLimitMotor m_angularLimits[3]
hinge_parameters
btScalar m_hiLimit
joint limit
btJacobianEntry m_jacLinear[3]
Jacobians.
btVector3 normalized() const
Return a normalized version of this vector.
int testLimitValue(btScalar test_value)
calculates error
void getInfo1NonVirtual(btConstraintInfo1 *info)
btScalar * m_J2linearAxis
btTransform m_frameInB
the constraint space w.r.t body B
TypedConstraint is the baseclass for Bullet constraints and vehicles.
btGeneric6DofConstraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &frameInA, const btTransform &frameInB, bool useLinearReferenceFrameA)
btVector3 m_calculatedAxis[3]
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
internal method used by the constraint solver, don't use them directly
btRotationalLimitMotor * getRotationalLimitMotor(int index)
Retrieves the angular limit informacion.
const btRigidBody & getRigidBodyA() const
btScalar solveAngularLimits(btScalar timeStep, btVector3 &axis, btScalar jacDiagABInv, btRigidBody *body0, btRigidBody *body1)
apply the correction impulses for two bodies
int testLimitValue(int limitIndex, btScalar test_value)
btMatrix3x3 transpose() const
Return the transpose of the matrix.
virtual void buildJacobian()
performs Jacobian calculation, and also calculates angle differences and axis
void calculateLinearInfo()
btVector3 m_maxMotorForce
max force on motor
btScalar m_maxMotorForce
max force on motor
btScalar m_maxLimitForce
max force on limit
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
const btVector3 & getInvInertiaDiagLocal() const
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
btVector3 m_currentLinearDiff
How much is violated this limit.
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
const btVector3 & getLinearVelocity() const
btScalar m_bounce
restitution factor
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
btVector3 m_stopERP
Error tolerance factor when joint is at limit.
btScalar btAsin(btScalar x)
Rotation Limit structure for generic joints.
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)
virtual void calcAnchorPos(void)
#define btAssertConstrParams(_par)
#define BT_6DOF_FLAGS_AXIS_SHIFT
const btRigidBody & getRigidBodyB() const
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
void updateRHS(btScalar timeStep)
btVector3 m_accumulatedImpulse