/*@brief Set the rotation using axis angle notation @param axis The axis around which to rotate @param angle The magnitude of the rotation in Radians */ public void setRotation( ref btVector3 axis, double _angle ) { double d = axis.length(); if( d != 0.0 ) { double s = btScalar.btSin( _angle * 0.5 ) / d; setValue( axis.x * s, axis.y * s, axis.z * s, btScalar.btCos( _angle * 0.5 ) ); } else x = y = z = w = 0; }
/*@brief Axis angle Constructor @param axis The axis which the rotation is around @param angle The magnitude of the rotation around the angle (Radians) */ public btQuaternion( ref btVector3 _axis, float _angle ) { //setRotation( _axis, _angle ); float d = _axis.length(); if( d != 0.0 ) { float s = btScalar.btSin( _angle * btScalar.BT_HALF ) / d; x = _axis.x * s; y = _axis.y * s; z = _axis.z * s; w = btScalar.btCos( _angle * btScalar.BT_HALF ); } else x = y = z = w = 0; }
/*@brief Axis angle Constructor @param axis The axis which the rotation is around @param angle The magnitude of the rotation around the angle (Radians) */ public btQuaternion( ref btVector3 _axis, double _angle ) { //setRotation( _axis, _angle ); double d = _axis.length(); if( d != 0.0 ) { double s = btScalar.btSin( _angle * 0.5 ) / d; x = _axis.x * s; y = _axis.y * s; z = _axis.z * s; w = btScalar.btCos( _angle * 0.5 ); } else x = y = z = w = 0; }
public static void integrateTransform( ref btTransform curTrans, ref btVector3 linvel, ref btVector3 angvel , double timeStep, out btTransform predictedTransform ) { btVector3 tmp; btVector3 tmp2; linvel.Mult( timeStep, out tmp ); curTrans.getOrigin( out tmp2 ); tmp2.Add( ref tmp, out predictedTransform.m_origin ); #if QUATERNION_DERIVATIVE btQuaternion predictedOrn = curTrans.getRotation(); predictedOrn += (angvel predictedOrn) * (timeStep 0.5); predictedOrn.normalize(); #else //Exponential map //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia btVector3 axis; double fAngle = angvel.length(); //limit the angular motion if( fAngle * timeStep > ANGULAR_MOTION_THRESHOLD ) { fAngle = ANGULAR_MOTION_THRESHOLD / timeStep; } if( fAngle < 0.001 ) { // use Taylor's expansions of sync function angvel.Mult( ( btScalar.BT_HALF * timeStep - ( timeStep * timeStep * timeStep ) * ( 0.020833333333 ) * fAngle * fAngle ), out axis ); } else { // sync(fAngle) = sin(cfAngle)/t angvel.Mult( ( btScalar.btSin( btScalar.BT_HALF * fAngle * timeStep ) / fAngle ), out axis ); } btQuaternion dorn = new btQuaternion( axis.x, axis.y, axis.z, btScalar.btCos( fAngle * timeStep * 0.5 ) ); btQuaternion orn0; curTrans.getRotation( out orn0 ); btQuaternion predictedOrn; dorn.Mult( ref orn0, out predictedOrn ); predictedOrn.normalize(); #endif btMatrix3x3.setRotation( out predictedTransform.m_basis, ref predictedOrn); //predictedTransform.setRotation( ref predictedOrn ); }
void coneLocalSupport( ref btVector3 v, out btVector3 result ) { btVector3 tmp = btVector3.Zero; double halfHeight = m_height * (double)( 0.5 ); if( v[m_coneIndices[1]] > v.length() * m_sinAngle ) { tmp[m_coneIndices[0]] = btScalar.BT_ZERO; tmp[m_coneIndices[1]] = halfHeight; tmp[m_coneIndices[2]] = btScalar.BT_ZERO; result = tmp; } else { double s = btScalar.btSqrt( v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]] ); if( s > btScalar.SIMD_EPSILON ) { double d = m_radius / s; tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d; tmp[m_coneIndices[1]] = -halfHeight; tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d; result = tmp; } else { tmp[m_coneIndices[0]] = btScalar.BT_ZERO; tmp[m_coneIndices[1]] = -halfHeight; tmp[m_coneIndices[2]] = btScalar.BT_ZERO; result = tmp; } } }
///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping public void applyDamping( double timeStep ) { //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74 //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway //#define USE_OLD_DAMPING_METHOD 1 #if USE_OLD_DAMPING_METHOD m_linearVelocity *= GEN_clamped( ( (double)( 1.0) - timeStep * m_linearDamping ), (double)btScalar.BT_ZERO, (double)(double)( 1.0 ) ); m_angularVelocity *= GEN_clamped( ( (double)( 1.0) - timeStep * m_angularDamping ), (double)btScalar.BT_ZERO, (double)(double)( 1.0 ) ); #else m_linearVelocity.Mult( btScalar.btPow( (double)( 1 ) - m_linearDamping, timeStep ), out m_linearVelocity ); m_angularVelocity.Mult( btScalar.btPow( (double)( 1 ) - m_angularDamping, timeStep ), out m_angularVelocity ); //m_linearVelocity *= btScalar.btPow( (double)( 1 ) - m_linearDamping, timeStep ); //m_angularVelocity *= btScalar.btPow( (double)( 1 ) - m_angularDamping, timeStep ); #endif if( m_additionalDamping ) { //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc. //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete if( ( m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr ) & ( m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr ) ) { m_linearVelocity.Mult( m_additionalDampingFactor, out m_linearVelocity ); m_angularVelocity.Mult( m_additionalDampingFactor, out m_angularVelocity ); //m_angularVelocity *= m_additionalDampingFactor; //m_linearVelocity *= m_additionalDampingFactor; } double speed = m_linearVelocity.length(); if( speed < m_linearDamping ) { double dampVel = (double)( 0.005 ); if( speed > dampVel ) { btVector3 dir; m_linearVelocity.normalized( out dir ); dir.Mult( dampVel, out dir ); m_linearVelocity.Sub( ref dir, out m_linearVelocity ); //m_linearVelocity -= dir * dampVel; } else { m_linearVelocity = btVector3.Zero; } } double angSpeed = m_angularVelocity.length(); if( angSpeed < m_angularDamping ) { double angDampVel = (double)( 0.005 ); if( angSpeed > angDampVel ) { btVector3 dir; m_angularVelocity.normalized( out dir ); dir.Mult( angDampVel, out dir ); m_angularVelocity.Sub( ref dir, out m_angularVelocity ); //m_angularVelocity -= dir * angDampVel; } else { m_angularVelocity = btVector3.Zero; } } } }
internal eStatus._ Evaluate( tShape shapearg, ref btVector3 guess ) { uint iterations = 0; double sqdist = 0; double alpha = 0; btVector3[] lastw = new btVector3[4]; uint clastw = 0; /* Initialize solver */ m_free[0] = new sSV(); m_free[1] = new sSV(); m_free[2] = new sSV(); m_free[3] = new sSV(); m_nfree = 4; m_current = 0; m_status = eStatus._.Valid; m_shape = shapearg; m_distance = 0; /* Initialize simplex */ m_simplices0.rank = 0; m_ray = guess; double sqrl = m_ray.length2(); btVector3 tmp; if( sqrl > 0 ) m_ray.Invert( out tmp ); else tmp = btVector3.xAxis; appendvertice( m_simplices0, ref tmp ); m_simplices0.p[0] = 1; m_ray = m_simplices0.c[0].w; sqdist = sqrl; lastw[0] = lastw[1] = lastw[2] = lastw[3] = m_ray; /* Loop */ do { uint next = 1 - m_current; sSimplex cs = m_current==0?m_simplices0:m_simplices1; sSimplex ns = next==0?m_simplices0:m_simplices1; /* Check zero */ double rl = m_ray.length(); if( rl < GJK_MIN_DISTANCE ) {/* Touching or inside */ m_status = eStatus._.Inside; break; } /* Append new vertice in -'v' direction */ m_ray.Invert( out tmp ); appendvertice( cs, ref tmp ); btVector3 w = cs.c[cs.rank - 1].w; bool found = false; for( uint i = 0; i < 4; ++i ) { w.Sub( ref lastw[i], out tmp ); if( tmp.length2() < GJK_DUPLICATED_EPS ) { found = true; break; } } if( found ) {/* Return old simplex */ removevertice( cs ); break; } else {/* Update lastw */ lastw[clastw = ( clastw + 1 ) & 3] = w; } /* Check for termination */ double omega = btVector3.btDot( ref m_ray, ref w ) / rl; alpha = btScalar.btMax( omega, alpha ); if( ( ( rl - alpha ) - ( GJK_ACCURARY * rl ) ) <= 0 ) {/* Return old simplex */ removevertice( cs ); break; } /* Reduce simplex */ double[] weights = new double[4]; uint mask = 0; switch( cs.rank ) { case 2: sqdist = projectorigin( ref cs.c[0].w, ref cs.c[1].w, weights, out mask ); break; case 3: sqdist = projectorigin( ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, weights, out mask ); break; case 4: sqdist = projectorigin( ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref cs.c[3].w, weights, out mask ); break; } if( sqdist >= 0 ) {/* Valid */ ns.rank = 0; m_ray = btVector3.Zero; m_current = next; for( int i = 0, ni = (int)cs.rank; i < ni; ++i ) { if( ( mask & ( (uint)1 << i ) ) != 0 ) { ns.c[ns.rank] = cs.c[i]; ns.p[ns.rank++] = weights[i]; btVector3 tmp2; cs.c[i].w.Mult( weights[i], out tmp2 ); m_ray.Add( ref tmp2, out m_ray ); } else { m_free[m_nfree++] = cs.c[i]; } } if( mask == 15 ) m_status = eStatus._.Inside; } else {/* Return old simplex */ removevertice( cs ); break; } m_status = ( ( ++iterations ) < GJK_MAX_ITERATIONS ) ? m_status : eStatus._.Failed; } while( m_status == eStatus._.Valid ); m_simplex = m_current==0?m_simplices0:m_simplices1; switch( m_status ) { case eStatus._.Valid: m_distance = m_ray.length(); break; case eStatus._.Inside: m_distance = 0; break; default: break; } return ( m_status ); }
/*@brief Set the rotation using axis angle notation @param axis The axis around which to rotate @param angle The magnitude of the rotation in Radians */ public void setRotation( ref btVector3 axis, float _angle ) { float d = axis.length(); if( d != 0.0 ) { float s = btScalar.btSin( _angle * btScalar.BT_HALF ) / d; setValue( axis.x * s, axis.y * s, axis.z * s, btScalar.btCos( _angle * btScalar.BT_HALF ) ); } else x = y = z = w = 0; }
///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) ///result is conservative public void calculateTemporalAabb( ref btTransform curTrans, ref btVector3 linvel, ref btVector3 angvel, double timeStep , out btVector3 temporalAabbMin, out btVector3 temporalAabbMax ) { //start with static aabb getAabb( ref curTrans, out temporalAabbMin, out temporalAabbMax ); double temporalAabbMaxx = temporalAabbMax.x; double temporalAabbMaxy = temporalAabbMax.y; double temporalAabbMaxz = temporalAabbMax.z; double temporalAabbMinx = temporalAabbMin.x; double temporalAabbMiny = temporalAabbMin.y; double temporalAabbMinz = temporalAabbMin.z; // add linear motion btVector3 linMotion; linvel.Mult( timeStep, out linMotion ); ///@todo: simd would have a vector max/min operation, instead of per-element access if( linMotion.x > btScalar.BT_ZERO ) temporalAabbMaxx += linMotion.x; else temporalAabbMinx += linMotion.x; if( linMotion.y > btScalar.BT_ZERO ) temporalAabbMaxy += linMotion.y; else temporalAabbMiny += linMotion.y; if( linMotion.z > btScalar.BT_ZERO ) temporalAabbMaxz += linMotion.z; else temporalAabbMinz += linMotion.z; //add conservative angular motion double angularMotion = angvel.length() * getAngularMotionDisc() * timeStep; btVector3 angularMotion3d = new btVector3( angularMotion, angularMotion, angularMotion ); temporalAabbMin = new btVector3( temporalAabbMinx, temporalAabbMiny, temporalAabbMinz ); temporalAabbMax = new btVector3( temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz ); temporalAabbMin.Sub( ref angularMotion3d, out temporalAabbMin ); temporalAabbMax.Add( ref angularMotion3d, out temporalAabbMax ); }
/* 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 ); } } }