public static void Run() { { btTransform planeObjWorld = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 3, 4, 5 ) ); btTransform convexWorldTransform = new btTransform( new btQuaternion( 0.2, 0.3, 0, 1 ), new btVector3( 3, 4, 5 ) ); btTransform convexInPlaneTrans; btTransform tmp; planeObjWorld.inverse( out tmp ); tmp.Apply( ref convexWorldTransform, out convexInPlaneTrans ); PrintTransform( ref convexInPlaneTrans ); planeObjWorld.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans ); PrintTransform( ref convexInPlaneTrans ); //convexInPlaneTrans = planeObjWorld( convexWorldTransform; //PrintTransform( &convexInPlaneTrans ); } { btTransform planeObjWorld = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 5, 2, 1 ) ); btTransform convexWorldTransform = new btTransform( new btQuaternion( 0.2, 0.3, 0, 1 ), new btVector3( 3, 4, 5 ) ); btTransform convexInPlaneTrans; btTransform tmp; planeObjWorld.inverse( out tmp ); tmp.Apply( ref convexWorldTransform, out convexInPlaneTrans ); PrintTransform( ref convexInPlaneTrans ); planeObjWorld.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans ); PrintTransform( ref convexInPlaneTrans ); //convexInPlaneTrans = planeObjWorld( convexWorldTransform; //PrintTransform( &convexInPlaneTrans ); btQuaternion perturbeRot = new btQuaternion( 0.1, 0.5, 0.25, 0.8 ); perturbeRot.normalize(); btTransform planeObjWrapTrans = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 4, 7, 2 ) ); planeObjWrapTrans.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans ); //now perturbe the convex-world transform btMatrix3x3 perturbeMat = new btMatrix3x3( ref perturbeRot ); btMatrix3x3 tmpPerturbe; convexWorldTransform.m_basis.Apply( ref perturbeMat, out tmpPerturbe ); convexWorldTransform.m_basis = tmpPerturbe; btTransform planeInConvex; convexWorldTransform.inverseTimes( ref planeObjWrapTrans, out planeInConvex ); PrintTransform( ref planeInConvex ); } Console.Read(); }
bool matrixToEulerYZX( ref btMatrix3x3 mat, ref btVector3 xyz ) { // rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx // sz cz*cx -cz*sx // -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx double fi = btGetMatrixElem( ref mat, 3 ); if( fi < (double)( 1.0f ) ) { if( fi > (double)( -1.0f ) ) { xyz[0] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 5 ), btGetMatrixElem( ref mat, 4 ) ); xyz[1] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 6 ), btGetMatrixElem( ref mat, 0 ) ); xyz[2] = btScalar.btAsin( btGetMatrixElem( ref mat, 3 ) ); return true; } else { xyz[0] = (double)( 0.0 ); xyz[1] = -btScalar.btAtan2( btGetMatrixElem( ref mat, 7 ), btGetMatrixElem( ref mat, 8 ) ); xyz[2] = -btScalar.SIMD_HALF_PI; return false; } } else { xyz[0] = (double)( 0.0 ); xyz[1] = btScalar.btAtan2( btGetMatrixElem( ref mat, 7 ), btGetMatrixElem( ref mat, 8 ) ); xyz[2] = btScalar.SIMD_HALF_PI; } return false; }
bool matrixToEulerYXZ( ref btMatrix3x3 mat, ref btVector3 xyz ) { // rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy // cx*sz cx*cz -sx // cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx double fi = btGetMatrixElem( ref mat, 5 ); if( fi < (double)( 1.0f ) ) { if( fi > (double)( -1.0f ) ) { xyz[0] = btScalar.btAsin( -btGetMatrixElem( ref mat, 5 ) ); xyz[1] = btScalar.btAtan2( btGetMatrixElem( ref mat, 2 ), btGetMatrixElem( ref mat, 8 ) ); xyz[2] = btScalar.btAtan2( btGetMatrixElem( ref mat, 3 ), btGetMatrixElem( ref mat, 4 ) ); return true; } else { xyz[0] = btScalar.SIMD_HALF_PI; xyz[1] = -btScalar.btAtan2( -btGetMatrixElem( ref mat, 1 ), btGetMatrixElem( ref mat, 0 ) ); xyz[2] = (double)( 0.0 ); return false; } } else { xyz[0] = -btScalar.SIMD_HALF_PI; xyz[1] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 1 ), btGetMatrixElem( ref mat, 0 ) ); xyz[2] = 0.0; } return false; }
bool matrixToEulerXZY( ref btMatrix3x3 mat, ref btVector3 xyz ) { // rot = cy*cz -sz sy*cz // cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx // cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy double fi = btGetMatrixElem( ref mat, 1 ); if( fi < (double)( 1.0f ) ) { if( fi > (double)( -1.0f ) ) { xyz[0] = btScalar.btAtan2( btGetMatrixElem( ref mat, 7 ), btGetMatrixElem( ref mat, 4 ) ); xyz[1] = btScalar.btAtan2( btGetMatrixElem( ref mat, 2 ), btGetMatrixElem( ref mat, 0 ) ); xyz[2] = btScalar.btAsin( -btGetMatrixElem( ref mat, 1 ) ); return true; } else { xyz[0] = -btScalar.btAtan2( -btGetMatrixElem( ref mat, 6 ), btGetMatrixElem( ref mat, 8 ) ); xyz[1] = (double)( 0.0 ); xyz[2] = btScalar.SIMD_HALF_PI; return false; } } else { xyz[0] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 6 ), btGetMatrixElem( ref mat, 8 ) ); xyz[1] = 0.0; xyz[2] = -btScalar.SIMD_HALF_PI; } return false; }
// MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html static bool matrixToEulerXYZ( ref btMatrix3x3 mat, ref btVector3 xyz ) { // rot = cy*cz -cy*sz sy // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy double fi = btGetMatrixElem( ref mat, 2 ); if( fi < (double)( 1.0f ) ) { if( fi > (double)( -1.0f ) ) { xyz[0] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 5 ), btGetMatrixElem( ref mat, 8 ) ); xyz[1] = btScalar.btAsin( btGetMatrixElem( ref mat, 2 ) ); xyz[2] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 1 ), btGetMatrixElem( ref mat, 0 ) ); return true; } else { // WARNING. Not unique. XA - ZA = -atan2(r10,r11) xyz[0] = -btScalar.btAtan2( btGetMatrixElem( ref mat, 3 ), btGetMatrixElem( ref mat, 4 ) ); xyz[1] = -btScalar.SIMD_HALF_PI; xyz[2] = (double)( 0.0 ); return false; } } else { // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) xyz[0] = btScalar.btAtan2( btGetMatrixElem( ref mat, 3 ), btGetMatrixElem( ref mat, 4 ) ); xyz[1] = btScalar.SIMD_HALF_PI; xyz[2] = 0.0; } return false; }
static double btGetMatrixElem( ref btMatrix3x3 mat, int index ) { int i = index % 3; int j = index / 3; return btMatrix3x3.getValue( ref mat, i,j ); }
public static void btSetCrossMatrixMinus( btMatrix3x3 res, ref btVector3 a ) { double a_0 = a.x, a_1 = a.y, a_2 = a.z; res.setValue( 0, +a_2, -a_1, -a_2, 0, +a_0, +a_1, -a_0, 0 ); }
void getInfo2NonVirtual( ref btConstraintInfo2 info, ref btTransform transA, ref btTransform transB, ref btMatrix3x3 invInertiaWorldA, ref btMatrix3x3 invInertiaWorldB ) { calcAngleInfo2( ref transA, ref transB, ref invInertiaWorldA, ref invInertiaWorldB ); Debug.Assert( !m_useSolveConstraintObsolete ); // set jacobian info.m_solverConstraints[0].m_contactNormal1 = btVector3.xAxis; info.m_solverConstraints[1].m_contactNormal1 = btVector3.yAxis; info.m_solverConstraints[2].m_contactNormal1 = btVector3.zAxis; //info.m_J1linearAxis = 1; //info.m_J1linearAxis[info.rowskip + 1] = 1; //info.m_J1linearAxis[2 * info.rowskip + 2] = 1; btVector3 a1; transA.m_basis.Apply( ref m_rbAFrame.m_origin, out a1 ); { //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.Invert( out a1neg ); a1neg.getSkewSymmetricMatrix( out info.m_solverConstraints[0].m_contactNormal1 , out info.m_solverConstraints[1].m_contactNormal1 , out info.m_solverConstraints[2].m_contactNormal1 ); } info.m_solverConstraints[0].m_contactNormal2 = -btVector3.xAxis; info.m_solverConstraints[1].m_contactNormal2 = -btVector3.yAxis; info.m_solverConstraints[2].m_contactNormal2 = -btVector3.zAxis; //info.m_J2linearAxis[0] = -1; //info.m_J2linearAxis[info.rowskip + 1] = -1; //info.m_J2linearAxis[2 * info.rowskip + 2] = -1; btVector3 a2; transB.m_basis.Apply( ref m_rbBFrame.m_origin, out a2 ); { a2.getSkewSymmetricMatrix( out info.m_solverConstraints[0].m_contactNormal1 , out info.m_solverConstraints[1].m_contactNormal1 , out info.m_solverConstraints[2].m_contactNormal1 ); } // set right hand side double linERP = ( m_flags & btConeTwistFlags.BT_CONETWIST_FLAGS_LIN_ERP ) != 0 ? m_linERP : info.erp; double k = info.fps * linERP; int j; for( j = 0; j < 3; j++ ) { info.m_solverConstraints[j].m_rhs = k * ( a2[j] + transB.m_origin[j] - a1[j] - transA.m_origin[j] ); info.m_solverConstraints[j].m_lowerLimit = btScalar.BT_MIN_FLOAT; info.m_solverConstraints[j].m_upperLimit = btScalar.BT_MAX_FLOAT; if( ( m_flags & btConeTwistFlags.BT_CONETWIST_FLAGS_LIN_CFM ) != 0 ) { info.m_solverConstraints[j].m_cfm = m_linCFM; } } int row = 3; //int srow = row * info.rowskip; btVector3 ax1; // angular limits if( m_solveSwingLimit ) { //double* J1 = info.m_J1angularAxis; //double* J2 = info.m_J2angularAxis; if( ( m_swingSpan1 < m_fixThresh ) && ( m_swingSpan2 < m_fixThresh ) ) { btTransform trA; transA.Apply( ref m_rbAFrame, out trA ); btVector3 p; trA.m_basis.getColumn( 1, out p ); btVector3 q; trA.m_basis.getColumn( 2, out q ); int row1 = row + 1; //int srow1 = srow + info.rowskip; info.m_solverConstraints[row].m_relpos1CrossNormal = p; info.m_solverConstraints[row1].m_relpos1CrossNormal = q; p.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal ); q.Invert( out info.m_solverConstraints[row1].m_relpos2CrossNormal ); double fact = info.fps * m_relaxationFactor; info.m_solverConstraints[row].m_rhs = fact * m_swingAxis.dot( p ); info.m_solverConstraints[row1].m_rhs = fact * m_swingAxis.dot( q ); info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT; info.m_solverConstraints[row1].m_upperLimit = btScalar.BT_MAX_FLOAT; info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT; info.m_solverConstraints[row1].m_upperLimit = btScalar.BT_MAX_FLOAT; row = row1 + 1; //srow = srow1 + info.rowskip; } else { ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor; info.m_solverConstraints[row].m_relpos1CrossNormal = ax1; ax1.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal ); k = info.fps * m_biasFactor; info.m_solverConstraints[row].m_rhs = k * m_swingCorrection; if( ( m_flags & btConeTwistFlags.BT_CONETWIST_FLAGS_ANG_CFM ) != 0 ) { info.m_solverConstraints[row].m_cfm = m_angCFM; } // m_swingCorrection is always positive or 0 info.m_solverConstraints[row].m_lowerLimit = 0; info.m_solverConstraints[row].m_upperLimit = ( m_bMotorEnabled && m_maxMotorImpulse >= 0.0f ) ? m_maxMotorImpulse : btScalar.BT_MAX_FLOAT; //srow += info.rowskip; row++; } } if( m_solveTwistLimit ) { ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor; info.m_solverConstraints[row].m_relpos1CrossNormal = ax1; ax1.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal ); k = info.fps * m_biasFactor; info.m_solverConstraints[row].m_rhs = k * m_twistCorrection; if( ( m_flags & btConeTwistFlags.BT_CONETWIST_FLAGS_ANG_CFM ) != 0 ) { info.m_solverConstraints[row].m_cfm = m_angCFM; } if( m_twistSpan > 0.0f ) { if( m_twistCorrection > 0.0f ) { info.m_solverConstraints[row].m_lowerLimit = 0; info.m_solverConstraints[row].m_upperLimit = btScalar.BT_MAX_FLOAT; } else { info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT; info.m_solverConstraints[row].m_upperLimit = 0; } } else { info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT; info.m_solverConstraints[row].m_upperLimit = btScalar.BT_MAX_FLOAT; } //srow += info.rowskip; row++; } }
public void evalEulerEqn( ref btVector3 w1, ref btVector3 w0, ref btVector3 T, double dt, ref btMatrix3x3 I, out btVector3 result ) { btVector3 Iw0; btVector3 dtT; btVector3 Iw1; btVector3 cross; T.Mult( dt, out dtT ); I.Apply( ref w0, out Iw0 ); dtT.Add( ref Iw0, out Iw0 ); // Iw0 = ( T * dt + I * w0 ) I.Apply( ref w1, out Iw1 ); w1.cross( ref Iw1, out cross ); cross.Mult( dt, out cross ); // cross = w1.cross( I * w1 ) * dt Iw1.Add( ref cross, out Iw1 ); // Iw1 = Iw1 + cross Iw1.Sub( ref Iw0, out result ); //btVector3 w2; = I * w1 + w1.cross( I * w1 ) * dt - ( T * dt + I * w0 ); //return w2; }
// // Convex-Convex collision algorithm // internal override void processCollision( btCollisionObjectWrapper body0Wrap , ref btTransform body0Transform , btCollisionObjectWrapper body1Wrap , ref btTransform body1Transform , btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { if( m_manifoldPtr == null ) { //swapped? m_manifoldPtr = m_dispatcher.getNewManifold( body0Wrap.m_collisionObject, body1Wrap.m_collisionObject ); m_ownManifold = true; } resultOut.setPersistentManifold( m_manifoldPtr ); //comment-out next line to test multi-contact generation //resultOut.getPersistentManifold().clearManifold(); btConvexShape min0 = (btConvexShape)body0Wrap.getCollisionShape(); btConvexShape min1 = (btConvexShape)body1Wrap.getCollisionShape(); btVector3 normalOnB; btVector3 pointOnBWorld; #if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if( ( min0.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE ) && ( min1.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE ) ) { btCapsuleShape capsuleA = (btCapsuleShape)min0; btCapsuleShape capsuleB = (btCapsuleShape)min1; // btVector3 localScalingA = capsuleA.getLocalScaling(); // btVector3 localScalingB = capsuleB.getLocalScaling(); double threshold = m_manifoldPtr.getContactBreakingThreshold(); double dist = capsuleCapsuleDistance( out normalOnB, out pointOnBWorld , capsuleA.getHalfHeight(), capsuleA.getRadius() , capsuleB.getHalfHeight(), capsuleB.getRadius() , capsuleA.getUpAxis(), capsuleB.getUpAxis() , ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform, threshold ); if( dist < threshold ) { Debug.Assert( normalOnB.length2() >= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) ); resultOut.addContactPoint( ref normalOnB, ref pointOnBWorld, dist ); } resultOut.refreshContactPoints(); return; } #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(),body1.getWorldTransform()); } if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0) #endif //USE_SEPDISTANCE_UTIL2 { btGjkPairDetector.ClosestPointInput input = BulletGlobals.ClosestPointInputPool.Get(); input.Initialize(); btGjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get(); gjkPairDetector.Initialize( min0, min1, m_simplexSolver, m_pdSolver ); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.setMinkowskiA( min0 ); gjkPairDetector.setMinkowskiB( min1 ); #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { input.m_maximumDistanceSquared = BT_LARGE_FLOAT; } else #endif //USE_SEPDISTANCE_UTIL2 { //if (dispatchInfo.m_convexMaxDistanceUseCPT) //{ // input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactProcessingThreshold(); //} else //{ input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactBreakingThreshold(); // } input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared; } input.m_transformA = body0Transform; input.m_transformB = body1Transform; #if USE_SEPDISTANCE_UTIL2 double sepDist = 0; if (dispatchInfo.m_useConvexConservativeDistanceUtil) { sepDist = gjkPairDetector.getCachedSeparatingDistance(); if (sepDist>SIMD_EPSILON) { sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; //now perturbe directions to get multiple contact points } } #endif //USE_SEPDISTANCE_UTIL2 if( min0.isPolyhedral() && min1.isPolyhedral() ) { btDummyResult dummy = new btDummyResult(); ///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it double min0Margin = min0.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min0.getMargin(); double min1Margin = min1.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min1.getMargin(); btWithoutMarginResult withoutMargin = new btWithoutMarginResult( resultOut, min0Margin, min1Margin ); btPolyhedralConvexShape polyhedronA = (btPolyhedralConvexShape)min0; btPolyhedralConvexShape polyhedronB = (btPolyhedralConvexShape)min1; if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getConvexPolyhedron() != null ) { double threshold = m_manifoldPtr.getContactBreakingThreshold(); double minDist = -1e30f; btVector3 sepNormalWorldSpace; bool foundSepAxis = true; if( dispatchInfo.m_enableSatConvex ) { foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis( polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform, out sepNormalWorldSpace, resultOut ); } else { #if ZERO_MARGIN gjkPairDetector.setIgnoreMargin(true); gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); #else gjkPairDetector.getClosestPoints( input, withoutMargin, dispatchInfo.m_debugDraw ); //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); #endif //ZERO_MARGIN //double l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); //if (l2>SIMD_EPSILON) { sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1/l2); //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0.getMargin()+min1.getMargin(); #if ZERO_MARGIN foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0; #else foundSepAxis = withoutMargin.m_foundResult && minDist < 0;//-(min0.getMargin()+min1.getMargin()); #endif } } if( foundSepAxis ) { // Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z); btPolyhedralContactClipping.clipHullAgainstHull( ref sepNormalWorldSpace, polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform , minDist - threshold, threshold, resultOut ); } if( m_ownManifold ) { resultOut.refreshContactPoints(); } BulletGlobals.ClosestPointInputPool.Free( input ); BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector ); return; } else { //we can also deal with convex versus triangle (without connectivity data) if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE ) { btVertexArray vertices = new btVertexArray(); btTriangleShape tri = (btTriangleShape)polyhedronB; btVector3 tmp; body1Transform.Apply( ref tri.m_vertices1, out tmp ); vertices.Add( ref tmp ); body1Transform.Apply( ref tri.m_vertices2, out tmp ); vertices.Add( ref tmp ); body1Transform.Apply( ref tri.m_vertices3, out tmp ); vertices.Add( ref tmp ); //tri.initializePolyhedralFeatures(); double threshold = m_manifoldPtr.getContactBreakingThreshold(); btVector3 sepNormalWorldSpace; double minDist = -btScalar.BT_LARGE_FLOAT; double maxDist = threshold; bool foundSepAxis = false; if( false ) { polyhedronB.initializePolyhedralFeatures(); foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis( polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform, out sepNormalWorldSpace, resultOut ); // Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z); btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace , polyhedronA.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut ); } else { #if ZERO_MARGIN gjkPairDetector.setIgnoreMargin(true); gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); #else gjkPairDetector.getClosestPoints( input, dummy, dispatchInfo.m_debugDraw ); #endif//ZERO_MARGIN double l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); if( l2 > btScalar.SIMD_EPSILON ) { gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace ); //minDist = gjkPairDetector.getCachedSeparatingDistance(); //maxDist = threshold; minDist = gjkPairDetector.getCachedSeparatingDistance() - min0.getMargin() - min1.getMargin(); //foundSepAxis = true; btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace , polyhedronA.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut ); } else { //sepNormalWorldSpace = btVector3.Zero; } } if( m_ownManifold ) { resultOut.refreshContactPoints(); } BulletGlobals.ClosestPointInputPool.Free( input ); BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector ); return; } } } gjkPairDetector.getClosestPoints( input, resultOut, dispatchInfo.m_debugDraw ); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points if( m_numPerturbationIterations != 0 && resultOut.m_manifoldPtr.m_cachedPoints < m_minimumPointsPerturbationThreshold ) { int i; btVector3 v0, v1; btVector3 sepNormalWorldSpace; double l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); if( l2 > btScalar.SIMD_EPSILON ) { gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace ); btVector3.btPlaneSpace1( ref sepNormalWorldSpace, out v0, out v1 ); bool perturbeA = true; double angleLimit = 0.125f * btScalar.SIMD_PI; double perturbeAngle; double radiusA = min0.getAngularMotionDisc(); double radiusB = min1.getAngularMotionDisc(); if( radiusA < radiusB ) { perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusA; perturbeA = true; } else { perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusB; perturbeA = false; } if( perturbeAngle > angleLimit ) perturbeAngle = angleLimit; btTransform unPerturbedTransform; if( perturbeA ) { unPerturbedTransform = input.m_transformA; } else { unPerturbedTransform = input.m_transformB; } for( i = 0; i < m_numPerturbationIterations; i++ ) { if( v0.length2() > btScalar.SIMD_EPSILON ) { btQuaternion perturbeRot = new btQuaternion( ref v0, perturbeAngle ); double iterationAngle = i * ( btScalar.SIMD_2_PI / (double)( m_numPerturbationIterations ) ); btQuaternion rotq = new btQuaternion( ref sepNormalWorldSpace, iterationAngle ); if( perturbeA ) { btQuaternion tmpq; btQuaternion tmpq2; rotq.inverse( out tmpq ); btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 ); btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq ); btMatrix3x3 m = new btMatrix3x3( ref tmpq ); btMatrix3x3 m2; body0Transform.getBasis( out m2 ); btMatrix3x3 m3; btMatrix3x3.Mult( ref m, ref m2, out m3 ); input.m_transformA.setBasis( ref m3 ); input.m_transformB = body1Transform; #if DEBUG_CONTACTS dispatchInfo.m_debugDraw.drawTransform(input.m_transformA,10.0); #endif //DEBUG_CONTACTS } else { btQuaternion tmpq; btQuaternion tmpq2; rotq.inverse( out tmpq ); btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 ); btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq ); btMatrix3x3 m = new btMatrix3x3( ref tmpq ); btMatrix3x3 m2; body1Transform.getBasis( out m2 ); btMatrix3x3 m3; btMatrix3x3.Mult( ref m, ref m2, out m3 ); input.m_transformA = body0Transform; input.m_transformB.setBasis( ref m3 ); #if DEBUG_CONTACTS dispatchInfo.m_debugDraw.drawTransform(input.m_transformB,10.0); #endif } btPerturbedContactResult perturbedResultOut = BulletGlobals.PerturbedContactResultPool.Get(); if( perturbeA ) perturbedResultOut.Initialize( resultOut , ref input.m_transformA, ref input.m_transformB , ref input.m_transformA , perturbeA , dispatchInfo.m_debugDraw ); else perturbedResultOut.Initialize( resultOut , ref input.m_transformA, ref input.m_transformB , ref input.m_transformB , perturbeA , dispatchInfo.m_debugDraw ); gjkPairDetector.getClosestPoints( input, perturbedResultOut, dispatchInfo.m_debugDraw ); BulletGlobals.PerturbedContactResultPool.Free( perturbedResultOut ); } } } } #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON)) { m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform()); } #endif //USE_SEPDISTANCE_UTIL2 BulletGlobals.ClosestPointInputPool.Free( input ); BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector ); } if( m_ownManifold ) { resultOut.refreshContactPoints(); } }
///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia ///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform ///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform ///of the collision object by the principal transform. void calculatePrincipalAxisTransform( double[] masses, ref btTransform principal, ref btVector3 inertia ) { int n = m_children.Count; double totalMass = 0; btVector3 center = btVector3.Zero; int k; for( k = 0; k < n; k++ ) { Debug.Assert( masses[k] > 0 ); center.AddScale( ref m_children[k].m_transform.m_origin, masses[k], out center ); totalMass += masses[k]; } Debug.Assert( totalMass > 0 ); center /= totalMass; principal.setOrigin( ref center ); btMatrix3x3 tensor = new btMatrix3x3( 0, 0, 0, 0, 0, 0, 0, 0, 0); for( k = 0; k < n; k++ ) { btVector3 i; m_children[k].m_childShape.calculateLocalInertia( masses[k], out i ); btTransform t = m_children[k].m_transform; btVector3 o; t.m_origin.Sub(ref center, out o ); //compute inertia tensor in coordinate system of compound shape btMatrix3x3 j; t.m_basis.transpose( out j); j.m_el0 *= i.x; j.m_el1 *= i.y; j.m_el2 *= i.z; btMatrix3x3 j2; t.m_basis.Mult( ref j, out j2 ); //add inertia tensor tensor.m_el0 += j2.m_el0; tensor.m_el1 += j2.m_el1; tensor.m_el2 += j2.m_el2; //compute inertia tensor of pointmass at o double o2 = o.length2(); j[0].setValue( o2, 0, 0 ); j[1].setValue( 0, o2, 0 ); j[2].setValue( 0, 0, o2 ); j.m_el0 += o * -o.x; j.m_el1 += o * -o.y; j.m_el2 += o * -o.z; //add inertia tensor of pointmass tensor.m_el0.AddScale( ref j.m_el0, masses[k], out tensor.m_el0 ); tensor.m_el1.AddScale( ref j.m_el1, masses[k], out tensor.m_el1 ); tensor.m_el2.AddScale( ref j.m_el2, masses[k], out tensor.m_el2 ); } tensor.diagonalize( out principal.m_basis, (double)( 0.00001 ), 20 ); inertia.setValue( tensor[0][0], tensor[1][1], tensor[2][2] ); }
void collideSingleContact( bool usePertube, ref btQuaternion perturbeRot , btCollisionObjectWrapper convexObjWrap , ref btTransform convexTransform , btCollisionObjectWrapper planeObjWrap , ref btTransform planeTransform , btDispatcherInfo dispatchInfo, btManifoldResult resultOut , ref btVector3 planeNormal, double planeConstant ) { //btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap; //btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap; btConvexShape convexShape = (btConvexShape)convexObjWrap.getCollisionShape(); btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.getCollisionShape(); bool hasCollision = false; //planeNormal = planeShape.getPlaneNormal().Copy( out planeNormal ); //double planeConstant = planeShape.getPlaneConstant(); btTransform convexWorldTransform = convexTransform; //btTransform planeWorldTransform = planeObjWrap.m_worldTransform; btTransform convexInPlaneTrans; planeTransform.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans ); if( usePertube ) { //now perturbe the convex-world transform btMatrix3x3 perturbeMat = new btMatrix3x3( ref perturbeRot ); btMatrix3x3 tmpPerturbe; convexWorldTransform.m_basis.Apply( ref perturbeMat, out tmpPerturbe ); convexWorldTransform.m_basis = tmpPerturbe; //convexWorldTransform.getBasis() *= btMatrix3x3( perturbeRot ); } btTransform planeInConvex; convexTransform.inverseTimes( ref planeObjWrap.m_collisionObject.m_worldTransform, out planeInConvex ); btVector3 tmp, tmp2; planeNormal.Invert( out tmp ); planeInConvex.m_basis.Apply( ref tmp, out tmp2 ); btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp2, out vtx ); btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane ); double distance = ( planeNormal.dot( ref vtxInPlane ) - planeConstant ); btVector3 vtxInPlaneProjected; vtxInPlane.AddScale( ref planeNormal, -distance, out vtxInPlaneProjected ); btVector3 vtxInPlaneWorld; planeTransform.Apply( ref vtxInPlaneProjected, out vtxInPlaneWorld ); hasCollision = distance < m_manifoldPtr.getContactBreakingThreshold(); resultOut.setPersistentManifold( m_manifoldPtr ); if( hasCollision ) { /// report a contact. internally this will be kept persistent, and contact reduction is done btVector3 normalOnSurfaceB; planeTransform.m_basis.Apply( ref planeNormal, out normalOnSurfaceB ); btScalar.Dbg( "Convex plane adds point " + normalOnSurfaceB + " " + vtxInPlaneWorld + " " + distance.ToString( "g17" ) ); resultOut.addContactPoint( ref normalOnSurfaceB, ref vtxInPlaneWorld, distance ); } }
public void dot3( ref btMatrix3x3 m, out btVector3 result ) { result.x = dot( ref m.m_el0 ); result.y = dot( ref m.m_el1 ); result.z = dot( ref m.m_el2 ); result.w = 0; }
/* 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 ); } } }
bool matrixToEulerZXY( ref btMatrix3x3 mat, ref btVector3 xyz ) { // rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx // cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx // -cx*sy sx cx*cy double fi = btGetMatrixElem( ref mat, 7 ); if( fi < (double)( 1.0f ) ) { if( fi > (double)( -1.0f ) ) { xyz[0] = btScalar.btAsin( btGetMatrixElem( ref mat, 7 ) ); xyz[1] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 6 ), btGetMatrixElem( ref mat, 8 ) ); xyz[2] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 1 ), btGetMatrixElem( ref mat, 4 ) ); return true; } else { xyz[0] = -btScalar.SIMD_HALF_PI; xyz[1] = (double)( 0.0 ); xyz[2] = -btScalar.btAtan2( btGetMatrixElem( ref mat, 2 ), btGetMatrixElem( ref mat, 0 ) ); return false; } } else { xyz[0] = btScalar.SIMD_HALF_PI; xyz[1] = (double)( 0.0 ); xyz[2] = btScalar.btAtan2( btGetMatrixElem( ref mat, 2 ), btGetMatrixElem( ref mat, 0 ) ); } return false; }
public void evalEulerEqnDeriv( ref btVector3 w1, ref btVector3 w0, double dt, ref btMatrix3x3 I, out btMatrix3x3 result ) { btMatrix3x3 w1x, Iw1x; btMatrix3x3 tmpm; btMatrix3x3 tmp2; btVector3 Iwi; I.Apply( ref w1, out Iwi ); w1.getSkewSymmetricMatrix( out w1x.m_el0, out w1x.m_el1, out w1x.m_el2 ); #if !DISABLE_ROW4 w1x.m_el3 = btVector3.wAxis; #endif Iwi.getSkewSymmetricMatrix( out Iw1x.m_el0, out Iw1x.m_el1, out Iw1x.m_el2 ); #if !DISABLE_ROW4 Iw1x.m_el3 = btVector3.wAxis; #endif I.Mult( ref w1x, out tmpm ); tmpm.Sub( ref Iw1x, out tmp2 ); tmp2.Mult( dt, out result ); //btMatrix3x3 dfw1 = I + ( w1x * I - Iw1x ) * dt; //return dfw1; }
bool matrixToEulerZYX( ref btMatrix3x3 mat, ref btVector3 xyz ) { // rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy // cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx // -sy cy*sx cy*cx double fi = btGetMatrixElem( ref mat, 6 ); if( fi < (double)( 1.0f ) ) { if( fi > (double)( -1.0f ) ) { xyz[0] = btScalar.btAtan2( btGetMatrixElem( ref mat, 7 ), btGetMatrixElem( ref mat, 8 ) ); xyz[1] = btScalar.btAsin( -btGetMatrixElem( ref mat, 6 ) ); xyz[2] = btScalar.btAtan2( btGetMatrixElem( ref mat, 3 ), btGetMatrixElem( ref mat, 0 ) ); return true; } else { xyz[0] = (double)( 0.0 ); xyz[1] = btScalar.SIMD_HALF_PI; xyz[2] = -btScalar.btAtan2( btGetMatrixElem( ref mat, 1 ), btGetMatrixElem( ref mat, 2 ) ); return false; } } else { xyz[0] = (double)( 0.0 ); xyz[1] = -btScalar.SIMD_HALF_PI; xyz[2] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 1 ), -btGetMatrixElem( ref mat, 2 ) ); } return false; }
public double computeAngularImpulseDenominator( ref btVector3 axis, btMatrix3x3 invInertiaWorld ) { btVector3 vec = axis * invInertiaWorld; return axis.dot( vec ); }