28 #define D6_USE_OBSOLETE_METHOD false
29 #define D6_USE_FRAME_OFFSET true
50 #define GENERIC_D6_DISABLE_WARMSTARTING 1
148 maxMotorForce *= timeStep;
156 vel_diff = angVelA - angVelB;
158 btScalar rel_vel = axis.dot(vel_diff);
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)
254 btScalar rel_vel = axis_normal_on_a.dot(vel);
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++)
608 if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
610 btVector3 axis = d6constraint->getAxis(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))
965 frameInW.setIdentity();
966 frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
967 xAxis[1], yAxis[1], zAxis[1],
968 xAxis[2], yAxis[2], zAxis[2]);
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)
virtual void setFrames(const btTransform &frameA, const btTransform &frameB)
void updateRHS(btScalar timeStep)
bool m_useSolveConstraintObsolete
virtual void getInfo1(btConstraintInfo1 *info)
void getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
const btRigidBody & getRigidBodyA() const
const btRigidBody & getRigidBodyB() const
void getInfo1NonVirtual(btConstraintInfo1 *info)
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....
#define D6_USE_FRAME_OFFSET
btScalar btGetMatrixElem(const btMatrix3x3 &mat, int index)
#define D6_USE_OBSOLETE_METHOD
btScalar getAngle(int axis_index) const
Get the relative Euler angle.
virtual void calcAnchorPos(void)
btTransform m_calculatedTransformB
void buildLinearJacobian(btJacobianEntry &jacLinear, const btVector3 &normalWorld, const btVector3 &pivotAInW, const btVector3 &pivotBInW)
btVector3 m_calculatedAxis[3]
void calculateLinearInfo()
btVector3 m_calculatedLinearDiff
btJacobianEntry m_jacAng[3]
btRotationalLimitMotor m_angularLimits[3]
btGeneric6DofConstraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &frameInA, const btTransform &frameInB, bool useLinearReferenceFrameA)
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
btJacobianEntry m_jacLinear[3]
3 orthogonal linear constraints
btScalar getRelativePivotPosition(int axis_index) const
Get the relative position of the constraint pivot.
void setAxis(const btVector3 &axis1, const btVector3 &axis2)
bool m_useLinearReferenceFrameA
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 buildAngularJacobian(btJacobianEntry &jacAngular, const btVector3 &jointAxisW)
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
bool m_useOffsetForConstraintFrame
#define BT_6DOF_FLAGS_AXIS_SHIFT
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 btAtan2(btScalar x, btScalar y)
static T sum(const btAlignedObjectArray< T > &items)
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()
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...
void applyTorqueImpulse(const btVector3 &torque)
const btVector3 & getAngularVelocity() const
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
btScalar getInvMass() const
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
const btTransform & getCenterOfMassTransform() const
const btVector3 & getLinearVelocity() const
const btVector3 & getCenterOfMassPosition() const
const btVector3 & getInvInertiaDiagLocal() 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
joint limit
btScalar m_currentLimitError
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_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
Softness for linear limit.
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
SyclQueue void void size_t num_bytes void
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)
btScalar * m_J2linearAxis
btScalar * m_J2angularAxis
btScalar * m_J1angularAxis
btScalar * m_constraintError
btScalar * m_J1linearAxis