///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 ); }
static double projectorigin( ref btVector3 a, ref btVector3 b, double[] w, out uint m ) { btVector3 d; b.Sub( ref a, out d ); double l = d.length2(); if( l > GJK_SIMPLEX2_EPS ) { double t = ( l > 0 ? -btVector3.btDot( ref a, ref d ) / l : 0 ); if( t >= 1 ) { w[0] = 0; w[1] = 1; m = 2; return ( b.length2() ); } else if( t <= 0 ) { w[0] = 1; w[1] = 0; m = 1; return ( a.length2() ); } else { w[0] = 1 - ( w[1] = t ); m = 3; btVector3 result; a.AddScale( ref d, t, out result ); return ( result.length2() ); } } m = 10; return ( -1 ); }
internal static void calculateDiffAxisAngleQuaternion( ref btQuaternion orn0, ref btQuaternion orn1a, out btVector3 axis, out double angle ) { btQuaternion orn1; orn0.nearest( ref orn1a, out orn1 ); btQuaternion dorn; btQuaternion tmp; orn0.inverse( out tmp ); orn1.Mult( ref tmp, out dorn ); angle = dorn.getAngle(); axis.x = dorn.x; axis.y = dorn.y; axis.z = dorn.z; axis.w = 0; //check for axis length double len = axis.length2(); if( len < btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) axis = btVector3.xAxis; else axis.Div( btScalar.btSqrt( len ), out axis ); }
internal static void calculateDiffAxisAngle( ref btTransform transform0, ref btTransform transform1, out btVector3 axis, out double angle ) { btMatrix3x3 tmp_m; transform0.m_basis.inverse( out tmp_m ); btMatrix3x3 dmat; transform1.m_basis.Mult( ref tmp_m, out dmat ); btQuaternion dorn; dmat.getRotation( out dorn ); ///floating point inaccuracy can lead to w component > 1..., which breaks dorn.normalize(); angle = dorn.getAngle(); axis.x = dorn.x; axis.y = dorn.y; axis.z = dorn.z; axis.w = 0; //check for axis length double len = axis.length2(); if( len < btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) axis = btVector3.xAxis; else axis.Div( btScalar.btSqrt( len ), out axis ); }
internal void getClosestPointsNonVirtual( btDiscreteCollisionDetectorInterface.ClosestPointInput input , btDiscreteCollisionDetectorInterface.Result output, btIDebugDraw debugDraw ) #endif { m_cachedSeparatingDistance = 0; double distance = btScalar.BT_ZERO; btVector3 normalInB = btVector3.Zero; btVector3 pointOnA, pointOnB = btVector3.Zero; btTransform localTransA; input.m_transformA.Get( out localTransA ); btTransform localTransB; input.m_transformB.Get( out localTransB ); btVector3 positionOffset; localTransA.m_origin.Add( ref localTransB.m_origin, out positionOffset ); positionOffset.Mult( (double)( 0.5 ), out positionOffset ); localTransA.m_origin.Sub( ref positionOffset, out localTransA.m_origin ); localTransB.m_origin.Sub( ref positionOffset, out localTransB.m_origin ); bool check2d = m_minkowskiA.isConvex2d() && m_minkowskiB.isConvex2d(); double marginA = m_marginA; double marginB = m_marginB; gNumGjkChecks++; //for CCD we don't use margins if( m_ignoreMargin ) { marginA = btScalar.BT_ZERO; marginB = btScalar.BT_ZERO; } m_curIter = 0; int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? m_cachedSeparatingAxis.setValue( 0, 1, 0 ); bool isValid = false; bool checkSimplex = false; bool checkPenetration = true; m_degenerateSimplex = 0; m_lastUsedMethod = -1; { double squaredDistance = btScalar.BT_LARGE_FLOAT; double delta = btScalar.BT_ZERO; double margin = marginA + marginB; m_simplexSolver.reset(); for( ;;) //while (true) { btVector3 tmp; m_cachedSeparatingAxis.Invert( out tmp ); btVector3 seperatingAxisInA; localTransA.m_basis.ApplyInverse( ref tmp, out seperatingAxisInA ); btVector3 seperatingAxisInB; localTransA.m_basis.ApplyInverse( ref m_cachedSeparatingAxis, out seperatingAxisInB ); btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA ); btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB ); btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld ); btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld ); btScalar.Dbg( "pWorld is " + pWorld + " qWorld is " + qWorld ); if( check2d ) { pWorld[2] = 0; qWorld[2] = 0; } btVector3 w; pWorld.Sub( ref qWorld, out w ); delta = m_cachedSeparatingAxis.dot( ref w ); // potential exit, they don't overlap if( ( delta > (double)( 0.0 ) ) && ( delta * delta > squaredDistance * input.m_maximumDistanceSquared ) ) { m_degenerateSimplex = 10; checkSimplex = true; //checkPenetration = false; break; } //exit 0: the new point is already in the simplex, or we didn't come any closer if( m_simplexSolver.inSimplex( ref w ) ) { m_degenerateSimplex = 1; checkSimplex = true; break; } // are we getting any closer ? double f0 = squaredDistance - delta; double f1 = squaredDistance * REL_ERROR2; btScalar.Dbg( "f0 is " + f0.ToString( "g17" ) + " f1 is " + f1.ToString( "g17" ) ); if( f0 <= f1 ) { if( f0 <= btScalar.BT_ZERO ) { m_degenerateSimplex = 2; } else { m_degenerateSimplex = 11; } checkSimplex = true; break; } //add current vertex to simplex m_simplexSolver.addVertex( ref w, ref pWorld, ref qWorld ); btVector3 newCachedSeparatingAxis; //calculate the closest point to the origin (update vector v) if( !m_simplexSolver.closest( out newCachedSeparatingAxis ) ) { m_degenerateSimplex = 3; checkSimplex = true; break; } if( newCachedSeparatingAxis.length2() < REL_ERROR2 ) { m_cachedSeparatingAxis = newCachedSeparatingAxis; m_degenerateSimplex = 6; checkSimplex = true; break; } double previousSquaredDistance = squaredDistance; squaredDistance = newCachedSeparatingAxis.length2(); #if asdfasdf ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo if (squaredDistance>previousSquaredDistance) { m_degenerateSimplex = 7; squaredDistance = previousSquaredDistance; checkSimplex = false; break; } #endif // //redundant m_simplexSolver.compute_points(pointOnA, pointOnB); //are we getting any closer ? if( previousSquaredDistance - squaredDistance <= btScalar.SIMD_EPSILON * previousSquaredDistance ) { // m_simplexSolver.backup_closest(m_cachedSeparatingAxis); checkSimplex = true; m_degenerateSimplex = 12; break; } m_cachedSeparatingAxis = newCachedSeparatingAxis; //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject if( m_curIter++ > gGjkMaxIter ) { #if DEBUG Console.WriteLine( "btGjkPairDetector maxIter exceeded:{0}", m_curIter ); Console.WriteLine( "sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4},shapeTypeB={5}\n", m_cachedSeparatingAxis.x, m_cachedSeparatingAxis.y, m_cachedSeparatingAxis.z, squaredDistance, m_minkowskiA.getShapeType(), m_minkowskiB.getShapeType() ); #endif break; } bool check = ( !m_simplexSolver.fullSimplex() ); //bool check = (!m_simplexSolver.fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver.maxVertex()); if( !check ) { //do we need this backup_closest here ? // m_simplexSolver.backup_closest(m_cachedSeparatingAxis); m_degenerateSimplex = 13; break; } } if( checkSimplex ) { m_simplexSolver.compute_points( out pointOnA, out pointOnB ); btScalar.Dbg( "new simplex points " + pointOnA.ToString() + " and " + pointOnB ); normalInB = m_cachedSeparatingAxis; double lenSqr = m_cachedSeparatingAxis.length2(); //valid normal if( lenSqr < 0.0001 ) { m_degenerateSimplex = 5; } if( lenSqr > btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) { double rlen = btScalar.BT_ONE / btScalar.btSqrt( lenSqr ); normalInB.Mult( rlen, out normalInB ); //normalInB *= rlen; //normalize double s = btScalar.btSqrt( squaredDistance ); Debug.Assert( s > (double)( 0.0 ) ); pointOnA.SubScale( ref m_cachedSeparatingAxis, ( marginA / s ), out pointOnA ); pointOnB.AddScale( ref m_cachedSeparatingAxis, ( marginB / s ), out pointOnB ); //pointOnA -= m_cachedSeparatingAxis * ( marginA / s ); //pointOnB += m_cachedSeparatingAxis * ( marginB / s ); distance = ( ( btScalar.BT_ONE / rlen ) - margin ); isValid = true; m_lastUsedMethod = 1; } else { m_lastUsedMethod = 2; } } bool catchDegeneratePenetrationCase = ( m_catchDegeneracies != 0 && m_penetrationDepthSolver != null && m_degenerateSimplex != 0 && ( ( distance + margin ) < 0.01 ) ); //if (checkPenetration && !isValid) if( checkPenetration && ( !isValid || catchDegeneratePenetrationCase ) ) { //penetration case //if there is no way to handle penetrations, bail out if( m_penetrationDepthSolver != null ) { // Penetration depth case. btVector3 tmpPointOnA, tmpPointOnB; gNumDeepPenetrationChecks++; m_cachedSeparatingAxis.setZero(); bool isValid2 = m_penetrationDepthSolver.calcPenDepth( m_simplexSolver, m_minkowskiA, m_minkowskiB, ref localTransA, ref localTransB, ref m_cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB, debugDraw ); btScalar.Dbg( "points are " + tmpPointOnA.ToString() + " and " + tmpPointOnB.ToString() ); if( isValid2 ) { btVector3 tmpNormalInB; tmpPointOnB.Sub( ref tmpPointOnA, out tmpNormalInB ); double lenSqr = tmpNormalInB.length2(); if( lenSqr <= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) ) { tmpNormalInB = m_cachedSeparatingAxis; lenSqr = m_cachedSeparatingAxis.length2(); } if( lenSqr > ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) ) { tmpNormalInB.Mult( btScalar.btSqrt( lenSqr ), out tmpNormalInB ); btVector3 tmp; tmpPointOnA.Sub( ref tmpPointOnB, out tmp ); double distance2 = -tmp.length(); m_lastUsedMethod = 3; //only replace valid penetrations when the result is deeper (check) if( !isValid || ( distance2 < distance ) ) { distance = distance2; pointOnA = tmpPointOnA; pointOnB = tmpPointOnB; normalInB = tmpNormalInB; ///todo: need to track down this EPA penetration solver degeneracy ///the penetration solver reports penetration but the contact normal ///connecting the contact points is pointing in the opposite direction ///until then, detect the issue and revert the normal { btScalar d1 = 0; { normalInB.Invert( out tmp ); btVector3 seperatingAxisInA; input.m_transformA.m_basis.ApplyInverse( ref normalInB, out seperatingAxisInA ); btVector3 seperatingAxisInB; input.m_transformB.m_basis.ApplyInverse( ref tmp, out seperatingAxisInB ); btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA ); btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB ); btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld ); btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld ); btVector3 w; pWorld.Sub( ref qWorld, out w ); d1 = ( tmp ).dot( w ); } btScalar d0 = btScalar.BT_ZERO; { normalInB.Invert( out tmp ); btVector3 seperatingAxisInA; input.m_transformA.m_basis.ApplyInverse( ref tmp, out seperatingAxisInA ); btVector3 seperatingAxisInB; input.m_transformB.m_basis.ApplyInverse( ref normalInB, out seperatingAxisInB ) ; btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA ); btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB ); btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld ); btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld ); btVector3 w; pWorld.Sub( ref qWorld, out w ); d0 = normalInB.dot( w ); } if( d1 > d0 ) { m_lastUsedMethod = 10; normalInB *= -1; } } isValid = true; } else { m_lastUsedMethod = 8; } } else { m_lastUsedMethod = 9; } } else { ///this is another degenerate case, where the initial GJK calculation reports a degenerate case ///EPA reports no penetration, and the second GJK (using the supporting vector without margin) ///reports a valid positive distance. Use the results of the second GJK instead of failing. ///thanks to Jacob.Langford for the reproduction case ///http://code.google.com/p/bullet/issues/detail?id=250 if( m_cachedSeparatingAxis.length2() > btScalar.BT_ZERO ) { btVector3 tmp; tmpPointOnA.Sub( ref tmpPointOnB, out tmp ); double distance2 = tmp.length() - margin; //only replace valid distances when the distance is less btScalar.Dbg( "old distance " + distance2.ToString( "g17" ) + " new distance " + distance.ToString( "g17" ) ); if( !isValid || ( distance2 < distance ) ) { distance = distance2; pointOnA = tmpPointOnA; pointOnB = tmpPointOnB; pointOnA.SubScale( ref m_cachedSeparatingAxis, marginA, out pointOnA ); pointOnB.AddScale( ref m_cachedSeparatingAxis, marginB, out pointOnA ); //pointOnA -= m_cachedSeparatingAxis * marginA; //pointOnB += m_cachedSeparatingAxis * marginB; normalInB = m_cachedSeparatingAxis; normalInB.normalize(); isValid = true; m_lastUsedMethod = 6; } else { m_lastUsedMethod = 5; } } } } } } btScalar.Dbg( "Pair detector : valid=" + (isValid?"1":"0" )+ " distance=" + distance.ToString( "g17" ) + " maxDistance=" + input.m_maximumDistanceSquared.ToString( "g17" ) ); if( isValid && ( ( distance < 0 ) || ( distance * distance < input.m_maximumDistanceSquared ) ) ) { m_cachedSeparatingAxis = normalInB; m_cachedSeparatingDistance = distance; btVector3 tmp; pointOnB.Add( ref positionOffset, out tmp ); output.addContactPoint( ref normalInB, ref tmp, distance ); } }
/*@brief Return the angle between this and another vector @param v The other vector */ public double angle( ref btVector3 v ) { double s = btScalar.btSqrt( ( length2() * v.length2() ) ); #if PARANOID_ASSERTS Debug.Assert( s != (double)(0.0)); #endif return btScalar.btAcos( ( dot( ref v ) / s ) ); }
/* 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 ); } } }