/* void solveConstraintObsolete( btSolverBody bodyA, btSolverBody bodyB, double timeStep ) { if( m_useSolveConstraintObsolete ) { btVector3 pivotAInW = m_rbA.m_worldTransform * m_rbAFrame.m_origin; btVector3 pivotBInW = m_rbB.m_worldTransform * m_rbBFrame.m_origin; double tau = (double)( 0.3 ); //linear part if( !m_angularOnly ) { btVector3 rel_pos1 = pivotAInW - m_rbA.m_worldTransform.m_origin; btVector3 rel_pos2 = pivotBInW - m_rbB.m_worldTransform.m_origin; btVector3 vel1; bodyA.internalGetVelocityInLocalPointObsolete( rel_pos1, vel1 ); btVector3 vel2; bodyB.internalGetVelocityInLocalPointObsolete( rel_pos2, vel2 ); btVector3 vel = vel1 - vel2; for( int i = 0; i < 3; i++ ) { btIVector3 normal = m_jac[i].m_linearJointAxis; double jacDiagABInv = btScalar.BT_ONE / m_jac[i].getDiagonal(); double rel_vel; rel_vel = normal.dot( vel ); //positional error (zeroth order error) double depth = -( pivotAInW - pivotBInW ).dot( normal ); //this is the error projected on the normal double impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; btVector3 ftorqueAxis1 = rel_pos1.cross( normal ); btVector3 ftorqueAxis2 = rel_pos2.cross( normal ); bodyA.internalApplyImpulse( normal * m_rbA.getInvMass(), m_rbA.m_invInertiaTensorWorld * ftorqueAxis1, impulse ); bodyB.internalApplyImpulse( normal * m_rbB.getInvMass(), m_rbB.m_invInertiaTensorWorld * ftorqueAxis2, -impulse ); } } // apply motor if( m_bMotorEnabled ) { // compute current and predicted transforms btTransform trACur = m_rbA.m_worldTransform; btTransform trBCur = m_rbB.m_worldTransform; btVector3 omegaA; bodyA.internalGetAngularVelocity( omegaA ); btVector3 omegaB; bodyB.internalGetAngularVelocity( 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; double kAxisAInv = 0, kAxisBInv = 0; if( dOmegaA.length2() > btScalar.SIMD_EPSILON ) { axisA = dOmegaA.normalized(); kAxisAInv = m_rbA.computeAngularImpulseDenominator( axisA ); } if( dOmegaB.length2() > btScalar.SIMD_EPSILON ) { axisB = dOmegaB.normalized(); kAxisBInv = m_rbB.computeAngularImpulseDenominator( axisB ); } btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB; static bool bDoTorque = true; if( bDoTorque & avgAxis.length2() > btScalar.SIMD_EPSILON ) { avgAxis.normalize(); kAxisAInv = m_rbA.computeAngularImpulseDenominator( avgAxis ); kAxisBInv = m_rbB.computeAngularImpulseDenominator( avgAxis ); double kInvCombined = kAxisAInv + kAxisBInv; btVector3 impulse = ( kAxisAInv * dOmegaA - kAxisBInv * dOmegaB ) / ( kInvCombined * kInvCombined ); if( m_maxMotorImpulse >= 0 ) { double fMaxImpulse = m_maxMotorImpulse; if( m_bNormalizedMotorStrength ) fMaxImpulse = fMaxImpulse / kAxisAInv; btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse; double newUnclampedMag = newUnclampedAccImpulse.length(); if( newUnclampedMag > fMaxImpulse ) { newUnclampedAccImpulse.normalize(); newUnclampedAccImpulse *= fMaxImpulse; impulse = newUnclampedAccImpulse - m_accMotorImpulse; } m_accMotorImpulse += impulse; } double impulseMag = impulse.length(); btVector3 impulseAxis = impulse / impulseMag; bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * impulseAxis, impulseMag ); bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * impulseAxis, -impulseMag ); } } else if( m_damping > btScalar.SIMD_EPSILON ) // no motor: do a little damping { btVector3 angVelA; bodyA.internalGetAngularVelocity( angVelA ); btVector3 angVelB; bodyB.internalGetAngularVelocity( angVelB ); btVector3 relVel = angVelB - angVelA; if( relVel.length2() > btScalar.SIMD_EPSILON ) { btVector3 relVelAxis = relVel.normalized(); double m_kDamping = btScalar.BT_ONE / ( m_rbA.computeAngularImpulseDenominator( relVelAxis ) + m_rbB.computeAngularImpulseDenominator( relVelAxis ) ); btVector3 impulse = m_damping * m_kDamping * relVel; double impulseMag = impulse.length(); btVector3 impulseAxis = impulse / impulseMag; bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * impulseAxis, impulseMag ); bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * impulseAxis, -impulseMag ); } } // joint limits { ///solve angular part btVector3 angVelA; bodyA.internalGetAngularVelocity( angVelA ); btVector3 angVelB; bodyB.internalGetAngularVelocity( angVelB ); // solve swing limit if( m_solveSwingLimit ) { double amplitude = m_swingLimitRatio * m_swingCorrection * m_biasFactor / timeStep; double relSwingVel = ( angVelB - angVelA ).dot( m_swingAxis ); if( relSwingVel > 0 ) amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor; double impulseMag = amplitude * m_kSwing; // Clamp the accumulated impulse double temp = m_accSwingLimitImpulse; m_accSwingLimitImpulse = btMax( m_accSwingLimitImpulse + impulseMag, (double)( 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.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * noTwistSwingAxis, impulseMag ); bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * noTwistSwingAxis, -impulseMag ); } // solve twist limit if( m_solveTwistLimit ) { double amplitude = m_twistLimitRatio * m_twistCorrection * m_biasFactor / timeStep; double 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; double impulseMag = amplitude * m_kTwist; // Clamp the accumulated impulse double temp = m_accTwistLimitImpulse; m_accTwistLimitImpulse = btMax( m_accTwistLimitImpulse + impulseMag, (double)( 0.0 ) ); impulseMag = m_accTwistLimitImpulse - temp; // btVector3 impulse = m_twistAxis * impulseMag; bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * m_twistAxis, impulseMag ); bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * m_twistAxis, -impulseMag ); } } } } */ /* void calcAngleInfo() { m_swingCorrection = btScalar.BT_ZERO; m_twistLimitSign = btScalar.BT_ZERO; m_solveTwistLimit = false; m_solveSwingLimit = false; btVector3 b1Axis1 = btVector3.Zero,b1Axis2 = btVector3.Zero, b1Axis3 = btVector3.Zero; btVector3 b2Axis1 = btVector3.Zero, b2Axis2 = btVector3.Zero; b1Axis1 = m_rbA.m_worldTransform.m_basis * this.m_rbAFrame.m_basis.getColumn( 0 ); b2Axis1 = m_rbB.m_worldTransform.m_basis * this.m_rbBFrame.m_basis.getColumn( 0 ); double swing1 = btScalar.BT_ZERO, swing2 = btScalar.BT_ZERO; double swx = btScalar.BT_ZERO, swy = btScalar.BT_ZERO; double thresh = 10; double fact; // Get Frame into world space if( m_swingSpan1 >= (double)( 0.05f ) ) { b1Axis2 = m_rbA.m_worldTransform.m_basis * this.m_rbAFrame.m_basis.getColumn( 1 ); swx = b2Axis1.dot( b1Axis1 ); swy = b2Axis1.dot( b1Axis2 ); swing1 = btScalar.btAtan2Fast( swy, swx ); fact = ( swy * swy + swx * swx ) * thresh * thresh; fact = fact / ( fact + (double)( 1.0 ) ); swing1 *= fact; } if( m_swingSpan2 >= (double)( 0.05f ) ) { b1Axis3 = m_rbA.m_worldTransform.m_basis * this.m_rbAFrame.m_basis.getColumn( 2 ); swx = b2Axis1.dot( b1Axis1 ); swy = b2Axis1.dot( b1Axis3 ); swing2 = btScalar.btAtan2Fast( swy, swx ); fact = ( swy * swy + swx * swx ) * thresh * thresh; fact = fact / ( fact + (double)( 1.0 ) ); swing2 *= fact; } double RMaxAngle1Sq = 1.0f / ( m_swingSpan1 * m_swingSpan1 ); double RMaxAngle2Sq = 1.0f / ( m_swingSpan2 * m_swingSpan2 ); double EllipseAngle = btScalar.btFabs( swing1 * swing1 ) * RMaxAngle1Sq + btScalar.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(); double swingAxisSign = ( b2Axis1.dot( b1Axis1 ) >= 0.0f ) ? 1.0f : -1.0f; m_swingAxis *= swingAxisSign; } // Twist limits if( m_twistSpan >= btScalar.BT_ZERO ) { btVector3 b2Axis2 = m_rbB.m_worldTransform.m_basis * this.m_rbBFrame.m_basis.getColumn( 1 ); btQuaternion rotationArc = btQuaternion.shortestArcQuat( b2Axis1, b1Axis1 ); btVector3 TwistRef = btQuaternion.quatRotate( rotationArc, b2Axis2 ); double twist = btScalar.btAtan2Fast( TwistRef.dot( b1Axis3 ), TwistRef.dot( b1Axis2 ) ); m_twistAngle = twist; // double lockedFreeFactor = (m_twistSpan > (double)(0.05f)) ? m_limitSoftness : btScalar.BT_ZERO; double lockedFreeFactor = ( m_twistSpan > (double)( 0.05f ) ) ? (double)( 1.0f ) : btScalar.BT_ZERO; 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(); } } } */ void calcAngleInfo2( ref btTransform transA, ref btTransform transB, ref btMatrix3x3 invInertiaWorldA, ref btMatrix3x3 invInertiaWorldB ) { m_swingCorrection = btScalar.BT_ZERO; m_twistLimitSign = btScalar.BT_ZERO; 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 = new btTransform( ref m_qTarget ); btTransform trA; transA.Apply( ref m_rbAFrame, out trA ); btTransform trB; transB.Apply( ref m_rbBFrame, out trB ); btTransform tmp; btTransform trAInv; trA.inverse( out trAInv ); trB.Apply( ref trPose, out tmp ); btTransform trDeltaAB;// = trB * trPose * trA.inverse(); tmp.Apply( ref trAInv, out trDeltaAB ); btQuaternion qDeltaAB; trDeltaAB.getRotation( out qDeltaAB ); btVector3 swingAxis = new btVector3( qDeltaAB.x, qDeltaAB.y, qDeltaAB.z ); double swingAxisLen2 = swingAxis.length2(); if( btScalar.btFuzzyZero( swingAxisLen2 ) ) { return; } m_swingAxis = swingAxis; m_swingAxis.normalize(); m_swingCorrection = qDeltaAB.getAngle(); if( !btScalar.btFuzzyZero( m_swingCorrection ) ) { m_solveSwingLimit = true; } return; } { // compute rotation of A wrt B (in constraint space) btQuaternion tmpA; transA.getRotation( out tmpA ); btQuaternion tmpAFrame; m_rbAFrame.getRotation( out tmpAFrame ); btQuaternion qA; tmpA.Mult( ref tmpAFrame, out qA ); transB.getRotation( out tmpA ); m_rbBFrame.getRotation( out tmpAFrame ); btQuaternion qB; tmpA.Mult( ref tmpAFrame, out qB );// transB.getRotation() * m_rbBFrame.getRotation(); btQuaternion qBInv; qB.inverse( out qBInv ); btQuaternion qAB; qBInv.Mult( ref qA, out qAB ); // split rotation into cone and twist // (all this is done from B's perspective. Maybe I should be averaging axes...) btVector3 vConeNoTwist; btQuaternion.quatRotate( ref qAB, ref vTwist, out vConeNoTwist ); vConeNoTwist.normalize(); btQuaternion qABCone; btQuaternion.shortestArcQuat( ref vTwist, ref vConeNoTwist, out qABCone ); qABCone.normalize(); btQuaternion qABInv; qABCone.inverse( out qABInv ); btQuaternion qABTwist; qABInv.Mult( ref qAB, out qABTwist ); qABTwist.normalize(); if( m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh ) { double swingAngle, swingLimit = 0; btVector3 swingAxis; computeConeLimitInfo( ref qABCone, out swingAngle, out swingAxis, out 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; if( swingAngle < swingLimit && m_limitSoftness < 1 - btScalar.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( ref swingAxis ); // Calculate necessary axis & factors btVector3 swingAxisInv; swingAxis.Invert( out swingAxisInv ); btQuaternion.quatRotate( ref qB, ref swingAxisInv, out m_swingAxis ); m_twistAxisA.setValue( 0, 0, 0 ); m_kSwing = btScalar.BT_ONE / ( computeAngularImpulseDenominator( ref m_swingAxis, invInertiaWorldA ) + computeAngularImpulseDenominator( ref 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.m_basis * m_rbAFrame.m_basis.getColumn( 0 ); btVector3 jvA = transA.m_basis * m_rbAFrame.m_basis.getColumn( 1 ); btVector3 kvA = transA.m_basis * m_rbAFrame.m_basis.getColumn( 2 ); btVector3 ivB = transB.m_basis * m_rbBFrame.m_basis.getColumn( 0 ); btVector3 target; double x = ivB.dot( ivA ); double y = ivB.dot( jvA ); double 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( ( !btScalar.btFuzzyZero( y ) ) || ( !( btScalar.btFuzzyZero( z ) ) ) ) { m_solveSwingLimit = true; m_swingAxis = -ivB.cross( ivA ); } } else { if( m_swingSpan1 < m_fixThresh ) { // hinge around Y axis // if(!(btFuzzyZero(y))) if( ( !( btScalar.btFuzzyZero( x ) ) ) || ( !( btScalar.btFuzzyZero( z ) ) ) ) { m_solveSwingLimit = true; if( m_swingSpan2 >= m_fixThresh ) { y = (double)( 0 ); double span2 = btScalar.btAtan2( z, x ); if( span2 > m_swingSpan2 ) { x = btScalar.btCos( m_swingSpan2 ); z = btScalar.btSin( m_swingSpan2 ); } else if( span2 < -m_swingSpan2 ) { x = btScalar.btCos( m_swingSpan2 ); z = -btScalar.btSin( m_swingSpan2 ); } } } } else { // hinge around Z axis // if(!btFuzzyZero(z)) if( ( !( btScalar.btFuzzyZero( x ) ) ) || ( !( btScalar.btFuzzyZero( y ) ) ) ) { m_solveSwingLimit = true; if( m_swingSpan1 >= m_fixThresh ) { z = (double)( 0 ); double span1 = btScalar.btAtan2( y, x ); if( span1 > m_swingSpan1 ) { x = btScalar.btCos( m_swingSpan1 ); y = btScalar.btSin( m_swingSpan1 ); } else if( span1 < -m_swingSpan1 ) { x = btScalar.btCos( m_swingSpan1 ); y = -btScalar.btSin( m_swingSpan1 ); } } } } target.x = x * ivA[0] + y * jvA[0] + z * kvA[0]; target.y = x * ivA[1] + y * jvA[1] + z * kvA[1]; target.z = x * ivA[2] + y * jvA[2] + z * kvA[2]; target.w = 0; target.normalize(); m_swingAxis = -ivB.cross( target ); m_swingCorrection = m_swingAxis.length(); if( !btScalar.btFuzzyZero( m_swingCorrection ) ) m_swingAxis.normalize(); } } if( m_twistSpan >= (double)( 0 ) ) { btVector3 twistAxis; computeTwistLimitInfo( ref qABTwist, out m_twistAngle, out twistAxis ); twistAxis.Invert( out twistAxis ); if( m_twistAngle > m_twistSpan * m_limitSoftness ) { m_solveTwistLimit = true; m_twistLimitRatio = 1; if( m_twistAngle < m_twistSpan && m_limitSoftness < 1 - btScalar.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 ); btQuaternion.quatRotate( ref qB, ref twistAxis, out m_twistAxis ); m_kTwist = btScalar.BT_ONE / ( computeAngularImpulseDenominator( ref m_twistAxis, invInertiaWorldA ) + computeAngularImpulseDenominator( ref m_twistAxis, invInertiaWorldB ) ); } if( m_solveSwingLimit ) { btQuaternion.quatRotate( ref qA, ref twistAxis, out m_twistAxisA ); } } else { m_twistAngle = (double)( 0 ); } } }