30#define D6_USE_OBSOLETE_METHOD false
31#define D6_USE_FRAME_OFFSET true
41, m_frameInB(frameInB),
42m_useLinearReferenceFrameA(useLinearReferenceFrameA),
55 m_useLinearReferenceFrameA(useLinearReferenceFrameB),
58 m_useSolveConstraintObsolete(false)
68#define GENERIC_D6_DISABLE_WARMSTARTING 1
173 maxMotorForce *= timeStep;
181 vel_diff = angVelA-angVelB;
191 if ( motor_relvel < SIMD_EPSILON && motor_relvel > -
SIMD_EPSILON )
204 if (unclippedMotorImpulse>0.0f)
206 clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
210 clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
219 btScalar sum = oldaccumImpulse + clippedMotorImpulse;
224 btVector3 motorImp = clippedMotorImpulse * axis;
229 return clippedMotorImpulse;
246 if(loLimit > hiLimit)
253 if (test_value < loLimit)
259 else if (test_value> hiLimit)
300 btScalar depth = -(pointInA - pointInB).
dot(axis_normal_on_a);
308 if (minLimit < maxLimit)
311 if (depth > maxLimit)
319 if (depth < minLimit)
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]);
btScalar btGetMatrixElem(const btMatrix3x3 &mat, int index)
#define D6_USE_FRAME_OFFSET
#define D6_USE_OBSOLETE_METHOD
bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3....
#define BT_6DOF_FLAGS_AXIS_SHIFT
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btAtan2(btScalar x, btScalar y)
btScalar btAsin(btScalar x)
static T sum(const btAlignedObjectArray< T > &items)
#define btAssertConstrParams(_par)
btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis locatio...
btTransform m_frameInA
relative_frames
void getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
virtual void calcAnchorPos(void)
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
void buildLinearJacobian(btJacobianEntry &jacLinear, const btVector3 &normalWorld, const btVector3 &pivotAInW, const btVector3 &pivotBInW)
btTransform m_calculatedTransformA
btVector3 m_calculatedLinearDiff
btJacobianEntry m_jacLinear[3]
Jacobians.
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 calculateLinearInfo()
btVector3 m_calculatedAxis[3]
btRotationalLimitMotor * getRotationalLimitMotor(int index)
Retrieves the angular limit informacion.
btGeneric6DofConstraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &frameInA, const btTransform &frameInB, bool useLinearReferenceFrameA)
btJacobianEntry m_jacAng[3]
3 orthogonal angular constraints
btScalar getRelativePivotPosition(int axis_index) const
Get the relative position of the constraint pivot.
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
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...
btTransform m_frameInB
the constraint space w.r.t body B
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)
void updateRHS(btScalar timeStep)
virtual void buildJacobian()
performs Jacobian calculation, and also calculates angle differences and axis
bool m_useOffsetForConstraintFrame
void calculateTransforms()
void buildAngularJacobian(btJacobianEntry &jacAngular, const btVector3 &jointAxisW)
bool m_useLinearReferenceFrameA
void calculateAngleInfo()
calcs the euler angles between the two bodies.
btVector3 getAxis(int axis_index) const
Get the rotation axis in global coordinates.
bool m_useSolveConstraintObsolete
for backwards compatibility during the transition to 'getInfo/getInfo2'
bool testAngularLimitMotor(int axis_index)
Test angular limit.
btScalar getAngle(int axis_index) const
Get the relative Euler angle.
void setAxis(const btVector3 &axis1, const btVector3 &axis2)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
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)
btTranslationalLimitMotor m_linearLimits
Linear_Limit_parameters.
btRotationalLimitMotor m_angularLimits[3]
hinge_parameters
btVector3 m_calculatedAxisAngleDiff
void setFrames(const btTransform &frameA, const btTransform &frameB)
btTransform m_calculatedTransformB
void getInfo1NonVirtual(btConstraintInfo1 *info)
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
btMatrix3x3 inverse() const
Return the inverse of the matrix.
btMatrix3x3 transpose() const
Return the transpose of the matrix.
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
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)
The btRigidBody is the main class for rigid body objects.
void applyTorqueImpulse(const btVector3 &torque)
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
btScalar getInvMass() const
const btVector3 & getInvInertiaDiagLocal() const
const btTransform & getCenterOfMassTransform() const
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
const btVector3 & getAngularVelocity() const
const btVector3 & getCenterOfMassPosition() const
const btVector3 & getLinearVelocity() const
Rotation Limit structure for generic joints.
btScalar m_currentPosition
How much is violated this limit.
btScalar m_targetVelocity
target motor velocity
btScalar m_hiLimit
joint limit
btScalar m_normalCFM
Relaxation factor.
btScalar m_maxMotorForce
max force on motor
btScalar m_damping
Damping.
btScalar m_bounce
restitution factor
btScalar m_loLimit
limit_parameters
btScalar m_currentLimitError
temp_variables
btScalar m_stopERP
Error tolerance factor when joint is at limit.
bool needApplyTorques() const
Need apply correction.
btScalar m_accumulatedImpulse
int testLimitValue(btScalar test_value)
calculates error
btScalar m_maxLimitForce
max force on limit
int m_currentLimit
current value of angle
btScalar solveAngularLimits(btScalar timeStep, btVector3 &axis, btScalar jacDiagABInv, btRigidBody *body0, btRigidBody *body1)
apply the correction impulses for two bodies
btScalar m_stopCFM
Constraint force mixing factor when joint is at limit.
btVector3 m_stopERP
Error tolerance factor when joint is at limit.
int m_currentLimit[3]
Current relative offset of constraint frames.
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)
btVector3 m_stopCFM
Constraint force mixing factor when joint is at limit.
btVector3 m_maxMotorForce
max force on motor
btVector3 m_accumulatedImpulse
int testLimitValue(int limitIndex, btScalar test_value)
btVector3 m_currentLinearDiff
How much is violated this limit.
btVector3 m_normalCFM
Bounce parameter for linear limit.
bool needApplyForce(int limitIndex) const
btVector3 m_currentLimitError
btScalar m_limitSoftness
Linear_Limit_parameters.
btVector3 m_targetVelocity
target motor velocity
btVector3 m_lowerLimit
the constraint lower limits
bool isLimited(int limitIndex) const
Test limit.
btScalar m_damping
Damping for linear limit.
btVector3 m_upperLimit
the constraint upper limits
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
const btRigidBody & getRigidBodyA() const
const btRigidBody & getRigidBodyB() const
btVector3 can be used to represent 3D points and vectors.
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.
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
btVector3 normalized() const
Return a normalized version of this vector.
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
btScalar * m_J2angularAxis
btScalar * m_J1linearAxis
btScalar * m_J2linearAxis
btScalar * m_J1angularAxis
btScalar * m_constraintError