24 #define HINGE_USE_OBSOLETE_SOLVER false
26 #define HINGE_USE_FRAME_OFFSET true
53 btScalar projection = axisInA.dot(rbAxisA1);
66 rbAxisA2 = axisInA.cross(rbAxisA1);
67 rbAxisA1 = rbAxisA2.cross(axisInA);
70 m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
71 rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
72 rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
76 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
79 m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
80 rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
81 rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
83 #ifndef _BT_USE_CENTER_LIMIT_
117 m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
118 rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
119 rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
125 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
128 m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
129 rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
130 rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
132 #ifndef _BT_USE_CENTER_LIMIT_
139 m_solveLimit =
false;
161 #ifndef _BT_USE_CENTER_LIMIT_
168 m_solveLimit =
false;
192 #ifndef _BT_USE_CENTER_LIMIT_
199 m_solveLimit =
false;
215 btVector3 relPos = pivotBInW - pivotAInW;
220 normal[0] = relPos.normalized();
229 for (
int i = 0; i < 3; i++)
319 return m_accumulatedAngle;
323 m_accumulatedAngle = accAngle;
440 btVector3 a1 = pivotAInW - transA.getOrigin();
446 a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
448 btVector3 a2 = pivotBInW - transB.getOrigin();
453 a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
461 for (i = 0; i < 3; i++)
475 btVector3 ax1 = trA.getBasis().getColumn(2);
477 btVector3 p = trA.getBasis().getColumn(0);
478 btVector3 q = trA.getBasis().getColumn(1);
511 btVector3 ax2 = trB.getBasis().getColumn(2);
522 #ifdef _BT_USE_CENTER_LIMIT_
527 limit = (limit_err >
btScalar(0.0)) ? 1 : 2;
531 if (limit || powered)
545 if (limit && (lostop == histop))
564 k = info->
fps * currERP;
570 if (lostop == histop)
587 #ifdef _BT_USE_CENTER_LIMIT_
595 vel -= angVelB.dot(ax1);
614 if (newc < info->m_constraintError[srow])
621 #ifdef _BT_USE_CENTER_LIMIT_
661 #ifdef _BT_USE_CENTER_LIMIT_
666 m_solveLimit =
false;
697 vNoHinge.normalize();
709 if (qHinge.getZ() < 0)
710 targetAngle = -targetAngle;
717 #ifdef _BT_USE_CENTER_LIMIT_
730 btScalar dAngle = targetAngle - curAngle;
746 btVector3 ofs = trB.getOrigin() - trA.getOrigin();
764 btVector3 ax1A = trA.getBasis().getColumn(2);
765 btVector3 ax1B = trB.getBasis().getColumn(2);
766 btVector3 ax1 = ax1A * factA + ax1B * factB;
771 ax1 = ax1A * factA + ax1B * factB;
784 relB = trB.getOrigin() - bodyB_trans.getOrigin();
790 relA = trA.getOrigin() - bodyA_trans.getOrigin();
795 relA = orthoA + totalDist * factA;
796 relB = orthoB - totalDist * factB;
798 p = orthoB * factA + orthoA * factB;
806 p = trA.getBasis().getColumn(1);
811 tmpA = relA.cross(p);
812 tmpB = relB.cross(p);
815 tmpA = relA.cross(q);
816 tmpB = relB.cross(q);
825 tmpA = relA.cross(ax1);
826 tmpB = relB.cross(ax1);
853 rhs = k * q.dot(ofs);
855 rhs = k * ax1.dot(ofs);
895 k = info->
fps * normalErp;
908 #ifdef _BT_USE_CENTER_LIMIT_
913 limit = (limit_err >
btScalar(0.0)) ? 1 : 2;
917 if (limit || powered)
931 if (limit && (lostop == histop))
950 k = info->
fps * currERP;
956 if (lostop == histop)
973 #ifdef _BT_USE_CENTER_LIMIT_
981 vel -= angVelB.dot(ax1);
1000 if (newc < info->m_constraintError[srow])
1007 #ifdef _BT_USE_CENTER_LIMIT_
1020 if ((axis == -1) || (axis == 5))
1054 if ((axis == -1) || (axis == 5))
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
btScalar m_maxMotorImpulse
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
btVector3 m_accMotorImpulse
const btRigidBody & getRigidBodyB() const
btScalar m_relaxationFactor
void getInfo1NonVirtual(btConstraintInfo1 *info)
void setMotorTarget(const btQuaternion &q)
btFixedConstraint btRigidBody & rbB
btJacobianEntry m_jacAng[3]
bool m_useOffsetForConstraintFrame
static btVector3 vHinge(0, 0, btScalar(1))
#define HINGE_USE_OBSOLETE_SOLVER
static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
static btScalar btNormalizeAnglePositive(btScalar angle)
static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
#define HINGE_USE_FRAME_OFFSET
void setAccumulatedHingeAngle(btScalar accAngle)
btScalar getHingeAngle()
The getHingeAngle gives the hinge angle in range [-PI,PI].
bool m_useReferenceFrameA
btScalar getLowerLimit() const
btHingeConstraint(btRigidBody &rbA, btRigidBody &rbB, const btVector3 &pivotInA, const btVector3 &pivotInB, const btVector3 &axisInA, const btVector3 &axisInB, bool useReferenceFrameA=false)
bool m_enableAngularMotor
btScalar getUpperLimit() const
btScalar getAccumulatedHingeAngle()
bool getEnableAngularMotor()
@ BT_HINGE_FLAGS_CFM_STOP
@ BT_HINGE_FLAGS_CFM_NORM
@ BT_HINGE_FLAGS_ERP_NORM
@ BT_HINGE_FLAGS_ERP_STOP
btScalar m_accLimitImpulse
btScalar m_motorTargetVelocity
void getInfo2Internal(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btVector3 &angVelA, const btVector3 &angVelB)
void getInfo2InternalUsingFrameOffset(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btVector3 &angVelA, const btVector3 &angVelB)
void testLimit(const btTransform &transA, const btTransform &transB)
#define _BT_USE_CENTER_LIMIT_
SIMD_FORCE_INLINE btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
SIMD_FORCE_INLINE btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
SIMD_FORCE_INLINE btScalar btFabs(btScalar x)
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y)
SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y)
btSimdScalar m_appliedImpulse
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)
SIMD_FORCE_INLINE void btPlaneSpace1(const T &n, T &p, T &q)
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...
btScalar getBiasFactor() const
Returns limit's bias factor.
btScalar getCorrection() const
Returns correction value evaluated when test() was invoked.
void test(const btScalar angle)
void fit(btScalar &angle) const
btScalar getRelaxationFactor() const
Returns limit's relaxation factor.
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
btQuaternion inverse() const
Return the inverse of this quaternion.
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
const btVector3 & getAngularVelocity() const
SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3 &axis) const
btScalar getInvMass() const
const btTransform & getCenterOfMassTransform() const
const btVector3 & getCenterOfMassPosition() const
const btVector3 & getInvInertiaDiagLocal() const
SyclQueue void void size_t num_bytes void
IconTextureDrawCall normal
btScalar * m_J2linearAxis
btScalar * m_J2angularAxis
btScalar * m_J1angularAxis
btScalar * m_constraintError
btScalar * m_J1linearAxis