| /* | |
| Bullet Continuous Collision Detection and Physics Library | |
| btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios | |
| This software is provided 'as-is', without any express or implied warranty. | |
| In no event will the authors be held liable for any damages arising from the use of this software. | |
| Permission is granted to anyone to use this software for any purpose, | |
| including commercial applications, and to alter it and redistribute it freely, | |
| subject to the following restrictions: | |
| 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. | |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | |
| 3. This notice may not be removed or altered from any source distribution. | |
| Written by: Marcus Hennix | |
| */ | |
| #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" | |
| #include "BulletDynamics/Dynamics/btRigidBody.h" | |
| #include "LinearMath/btTransformUtil.h" | |
| #include "LinearMath/btMinMax.h" | |
| #include <new> | |
| //#define CONETWIST_USE_OBSOLETE_SOLVER true | |
| #define CONETWIST_USE_OBSOLETE_SOLVER false | |
| #define CONETWIST_DEF_FIX_THRESH btScalar(.05f) | |
| SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld) | |
| { | |
| btVector3 vec = axis * invInertiaWorld; | |
| return axis.dot(vec); | |
| } | |
| btConeTwistConstraint::btConeTwistConstraint() | |
| :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE), | |
| m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) | |
| { | |
| } | |
| btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, | |
| const btTransform& rbAFrame,const btTransform& rbBFrame) | |
| :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), | |
| m_angularOnly(false), | |
| m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) | |
| { | |
| init(); | |
| } | |
| btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) | |
| :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), | |
| m_angularOnly(false), | |
| m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) | |
| { | |
| m_rbBFrame = m_rbAFrame; | |
| init(); | |
| } | |
| void btConeTwistConstraint::init() | |
| { | |
| m_angularOnly = false; | |
| m_solveTwistLimit = false; | |
| m_solveSwingLimit = false; | |
| m_bMotorEnabled = false; | |
| m_maxMotorImpulse = btScalar(-1); | |
| setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); | |
| m_damping = btScalar(0.01); | |
| m_fixThresh = CONETWIST_DEF_FIX_THRESH; | |
| } | |
| void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) | |
| { | |
| if (m_useSolveConstraintObsolete) | |
| { | |
| info->m_numConstraintRows = 0; | |
| info->nub = 0; | |
| } | |
| else | |
| { | |
| info->m_numConstraintRows = 3; | |
| info->nub = 3; | |
| calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); | |
| if(m_solveSwingLimit) | |
| { | |
| info->m_numConstraintRows++; | |
| info->nub--; | |
| if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) | |
| { | |
| info->m_numConstraintRows++; | |
| info->nub--; | |
| } | |
| } | |
| if(m_solveTwistLimit) | |
| { | |
| info->m_numConstraintRows++; | |
| info->nub--; | |
| } | |
| } | |
| } | |
| void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info) | |
| { | |
| //always reserve 6 rows: object transform is not available on SPU | |
| info->m_numConstraintRows = 6; | |
| info->nub = 0; | |
| } | |
| void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) | |
| { | |
| getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); | |
| } | |
| void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) | |
| { | |
| calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB); | |
| btAssert(!m_useSolveConstraintObsolete); | |
| // set jacobian | |
| info->m_J1linearAxis[0] = 1; | |
| info->m_J1linearAxis[info->rowskip+1] = 1; | |
| info->m_J1linearAxis[2*info->rowskip+2] = 1; | |
| btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin(); | |
| { | |
| btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); | |
| btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); | |
| btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); | |
| btVector3 a1neg = -a1; | |
| a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); | |
| } | |
| btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin(); | |
| { | |
| btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); | |
| btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); | |
| btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); | |
| a2.getSkewSymmetricMatrix(angular0,angular1,angular2); | |
| } | |
| // set right hand side | |
| btScalar k = info->fps * info->erp; | |
| int j; | |
| for (j=0; j<3; j++) | |
| { | |
| info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]); | |
| info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY; | |
| info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY; | |
| } | |
| int row = 3; | |
| int srow = row * info->rowskip; | |
| btVector3 ax1; | |
| // angular limits | |
| if(m_solveSwingLimit) | |
| { | |
| btScalar *J1 = info->m_J1angularAxis; | |
| btScalar *J2 = info->m_J2angularAxis; | |
| if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) | |
| { | |
| btTransform trA = transA*m_rbAFrame; | |
| btVector3 p = trA.getBasis().getColumn(1); | |
| btVector3 q = trA.getBasis().getColumn(2); | |
| int srow1 = srow + info->rowskip; | |
| J1[srow+0] = p[0]; | |
| J1[srow+1] = p[1]; | |
| J1[srow+2] = p[2]; | |
| J1[srow1+0] = q[0]; | |
| J1[srow1+1] = q[1]; | |
| J1[srow1+2] = q[2]; | |
| J2[srow+0] = -p[0]; | |
| J2[srow+1] = -p[1]; | |
| J2[srow+2] = -p[2]; | |
| J2[srow1+0] = -q[0]; | |
| J2[srow1+1] = -q[1]; | |
| J2[srow1+2] = -q[2]; | |
| btScalar fact = info->fps * m_relaxationFactor; | |
| info->m_constraintError[srow] = fact * m_swingAxis.dot(p); | |
| info->m_constraintError[srow1] = fact * m_swingAxis.dot(q); | |
| info->m_lowerLimit[srow] = -SIMD_INFINITY; | |
| info->m_upperLimit[srow] = SIMD_INFINITY; | |
| info->m_lowerLimit[srow1] = -SIMD_INFINITY; | |
| info->m_upperLimit[srow1] = SIMD_INFINITY; | |
| srow = srow1 + info->rowskip; | |
| } | |
| else | |
| { | |
| ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor; | |
| J1[srow+0] = ax1[0]; | |
| J1[srow+1] = ax1[1]; | |
| J1[srow+2] = ax1[2]; | |
| J2[srow+0] = -ax1[0]; | |
| J2[srow+1] = -ax1[1]; | |
| J2[srow+2] = -ax1[2]; | |
| btScalar k = info->fps * m_biasFactor; | |
| info->m_constraintError[srow] = k * m_swingCorrection; | |
| info->cfm[srow] = 0.0f; | |
| // m_swingCorrection is always positive or 0 | |
| info->m_lowerLimit[srow] = 0; | |
| info->m_upperLimit[srow] = SIMD_INFINITY; | |
| srow += info->rowskip; | |
| } | |
| } | |
| if(m_solveTwistLimit) | |
| { | |
| ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor; | |
| btScalar *J1 = info->m_J1angularAxis; | |
| btScalar *J2 = info->m_J2angularAxis; | |
| J1[srow+0] = ax1[0]; | |
| J1[srow+1] = ax1[1]; | |
| J1[srow+2] = ax1[2]; | |
| J2[srow+0] = -ax1[0]; | |
| J2[srow+1] = -ax1[1]; | |
| J2[srow+2] = -ax1[2]; | |
| btScalar k = info->fps * m_biasFactor; | |
| info->m_constraintError[srow] = k * m_twistCorrection; | |
| info->cfm[srow] = 0.0f; | |
| if(m_twistSpan > 0.0f) | |
| { | |
| if(m_twistCorrection > 0.0f) | |
| { | |
| info->m_lowerLimit[srow] = 0; | |
| info->m_upperLimit[srow] = SIMD_INFINITY; | |
| } | |
| else | |
| { | |
| info->m_lowerLimit[srow] = -SIMD_INFINITY; | |
| info->m_upperLimit[srow] = 0; | |
| } | |
| } | |
| else | |
| { | |
| info->m_lowerLimit[srow] = -SIMD_INFINITY; | |
| info->m_upperLimit[srow] = SIMD_INFINITY; | |
| } | |
| srow += info->rowskip; | |
| } | |
| } | |
| void btConeTwistConstraint::buildJacobian() | |
| { | |
| if (m_useSolveConstraintObsolete) | |
| { | |
| m_appliedImpulse = btScalar(0.); | |
| m_accTwistLimitImpulse = btScalar(0.); | |
| m_accSwingLimitImpulse = btScalar(0.); | |
| m_accMotorImpulse = btVector3(0.,0.,0.); | |
| if (!m_angularOnly) | |
| { | |
| 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]); | |
| 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(), | |
| pivotBInW - m_rbB.getCenterOfMassPosition(), | |
| normal[i], | |
| m_rbA.getInvInertiaDiagLocal(), | |
| m_rbA.getInvMass(), | |
| m_rbB.getInvInertiaDiagLocal(), | |
| m_rbB.getInvMass()); | |
| } | |
| } | |
| calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); | |
| } | |
| } | |
| void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) | |
| { | |
| #ifndef __SPU__ | |
| if (m_useSolveConstraintObsolete) | |
| { | |
| btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); | |
| btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); | |
| 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; | |
| bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); | |
| btVector3 vel2; | |
| 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 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); | |
| } | |
| } | |
| // apply motor | |
| if (m_bMotorEnabled) | |
| { | |
| // compute current and predicted transforms | |
| btTransform trACur = m_rbA.getCenterOfMassTransform(); | |
| btTransform trBCur = m_rbB.getCenterOfMassTransform(); | |
| btVector3 omegaA; bodyA.getAngularVelocity(omegaA); | |
| btVector3 omegaB; bodyB.getAngularVelocity(omegaB); | |
| btTransform trAPred; trAPred.setIdentity(); | |
| btVector3 zerovec(0,0,0); | |
| btTransformUtil::integrateTransform( | |
| trACur, zerovec, omegaA, timeStep, trAPred); | |
| btTransform trBPred; trBPred.setIdentity(); | |
| btTransformUtil::integrateTransform( | |
| trBCur, zerovec, omegaB, timeStep, trBPred); | |
| // compute desired transforms in world | |
| btTransform trPose(m_qTarget); | |
| btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse(); | |
| btTransform trADes = trBPred * trABDes; | |
| btTransform trBDes = trAPred * trABDes.inverse(); | |
| // compute desired omegas in world | |
| btVector3 omegaADes, omegaBDes; | |
| btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes); | |
| btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes); | |
| // compute delta omegas | |
| btVector3 dOmegaA = omegaADes - omegaA; | |
| btVector3 dOmegaB = omegaBDes - omegaB; | |
| // compute weighted avg axis of dOmega (weighting based on inertias) | |
| btVector3 axisA, axisB; | |
| btScalar kAxisAInv = 0, kAxisBInv = 0; | |
| if (dOmegaA.length2() > SIMD_EPSILON) | |
| { | |
| axisA = dOmegaA.normalized(); | |
| kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA); | |
| } | |
| if (dOmegaB.length2() > SIMD_EPSILON) | |
| { | |
| axisB = dOmegaB.normalized(); | |
| kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB); | |
| } | |
| btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB; | |
| static bool bDoTorque = true; | |
| if (bDoTorque && avgAxis.length2() > SIMD_EPSILON) | |
| { | |
| avgAxis.normalize(); | |
| kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis); | |
| kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis); | |
| btScalar kInvCombined = kAxisAInv + kAxisBInv; | |
| btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) / | |
| (kInvCombined * kInvCombined); | |
| if (m_maxMotorImpulse >= 0) | |
| { | |
| btScalar fMaxImpulse = m_maxMotorImpulse; | |
| if (m_bNormalizedMotorStrength) | |
| fMaxImpulse = fMaxImpulse/kAxisAInv; | |
| btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse; | |
| btScalar newUnclampedMag = newUnclampedAccImpulse.length(); | |
| if (newUnclampedMag > fMaxImpulse) | |
| { | |
| newUnclampedAccImpulse.normalize(); | |
| newUnclampedAccImpulse *= fMaxImpulse; | |
| impulse = newUnclampedAccImpulse - m_accMotorImpulse; | |
| } | |
| m_accMotorImpulse += impulse; | |
| } | |
| btScalar impulseMag = impulse.length(); | |
| btVector3 impulseAxis = impulse / impulseMag; | |
| bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); | |
| bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); | |
| } | |
| } | |
| else if (m_damping > SIMD_EPSILON) // no motor: do a little damping | |
| { | |
| btVector3 angVelA; bodyA.getAngularVelocity(angVelA); | |
| btVector3 angVelB; bodyB.getAngularVelocity(angVelB); | |
| btVector3 relVel = angVelB - angVelA; | |
| if (relVel.length2() > SIMD_EPSILON) | |
| { | |
| btVector3 relVelAxis = relVel.normalized(); | |
| btScalar m_kDamping = btScalar(1.) / | |
| (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) + | |
| getRigidBodyB().computeAngularImpulseDenominator(relVelAxis)); | |
| btVector3 impulse = m_damping * m_kDamping * relVel; | |
| btScalar impulseMag = impulse.length(); | |
| btVector3 impulseAxis = impulse / impulseMag; | |
| bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); | |
| bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); | |
| } | |
| } | |
| // joint limits | |
| { | |
| ///solve angular part | |
| btVector3 angVelA; | |
| bodyA.getAngularVelocity(angVelA); | |
| btVector3 angVelB; | |
| bodyB.getAngularVelocity(angVelB); | |
| // solve swing limit | |
| if (m_solveSwingLimit) | |
| { | |
| btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep; | |
| btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis); | |
| if (relSwingVel > 0) | |
| amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor; | |
| btScalar impulseMag = amplitude * m_kSwing; | |
| // Clamp the accumulated impulse | |
| btScalar temp = m_accSwingLimitImpulse; | |
| m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); | |
| impulseMag = m_accSwingLimitImpulse - temp; | |
| btVector3 impulse = m_swingAxis * impulseMag; | |
| // don't let cone response affect twist | |
| // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit) | |
| { | |
| btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA; | |
| btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple; | |
| impulse = impulseNoTwistCouple; | |
| } | |
| impulseMag = impulse.length(); | |
| btVector3 noTwistSwingAxis = impulse / impulseMag; | |
| bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); | |
| bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag); | |
| } | |
| // solve twist limit | |
| if (m_solveTwistLimit) | |
| { | |
| btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep; | |
| btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis ); | |
| if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important) | |
| amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor; | |
| btScalar impulseMag = amplitude * m_kTwist; | |
| // Clamp the accumulated impulse | |
| btScalar temp = m_accTwistLimitImpulse; | |
| m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); | |
| impulseMag = m_accTwistLimitImpulse - temp; | |
| btVector3 impulse = m_twistAxis * impulseMag; | |
| bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); | |
| bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); | |
| } | |
| } | |
| } | |
| #else | |
| btAssert(0); | |
| #endif //__SPU__ | |
| } | |
| void btConeTwistConstraint::updateRHS(btScalar timeStep) | |
| { | |
| (void)timeStep; | |
| } | |
| #ifndef __SPU__ | |
| void btConeTwistConstraint::calcAngleInfo() | |
| { | |
| m_swingCorrection = btScalar(0.); | |
| m_twistLimitSign = btScalar(0.); | |
| m_solveTwistLimit = false; | |
| m_solveSwingLimit = false; | |
| btVector3 b1Axis1,b1Axis2,b1Axis3; | |
| btVector3 b2Axis1,b2Axis2; | |
| b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); | |
| b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); | |
| btScalar swing1=btScalar(0.),swing2 = btScalar(0.); | |
| btScalar swx=btScalar(0.),swy = btScalar(0.); | |
| btScalar thresh = btScalar(10.); | |
| btScalar fact; | |
| // Get Frame into world space | |
| if (m_swingSpan1 >= btScalar(0.05f)) | |
| { | |
| b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); | |
| swx = b2Axis1.dot(b1Axis1); | |
| swy = b2Axis1.dot(b1Axis2); | |
| swing1 = btAtan2Fast(swy, swx); | |
| fact = (swy*swy + swx*swx) * thresh * thresh; | |
| fact = fact / (fact + btScalar(1.0)); | |
| swing1 *= fact; | |
| } | |
| if (m_swingSpan2 >= btScalar(0.05f)) | |
| { | |
| b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); | |
| swx = b2Axis1.dot(b1Axis1); | |
| swy = b2Axis1.dot(b1Axis3); | |
| swing2 = btAtan2Fast(swy, swx); | |
| fact = (swy*swy + swx*swx) * thresh * thresh; | |
| fact = fact / (fact + btScalar(1.0)); | |
| swing2 *= fact; | |
| } | |
| btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); | |
| btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); | |
| btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq; | |
| if (EllipseAngle > 1.0f) | |
| { | |
| m_swingCorrection = EllipseAngle-1.0f; | |
| m_solveSwingLimit = true; | |
| // Calculate necessary axis & factors | |
| m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); | |
| m_swingAxis.normalize(); | |
| btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; | |
| m_swingAxis *= swingAxisSign; | |
| } | |
| // Twist limits | |
| if (m_twistSpan >= btScalar(0.)) | |
| { | |
| btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1); | |
| btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); | |
| btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); | |
| btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); | |
| m_twistAngle = twist; | |
| // btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); | |
| btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.); | |
| if (twist <= -m_twistSpan*lockedFreeFactor) | |
| { | |
| m_twistCorrection = -(twist + m_twistSpan); | |
| m_solveTwistLimit = true; | |
| m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; | |
| m_twistAxis.normalize(); | |
| m_twistAxis *= -1.0f; | |
| } | |
| else if (twist > m_twistSpan*lockedFreeFactor) | |
| { | |
| m_twistCorrection = (twist - m_twistSpan); | |
| m_solveTwistLimit = true; | |
| m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; | |
| m_twistAxis.normalize(); | |
| } | |
| } | |
| } | |
| #endif //__SPU__ | |
| static btVector3 vTwist(1,0,0); // twist axis in constraint's space | |
| void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) | |
| { | |
| m_swingCorrection = btScalar(0.); | |
| m_twistLimitSign = btScalar(0.); | |
| m_solveTwistLimit = false; | |
| m_solveSwingLimit = false; | |
| // compute rotation of A wrt B (in constraint space) | |
| if (m_bMotorEnabled && (!m_useSolveConstraintObsolete)) | |
| { // it is assumed that setMotorTarget() was alredy called | |
| // and motor target m_qTarget is within constraint limits | |
| // TODO : split rotation to pure swing and pure twist | |
| // compute desired transforms in world | |
| btTransform trPose(m_qTarget); | |
| btTransform trA = transA * m_rbAFrame; | |
| btTransform trB = transB * m_rbBFrame; | |
| btTransform trDeltaAB = trB * trPose * trA.inverse(); | |
| btQuaternion qDeltaAB = trDeltaAB.getRotation(); | |
| btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); | |
| m_swingAxis = swingAxis; | |
| m_swingAxis.normalize(); | |
| m_swingCorrection = qDeltaAB.getAngle(); | |
| if(!btFuzzyZero(m_swingCorrection)) | |
| { | |
| m_solveSwingLimit = true; | |
| } | |
| return; | |
| } | |
| { | |
| // compute rotation of A wrt B (in constraint space) | |
| btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation(); | |
| btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation(); | |
| btQuaternion qAB = qB.inverse() * qA; | |
| // split rotation into cone and twist | |
| // (all this is done from B's perspective. Maybe I should be averaging axes...) | |
| btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); | |
| btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); | |
| btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); | |
| if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh) | |
| { | |
| btScalar swingAngle, swingLimit = 0; btVector3 swingAxis; | |
| computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit); | |
| if (swingAngle > swingLimit * m_limitSoftness) | |
| { | |
| m_solveSwingLimit = true; | |
| // compute limit ratio: 0->1, where | |
| // 0 == beginning of soft limit | |
| // 1 == hard/real limit | |
| m_swingLimitRatio = 1.f; | |
| if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON) | |
| { | |
| m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/ | |
| (swingLimit - swingLimit * m_limitSoftness); | |
| } | |
| // swing correction tries to get back to soft limit | |
| m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness); | |
| // adjustment of swing axis (based on ellipse normal) | |
| adjustSwingAxisToUseEllipseNormal(swingAxis); | |
| // Calculate necessary axis & factors | |
| m_swingAxis = quatRotate(qB, -swingAxis); | |
| m_twistAxisA.setValue(0,0,0); | |
| m_kSwing = btScalar(1.) / | |
| (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) + | |
| computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB)); | |
| } | |
| } | |
| else | |
| { | |
| // you haven't set any limits; | |
| // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?) | |
| // anyway, we have either hinge or fixed joint | |
| btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); | |
| btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); | |
| btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2); | |
| btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0); | |
| btVector3 target; | |
| btScalar x = ivB.dot(ivA); | |
| btScalar y = ivB.dot(jvA); | |
| btScalar z = ivB.dot(kvA); | |
| if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) | |
| { // fixed. We'll need to add one more row to constraint | |
| if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) | |
| { | |
| m_solveSwingLimit = true; | |
| m_swingAxis = -ivB.cross(ivA); | |
| } | |
| } | |
| else | |
| { | |
| if(m_swingSpan1 < m_fixThresh) | |
| { // hinge around Y axis | |
| if(!(btFuzzyZero(y))) | |
| { | |
| m_solveSwingLimit = true; | |
| if(m_swingSpan2 >= m_fixThresh) | |
| { | |
| y = btScalar(0.f); | |
| btScalar span2 = btAtan2(z, x); | |
| if(span2 > m_swingSpan2) | |
| { | |
| x = btCos(m_swingSpan2); | |
| z = btSin(m_swingSpan2); | |
| } | |
| else if(span2 < -m_swingSpan2) | |
| { | |
| x = btCos(m_swingSpan2); | |
| z = -btSin(m_swingSpan2); | |
| } | |
| } | |
| } | |
| } | |
| else | |
| { // hinge around Z axis | |
| if(!btFuzzyZero(z)) | |
| { | |
| m_solveSwingLimit = true; | |
| if(m_swingSpan1 >= m_fixThresh) | |
| { | |
| z = btScalar(0.f); | |
| btScalar span1 = btAtan2(y, x); | |
| if(span1 > m_swingSpan1) | |
| { | |
| x = btCos(m_swingSpan1); | |
| y = btSin(m_swingSpan1); | |
| } | |
| else if(span1 < -m_swingSpan1) | |
| { | |
| x = btCos(m_swingSpan1); | |
| y = -btSin(m_swingSpan1); | |
| } | |
| } | |
| } | |
| } | |
| target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0]; | |
| target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1]; | |
| target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2]; | |
| target.normalize(); | |
| m_swingAxis = -ivB.cross(target); | |
| m_swingCorrection = m_swingAxis.length(); | |
| m_swingAxis.normalize(); | |
| } | |
| } | |
| if (m_twistSpan >= btScalar(0.f)) | |
| { | |
| btVector3 twistAxis; | |
| computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis); | |
| if (m_twistAngle > m_twistSpan*m_limitSoftness) | |
| { | |
| m_solveTwistLimit = true; | |
| m_twistLimitRatio = 1.f; | |
| if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON) | |
| { | |
| m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/ | |
| (m_twistSpan - m_twistSpan * m_limitSoftness); | |
| } | |
| // twist correction tries to get back to soft limit | |
| m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness); | |
| m_twistAxis = quatRotate(qB, -twistAxis); | |
| m_kTwist = btScalar(1.) / | |
| (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) + | |
| computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB)); | |
| } | |
| if (m_solveSwingLimit) | |
| m_twistAxisA = quatRotate(qA, -twistAxis); | |
| } | |
| else | |
| { | |
| m_twistAngle = btScalar(0.f); | |
| } | |
| } | |
| } | |
| // given a cone rotation in constraint space, (pre: twist must already be removed) | |
| // this method computes its corresponding swing angle and axis. | |
| // more interestingly, it computes the cone/swing limit (angle) for this cone "pose". | |
| void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, | |
| btScalar& swingAngle, // out | |
| btVector3& vSwingAxis, // out | |
| btScalar& swingLimit) // out | |
| { | |
| swingAngle = qCone.getAngle(); | |
| if (swingAngle > SIMD_EPSILON) | |
| { | |
| vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z()); | |
| vSwingAxis.normalize(); | |
| if (fabs(vSwingAxis.x()) > SIMD_EPSILON) | |
| { | |
| // non-zero twist?! this should never happen. | |
| int wtf = 0; wtf = wtf; | |
| } | |
| // Compute limit for given swing. tricky: | |
| // Given a swing axis, we're looking for the intersection with the bounding cone ellipse. | |
| // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.) | |
| // For starters, compute the direction from center to surface of ellipse. | |
| // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis. | |
| // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.) | |
| btScalar xEllipse = vSwingAxis.y(); | |
| btScalar yEllipse = -vSwingAxis.z(); | |
| // Now, we use the slope of the vector (using x/yEllipse) and find the length | |
| // of the line that intersects the ellipse: | |
| // x^2 y^2 | |
| // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) | |
| // a^2 b^2 | |
| // Do the math and it should be clear. | |
| swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1 | |
| if (fabs(xEllipse) > SIMD_EPSILON) | |
| { | |
| btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); | |
| btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); | |
| norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); | |
| btScalar swingLimit2 = (1 + surfaceSlope2) / norm; | |
| swingLimit = sqrt(swingLimit2); | |
| } | |
| // test! | |
| /*swingLimit = m_swingSpan2; | |
| if (fabs(vSwingAxis.z()) > SIMD_EPSILON) | |
| { | |
| btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2; | |
| btScalar sinphi = m_swingSpan2 / sqrt(mag_2); | |
| btScalar phi = asin(sinphi); | |
| btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z())); | |
| btScalar alpha = 3.14159f - theta - phi; | |
| btScalar sinalpha = sin(alpha); | |
| swingLimit = m_swingSpan1 * sinphi/sinalpha; | |
| }*/ | |
| } | |
| else if (swingAngle < 0) | |
| { | |
| // this should never happen! | |
| int wtf = 0; wtf = wtf; | |
| } | |
| } | |
| btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const | |
| { | |
| // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone) | |
| btScalar xEllipse = btCos(fAngleInRadians); | |
| btScalar yEllipse = btSin(fAngleInRadians); | |
| // Use the slope of the vector (using x/yEllipse) and find the length | |
| // of the line that intersects the ellipse: | |
| // x^2 y^2 | |
| // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) | |
| // a^2 b^2 | |
| // Do the math and it should be clear. | |
| float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1) | |
| if (fabs(xEllipse) > SIMD_EPSILON) | |
| { | |
| btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); | |
| btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); | |
| norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); | |
| btScalar swingLimit2 = (1 + surfaceSlope2) / norm; | |
| swingLimit = sqrt(swingLimit2); | |
| } | |
| // convert into point in constraint space: | |
| // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively | |
| btVector3 vSwingAxis(0, xEllipse, -yEllipse); | |
| btQuaternion qSwing(vSwingAxis, swingLimit); | |
| btVector3 vPointInConstraintSpace(fLength,0,0); | |
| return quatRotate(qSwing, vPointInConstraintSpace); | |
| } | |
| // given a twist rotation in constraint space, (pre: cone must already be removed) | |
| // this method computes its corresponding angle and axis. | |
| void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, | |
| btScalar& twistAngle, // out | |
| btVector3& vTwistAxis) // out | |
| { | |
| btQuaternion qMinTwist = qTwist; | |
| twistAngle = qTwist.getAngle(); | |
| if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. | |
| { | |
| qMinTwist = operator-(qTwist); | |
| twistAngle = qMinTwist.getAngle(); | |
| } | |
| if (twistAngle < 0) | |
| { | |
| // this should never happen | |
| int wtf = 0; wtf = wtf; | |
| } | |
| vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); | |
| if (twistAngle > SIMD_EPSILON) | |
| vTwistAxis.normalize(); | |
| } | |
| void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const | |
| { | |
| // the swing axis is computed as the "twist-free" cone rotation, | |
| // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2). | |
| // so, if we're outside the limits, the closest way back inside the cone isn't | |
| // along the vector back to the center. better (and more stable) to use the ellipse normal. | |
| // convert swing axis to direction from center to surface of ellipse | |
| // (ie. rotate 2D vector by PI/2) | |
| btScalar y = -vSwingAxis.z(); | |
| btScalar z = vSwingAxis.y(); | |
| // do the math... | |
| if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0. | |
| { | |
| // compute gradient/normal of ellipse surface at current "point" | |
| btScalar grad = y/z; | |
| grad *= m_swingSpan2 / m_swingSpan1; | |
| // adjust y/z to represent normal at point (instead of vector to point) | |
| if (y > 0) | |
| y = fabs(grad * z); | |
| else | |
| y = -fabs(grad * z); | |
| // convert ellipse direction back to swing axis | |
| vSwingAxis.setZ(-y); | |
| vSwingAxis.setY( z); | |
| vSwingAxis.normalize(); | |
| } | |
| } | |
| void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) | |
| { | |
| btTransform trACur = m_rbA.getCenterOfMassTransform(); | |
| btTransform trBCur = m_rbB.getCenterOfMassTransform(); | |
| btTransform trABCur = trBCur.inverse() * trACur; | |
| btQuaternion qABCur = trABCur.getRotation(); | |
| btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); | |
| btQuaternion qConstraintCur = trConstraintCur.getRotation(); | |
| btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation(); | |
| setMotorTargetInConstraintSpace(qConstraint); | |
| } | |
| void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q) | |
| { | |
| m_qTarget = q; | |
| // clamp motor target to within limits | |
| { | |
| btScalar softness = 1.f;//m_limitSoftness; | |
| // split into twist and cone | |
| btVector3 vTwisted = quatRotate(m_qTarget, vTwist); | |
| btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize(); | |
| btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize(); | |
| // clamp cone | |
| if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f)) | |
| { | |
| btScalar swingAngle, swingLimit; btVector3 swingAxis; | |
| computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit); | |
| if (fabs(swingAngle) > SIMD_EPSILON) | |
| { | |
| if (swingAngle > swingLimit*softness) | |
| swingAngle = swingLimit*softness; | |
| else if (swingAngle < -swingLimit*softness) | |
| swingAngle = -swingLimit*softness; | |
| qTargetCone = btQuaternion(swingAxis, swingAngle); | |
| } | |
| } | |
| // clamp twist | |
| if (m_twistSpan >= btScalar(0.05f)) | |
| { | |
| btScalar twistAngle; btVector3 twistAxis; | |
| computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis); | |
| if (fabs(twistAngle) > SIMD_EPSILON) | |
| { | |
| // eddy todo: limitSoftness used here??? | |
| if (twistAngle > m_twistSpan*softness) | |
| twistAngle = m_twistSpan*softness; | |
| else if (twistAngle < -m_twistSpan*softness) | |
| twistAngle = -m_twistSpan*softness; | |
| qTargetTwist = btQuaternion(twistAxis, twistAngle); | |
| } | |
| } | |
| m_qTarget = qTargetCone * qTargetTwist; | |
| } | |
| } | |