diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp')
-rw-r--r-- | extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp | 667 |
1 files changed, 462 insertions, 205 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index a0523a8c76b..b6b34305804 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -19,19 +19,33 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" #include "LinearMath/btMinMax.h" #include <new> +#include "btSolverBody.h" + +//----------------------------------------------------------------------------- + +#define HINGE_USE_OBSOLETE_SOLVER false + +//----------------------------------------------------------------------------- btHingeConstraint::btHingeConstraint() : btTypedConstraint (HINGE_CONSTRAINT_TYPE), -m_enableAngularMotor(false) +m_enableAngularMotor(false), +m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), +m_useReferenceFrameA(false) { + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- + btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, - btVector3& axisInA,btVector3& axisInB) + btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), m_angularOnly(false), - m_enableAngularMotor(false) + m_enableAngularMotor(false), + m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), + m_useReferenceFrameA(useReferenceFrameA) { m_rbAFrame.getOrigin() = pivotInA; @@ -60,9 +74,9 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = pivotInB; - m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(), - rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(), - rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() ); + m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), + rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), + rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); //start with free m_lowerLimit = btScalar(1e30); @@ -71,32 +85,28 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; - + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- -btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) -:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false) +btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA) +:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), +m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), +m_useReferenceFrameA(useReferenceFrameA) { // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace - btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); - btScalar projection = rbAxisA1.dot(axisInA); - if (projection > SIMD_EPSILON) - rbAxisA1 = rbAxisA1*projection - axisInA; - else - rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); - - btVector3 rbAxisA2 = axisInA.cross(rbAxisA1); + btVector3 rbAxisA1, rbAxisA2; + btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); - - btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA; + btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); @@ -115,19 +125,19 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA, m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- + btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, - const btTransform& rbAFrame, const btTransform& rbBFrame) + const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), m_angularOnly(false), -m_enableAngularMotor(false) +m_enableAngularMotor(false), +m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), +m_useReferenceFrameA(useReferenceFrameA) { - // flip axis - m_rbBFrame.getBasis()[0][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); - //start with free m_lowerLimit = btScalar(1e30); m_upperLimit = btScalar(-1e30); @@ -135,22 +145,20 @@ m_enableAngularMotor(false) m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- - -btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame) +btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), m_angularOnly(false), -m_enableAngularMotor(false) +m_enableAngularMotor(false), +m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), +m_useReferenceFrameA(useReferenceFrameA) { ///not providing rigidbody B means implicitly using worldspace for body B - // flip axis - m_rbBFrame.getBasis()[0][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); - m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); //start with free @@ -160,33 +168,38 @@ m_enableAngularMotor(false) m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- + void btHingeConstraint::buildJacobian() { - m_appliedImpulse = btScalar(0.); - - if (!m_angularOnly) + if (m_useSolveConstraintObsolete) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btVector3 relPos = pivotBInW - pivotAInW; + m_appliedImpulse = btScalar(0.); - btVector3 normal[3]; - if (relPos.length2() > SIMD_EPSILON) + if (!m_angularOnly) { - normal[0] = relPos.normalized(); - } - else - { - normal[0].setValue(btScalar(1.0),0,0); - } + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + btVector3 relPos = pivotBInW - pivotAInW; + + btVector3 normal[3]; + if (relPos.length2() > SIMD_EPSILON) + { + normal[0] = relPos.normalized(); + } + else + { + normal[0].setValue(btScalar(1.0),0,0); + } - btPlaneSpace1(normal[0], normal[1], normal[2]); + btPlaneSpace1(normal[0], normal[1], normal[2]); - for (int i=0;i<3;i++) - { - new (&m_jac[i]) btJacobianEntry( + for (int i=0;i<3;i++) + { + new (&m_jac[i]) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), pivotAInW - m_rbA.getCenterOfMassPosition(), @@ -196,214 +209,458 @@ void btHingeConstraint::buildJacobian() m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); + } } - } - //calculate two perpendicular jointAxis, orthogonal to hingeAxis - //these two jointAxis require equal angular velocities for both bodies + //calculate two perpendicular jointAxis, orthogonal to hingeAxis + //these two jointAxis require equal angular velocities for both bodies - //this is unused for now, it's a todo - btVector3 jointAxis0local; - btVector3 jointAxis1local; - - btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); - - getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; - btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; - btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + //this is unused for now, it's a todo + btVector3 jointAxis0local; + btVector3 jointAxis1local; - new (&m_jacAng[0]) btJacobianEntry(jointAxis0, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - new (&m_jacAng[1]) btJacobianEntry(jointAxis1, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); + btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); + getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; + btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; + btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + + new (&m_jacAng[0]) btJacobianEntry(jointAxis0, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + new (&m_jacAng[1]) btJacobianEntry(jointAxis1, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + // clear accumulator + m_accLimitImpulse = btScalar(0.); + + // test angular limit + testLimit(); + + //Compute K = J*W*J' for hinge axis + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + + getRigidBodyB().computeAngularImpulseDenominator(axisA)); - // Compute limit information - btScalar hingeAngle = getHingeAngle(); + } +} - //set bias, sign, clear accumulator - m_correction = btScalar(0.); - m_limitSign = btScalar(0.); - m_solveLimit = false; - m_accLimitImpulse = btScalar(0.); +//----------------------------------------------------------------------------- -// if (m_lowerLimit < m_upperLimit) - if (m_lowerLimit <= m_upperLimit) +void btHingeConstraint::getInfo1(btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) { -// if (hingeAngle <= m_lowerLimit*m_limitSoftness) - if (hingeAngle <= m_lowerLimit) - { - m_correction = (m_lowerLimit - hingeAngle); - m_limitSign = 1.0f; - m_solveLimit = true; - } -// else if (hingeAngle >= m_upperLimit*m_limitSoftness) - else if (hingeAngle >= m_upperLimit) + info->m_numConstraintRows = 0; + info->nub = 0; + } + else + { + info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular + info->nub = 1; + //prepare constraint + testLimit(); + if(getSolveLimit() || getEnableAngularMotor()) { - m_correction = m_upperLimit - hingeAngle; - m_limitSign = -1.0f; - m_solveLimit = true; + info->m_numConstraintRows++; // limit 3rd anguar as well + info->nub--; } } +} // btHingeConstraint::getInfo1 () - //Compute K = J*W*J' for hinge axis - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + - getRigidBodyB().computeAngularImpulseDenominator(axisA)); +//----------------------------------------------------------------------------- +void btHingeConstraint::getInfo2 (btConstraintInfo2* info) +{ + btAssert(!m_useSolveConstraintObsolete); + int i, s = info->rowskip; + // transforms in world space + btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame; + btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame; + // pivot point + btVector3 pivotAInW = trA.getOrigin(); + btVector3 pivotBInW = trB.getOrigin(); + // linear (all fixed) + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[s + 1] = 1; + info->m_J1linearAxis[2 * s + 2] = 1; + btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin(); + { + btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s); + btVector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin(); + { + btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + // linear RHS + btScalar k = info->fps * info->erp; + for(i = 0; i < 3; i++) + { + info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]); + } + // make rotations around X and Y equal + // the hinge axis should be the only unconstrained + // rotational axis, the angular velocity of the two bodies perpendicular to + // the hinge axis should be equal. thus the constraint equations are + // p*w1 - p*w2 = 0 + // q*w1 - q*w2 = 0 + // where p and q are unit vectors normal to the hinge axis, and w1 and w2 + // are the angular velocity vectors of the two bodies. + // get hinge axis (Z) + btVector3 ax1 = trA.getBasis().getColumn(2); + // get 2 orthos to hinge axis (X, Y) + btVector3 p = trA.getBasis().getColumn(0); + btVector3 q = trA.getBasis().getColumn(1); + // set the two hinge angular rows + int s3 = 3 * info->rowskip; + int s4 = 4 * info->rowskip; + + info->m_J1angularAxis[s3 + 0] = p[0]; + info->m_J1angularAxis[s3 + 1] = p[1]; + info->m_J1angularAxis[s3 + 2] = p[2]; + info->m_J1angularAxis[s4 + 0] = q[0]; + info->m_J1angularAxis[s4 + 1] = q[1]; + info->m_J1angularAxis[s4 + 2] = q[2]; + + info->m_J2angularAxis[s3 + 0] = -p[0]; + info->m_J2angularAxis[s3 + 1] = -p[1]; + info->m_J2angularAxis[s3 + 2] = -p[2]; + info->m_J2angularAxis[s4 + 0] = -q[0]; + info->m_J2angularAxis[s4 + 1] = -q[1]; + info->m_J2angularAxis[s4 + 2] = -q[2]; + // compute the right hand side of the constraint equation. set relative + // body velocities along p and q to bring the hinge back into alignment. + // if ax1,ax2 are the unit length hinge axes as computed from body1 and + // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if `theta' is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + btVector3 ax2 = trB.getBasis().getColumn(2); + btVector3 u = ax1.cross(ax2); + info->m_constraintError[s3] = k * u.dot(p); + info->m_constraintError[s4] = k * u.dot(q); + // check angular limits + int nrow = 4; // last filled row + int srow; + btScalar limit_err = btScalar(0.0); + int limit = 0; + if(getSolveLimit()) + { + limit_err = m_correction * m_referenceSign; + limit = (limit_err > btScalar(0.0)) ? 1 : 2; + } + // if the hinge has joint limits or motor, add in the extra row + int powered = 0; + if(getEnableAngularMotor()) + { + powered = 1; + } + if(limit || powered) + { + nrow++; + srow = nrow * info->rowskip; + info->m_J1angularAxis[srow+0] = ax1[0]; + info->m_J1angularAxis[srow+1] = ax1[1]; + info->m_J1angularAxis[srow+2] = ax1[2]; + + info->m_J2angularAxis[srow+0] = -ax1[0]; + info->m_J2angularAxis[srow+1] = -ax1[1]; + info->m_J2angularAxis[srow+2] = -ax1[2]; + + btScalar lostop = getLowerLimit(); + btScalar histop = getUpperLimit(); + if(limit && (lostop == histop)) + { // the joint motor is ineffective + powered = 0; + } + info->m_constraintError[srow] = btScalar(0.0f); + if(powered) + { + info->cfm[srow] = btScalar(0.0); + btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp); + info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; + info->m_lowerLimit[srow] = - m_maxMotorImpulse; + info->m_upperLimit[srow] = m_maxMotorImpulse; + } + if(limit) + { + k = info->fps * info->erp; + info->m_constraintError[srow] += k * limit_err; + info->cfm[srow] = btScalar(0.0); + if(lostop == histop) + { + // limited low and high simultaneously + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else if(limit == 1) + { // low limit + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { // high limit + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } + // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) + btScalar bounce = m_relaxationFactor; + if(bounce > btScalar(0.0)) + { + btScalar vel = m_rbA.getAngularVelocity().dot(ax1); + vel -= m_rbB.getAngularVelocity().dot(ax1); + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if(limit == 1) + { // low limit + if(vel < 0) + { + btScalar newc = -bounce * vel; + if(newc > info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + else + { // high limit - all those computations are reversed + if(vel > 0) + { + btScalar newc = -bounce * vel; + if(newc < info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + } + info->m_constraintError[srow] *= m_biasFactor; + } // if(limit) + } // if angular limit or powered } -void btHingeConstraint::solveConstraint(btScalar timeStep) +//----------------------------------------------------------------------------- + +void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + if (m_useSolveConstraintObsolete) + { + + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btScalar tau = btScalar(0.3); + btScalar tau = btScalar(0.3); - //linear part - if (!m_angularOnly) - { - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - for (int i=0;i<3;i++) - { - const btVector3& normal = m_jac[i].m_linearJointAxis; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - - btScalar rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - btVector3 impulse_vector = normal * impulse; - m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); - m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); + //linear part + if (!m_angularOnly) + { + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 vel1,vel2; + bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel = vel1 - vel2; + + for (int i=0;i<3;i++) + { + const btVector3& normal = m_jac[i].m_linearJointAxis; + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); + + btScalar rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; + m_appliedImpulse += impulse; + btVector3 impulse_vector = normal * impulse; + btVector3 ftorqueAxis1 = rel_pos1.cross(normal); + btVector3 ftorqueAxis2 = rel_pos2.cross(normal); + bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); + bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); + } } - } - - { - ///solve angular part + + { + ///solve angular part - // get axes in world space - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); + // get axes in world space + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); - const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); - const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); + btVector3 angVelA; + bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.getAngularVelocity(angVelB); - btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); - btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); + btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); + btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); - btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; - btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; - btVector3 velrelOrthog = angAorthog-angBorthog; - { - //solve orthogonal angular velocity correction - btScalar relaxation = btScalar(1.); - btScalar len = velrelOrthog.length(); - if (len > btScalar(0.00001)) + btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; + btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; + btVector3 velrelOrthog = angAorthog-angBorthog; { - btVector3 normal = velrelOrthog.normalized(); - btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + - getRigidBodyB().computeAngularImpulseDenominator(normal); - // scale for mass and relaxation - //todo: expose this 0.9 factor to developer - velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; - } + - //solve angular positional correction - btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep); - btScalar len2 = angularError.length(); - if (len2>btScalar(0.00001)) - { - btVector3 normal2 = angularError.normalized(); - btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + - getRigidBodyB().computeAngularImpulseDenominator(normal2); - angularError *= (btScalar(1.)/denom2) * relaxation; - } + //solve orthogonal angular velocity correction + btScalar relaxation = btScalar(1.); + btScalar len = velrelOrthog.length(); + if (len > btScalar(0.00001)) + { + btVector3 normal = velrelOrthog.normalized(); + btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + + getRigidBodyB().computeAngularImpulseDenominator(normal); + // scale for mass and relaxation + //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; - m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); - m_rbB.applyTorqueImpulse(velrelOrthog-angularError); + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom)); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom)); - // solve limit - if (m_solveLimit) - { - btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; + } - btScalar impulseMag = amplitude * m_kHinge; + //solve angular positional correction + btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep); + btScalar len2 = angularError.length(); + if (len2>btScalar(0.00001)) + { + btVector3 normal2 = angularError.normalized(); + btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + + getRigidBodyB().computeAngularImpulseDenominator(normal2); + //angularError *= (btScalar(1.)/denom2) * relaxation; + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2)); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2)); - // Clamp the accumulated impulse - btScalar temp = m_accLimitImpulse; - m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); - impulseMag = m_accLimitImpulse - temp; + } + + - btVector3 impulse = axisA * impulseMag * m_limitSign; - m_rbA.applyTorqueImpulse(impulse); - m_rbB.applyTorqueImpulse(-impulse); - } - } - //apply motor - if (m_enableAngularMotor) - { - //todo: add limits too - btVector3 angularLimit(0,0,0); + // solve limit + if (m_solveLimit) + { + btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; + + btScalar impulseMag = amplitude * m_kHinge; + + // Clamp the accumulated impulse + btScalar temp = m_accLimitImpulse; + m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); + impulseMag = m_accLimitImpulse - temp; + + + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign)); + + } + } - btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; - btScalar projRelVel = velrel.dot(axisA); + //apply motor + if (m_enableAngularMotor) + { + //todo: add limits too + btVector3 angularLimit(0,0,0); - btScalar desiredMotorVel = m_motorTargetVelocity; - btScalar motor_relvel = desiredMotorVel - projRelVel; + btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; + btScalar projRelVel = velrel.dot(axisA); - btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; - //todo: should clip against accumulated impulse - btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; - clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; - btVector3 motorImp = clippedMotorImpulse * axisA; + btScalar desiredMotorVel = m_motorTargetVelocity; + btScalar motor_relvel = desiredMotorVel - projRelVel; - m_rbA.applyTorqueImpulse(motorImp+angularLimit); - m_rbB.applyTorqueImpulse(-motorImp-angularLimit); + btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; + //todo: should clip against accumulated impulse + btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; + clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; + btVector3 motorImp = clippedMotorImpulse * axisA; + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse); + + } } } } +//----------------------------------------------------------------------------- + void btHingeConstraint::updateRHS(btScalar timeStep) { (void)timeStep; } +//----------------------------------------------------------------------------- + btScalar btHingeConstraint::getHingeAngle() { const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1); - - return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1) ); + btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); + return m_referenceSign * angle; } +//----------------------------------------------------------------------------- + +void btHingeConstraint::testLimit() +{ + // Compute limit information + m_hingeAngle = getHingeAngle(); + m_correction = btScalar(0.); + m_limitSign = btScalar(0.); + m_solveLimit = false; + if (m_lowerLimit <= m_upperLimit) + { + if (m_hingeAngle <= m_lowerLimit) + { + m_correction = (m_lowerLimit - m_hingeAngle); + m_limitSign = 1.0f; + m_solveLimit = true; + } + else if (m_hingeAngle >= m_upperLimit) + { + m_correction = m_upperLimit - m_hingeAngle; + m_limitSign = -1.0f; + m_solveLimit = true; + } + } + return; +} // btHingeConstraint::testLimit() + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- |