28 #define D6_USE_OBSOLETE_METHOD false 29 #define D6_USE_FRAME_OFFSET true 40 m_useLinearReferenceFrameA(useLinearReferenceFrameB),
43 m_useSolveConstraintObsolete(false)
50 #define GENERIC_D6_DISABLE_WARMSTARTING 1 148 maxMotorForce *= timeStep;
156 vel_diff = angVelA - angVelB;
163 if (motor_relvel < SIMD_EPSILON && motor_relvel > -
SIMD_EPSILON)
169 btScalar unclippedMotorImpulse = (1 +
m_bounce) * motor_relvel * jacDiagABInv;
175 if (unclippedMotorImpulse > 0.0f)
177 clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
181 clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
189 btScalar sum = oldaccumImpulse + clippedMotorImpulse;
194 btVector3 motorImp = clippedMotorImpulse * axis;
199 return clippedMotorImpulse;
210 if (loLimit > hiLimit)
217 if (test_value < loLimit)
223 else if (test_value > hiLimit)
259 btScalar depth = -(pointInA - pointInB).
dot(axis_normal_on_a);
267 if (minLimit < maxLimit)
270 if (depth > maxLimit)
277 if (depth < minLimit)
297 btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
301 return normalImpulse;
409 for (i = 0; i < 3; i++)
428 for (i = 0; i < 3; i++)
439 pivotAInW, pivotBInW);
444 for (i = 0; i < 3; i++)
449 normalWorld = this->
getAxis(i);
473 for (i = 0; i < 3; i++)
482 for (i = 0; i < 3; i++)
521 int row =
setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
522 setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
526 int row =
setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
527 setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
538 for (i = 0; i < 3; i++)
545 int row =
setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
546 setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
550 int row =
setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
551 setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
560 for (
int i = 0; i < 3; i++)
583 int indx1 = (i + 1) % 3;
584 int indx2 = (i + 2) % 3;
590 row +=
get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
594 row +=
get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0);
604 int row = row_offset;
606 for (
int i = 0; i < 3; i++)
625 transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
671 weight = imA / (imA + imB);
683 for (
int i = 0; i < 3; i++)
695 int srow = row * info->
rowskip;
698 if (powered || limit)
702 J1[srow + 0] = ax1[0];
703 J1[srow + 1] = ax1[1];
704 J1[srow + 2] = ax1[2];
706 J2[srow + 0] = -ax1[0];
707 J2[srow + 1] = -ax1[1];
708 J2[srow + 2] = -ax1[2];
728 btVector3 totalDist = projA + ax1 * desiredOffs - projB;
730 relA = orthoA + totalDist *
m_factA;
731 relB = orthoB - totalDist *
m_factB;
732 tmpA = relA.
cross(ax1);
733 tmpB = relB.
cross(ax1);
816 vel = angVelA.
dot(ax1);
819 vel -= angVelB.
dot(ax1);
823 vel = linVelA.
dot(ax1);
826 vel -= linVelB.
dot(ax1);
844 if (newc < info->m_constraintError[srow])
861 if ((axis >= 0) && (axis < 3))
881 else if ((axis >= 3) && (axis < 6))
911 if ((axis >= 0) && (axis < 3))
931 else if ((axis >= 3) && (axis < 6))
967 xAxis[1], yAxis[1], zAxis[1],
968 xAxis[2], yAxis[2], zAxis[2]);
btScalar * m_constraintError
btScalar m_damping
Damping for linear limit.
static T sum(const btAlignedObjectArray< T > &items)
int m_currentLimit[3]
Current relative offset of constraint frames.
btScalar * m_J2angularAxis
const btVector3 & getAngularVelocity() const
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
const btTransform & getCenterOfMassTransform() const
btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis locatio...
btScalar m_stopERP
Error tolerance factor when joint is at limit.
const btRigidBody & getRigidBodyA() const
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.
btVector3 getAxis(int axis_index) const
Get the rotation axis in global coordinates.
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
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...
const btRigidBody & getRigidBodyB() const
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.
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
btVector3 normalized() const
Return a normalized version of this vector.
void calculateTransforms()
btMatrix3x3 transpose() const
Return the transpose of the matrix.
bool m_useSolveConstraintObsolete
for backwards compatibility during the transition to 'getInfo/getInfo2'
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
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)
btScalar getRelativePivotPosition(int axis_index) const
Get the relative position of the constraint pivot.
const btVector3 & getInvInertiaDiagLocal() const
btScalar * m_J1linearAxis
btScalar m_accumulatedImpulse
#define D6_USE_FRAME_OFFSET
const btVector3 & getLinearVelocity() const
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)
btScalar getInvMass() const
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.
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)
btTranslationalLimitMotor m_linearLimits
Linear_Limit_parameters.
btVector3 m_stopCFM
Constraint force mixing factor when joint is at limit.
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)
The btRigidBody is the main class for rigid body objects.
btVector3 m_lowerLimit
the constraint lower limits
bool needApplyForce(int limitIndex) const
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.
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
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
bool needApplyTorques() const
Need apply correction.
btScalar m_hiLimit
joint limit
const btVector3 & getCenterOfMassPosition() const
btJacobianEntry m_jacLinear[3]
Jacobians.
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.
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)
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...
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
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.
btScalar getAngle(int axis_index) const
Get the relative Euler angle.
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)
bool isLimited(int limitIndex) const
Test limit.
btMatrix3x3 inverse() const
Return the inverse of the matrix.
#define btAssertConstrParams(_par)
#define BT_6DOF_FLAGS_AXIS_SHIFT
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
void updateRHS(btScalar timeStep)
btVector3 m_accumulatedImpulse