public override void addConstraint( btTypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies = false ) { m_constraints.Add( constraint ); //Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?) Debug.Assert( constraint.getRigidBodyA() != constraint.getRigidBodyB() ); if( disableCollisionsBetweenLinkedBodies ) { constraint.m_rbA.addConstraintRef( constraint ); constraint.m_rbB.addConstraintRef( constraint ); } }
public override void removeConstraint( btTypedConstraint constraint ) { m_constraints.Remove( constraint ); constraint.m_rbA.removeConstraintRef( constraint ); constraint.m_rbB.removeConstraintRef( constraint ); }
public void setup( btContactSolverInfo solverInfo , btTypedConstraint[] sortedConstraints, int numConstraints , btIDebugDraw debugDrawer ) { Debug.Assert( solverInfo != null ); m_solverInfo = solverInfo; m_sortedConstraints = sortedConstraints; m_numConstraints = numConstraints; m_debugDrawer = debugDrawer; m_bodies.Count = ( 0 ); m_manifolds.Count = ( 0 ); m_constraints.Count = ( 0 ); }
public static int btGetConstraintIslandId( btTypedConstraint lhs ) { int islandId; btCollisionObject rcolObj0 = lhs.getRigidBodyA(); btCollisionObject rcolObj1 = lhs.getRigidBodyB(); islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); return islandId; }
static bool compare( btTypedConstraint lhs, btTypedConstraint rhs ) { int rIslandId0, lIslandId0; rIslandId0 = btDiscreteDynamicsWorld.btGetConstraintIslandId( rhs ); lIslandId0 = btDiscreteDynamicsWorld.btGetConstraintIslandId( lhs ); return lIslandId0 < rIslandId0; }
internal void debugDrawConstraint( btTypedConstraint constraint ) { bool drawFrames = ( getDebugDrawer().getDebugMode() & btIDebugDraw.DebugDrawModes.DBG_DrawConstraints ) != 0; bool drawLimits = ( getDebugDrawer().getDebugMode() & btIDebugDraw.DebugDrawModes.DBG_DrawConstraintLimits ) != 0; double dbgDrawSize = constraint.getDbgDrawSize(); if( dbgDrawSize <= (double)( 0 ) ) { return; } switch( constraint.getConstraintType() ) { case btObjectTypes.POINT2POINT_CONSTRAINT_TYPE: { btPoint2PointConstraint p2pC = (btPoint2PointConstraint)constraint; btTransform tr = btTransform.Identity; btVector3 pivot; p2pC.getPivotInA( out pivot ); btVector3 tmp; p2pC.getRigidBodyA().m_worldTransform.Apply( ref pivot, out tmp ); tr.setOrigin( ref tmp ); getDebugDrawer().drawTransform( ref tr, dbgDrawSize ); // that ideally should draw the same frame p2pC.getPivotInB( out pivot ); p2pC.getRigidBodyB().m_worldTransform.Apply( ref pivot, out tmp ); tr.setOrigin( ref tmp ); if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize ); } break; case btObjectTypes.HINGE_CONSTRAINT_TYPE: { btHingeConstraint pHinge = (btHingeConstraint)constraint; btTransform tr; pHinge.getRigidBodyA().m_worldTransform.Apply( ref pHinge.m_rbAFrame, out tr ); if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize ); pHinge.getRigidBodyB().m_worldTransform.Apply( ref pHinge.m_rbBFrame, out tr ); if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize ); double minAng = pHinge.getLowerLimit(); double maxAng = pHinge.getUpperLimit(); if( minAng == maxAng ) { break; } bool drawSect = true; if( !pHinge.hasLimit() ) { minAng = (double)( 0 ); maxAng = btScalar.SIMD_2_PI; drawSect = false; } if( drawLimits ) { btVector3 center = tr.m_origin; btVector3 normal; tr.m_basis.getColumn( 2, out normal ); btVector3 axis; tr.m_basis.getColumn( 0, out axis ); getDebugDrawer().drawArc( ref tr.m_origin, ref normal, ref axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, ref btVector3.Zero, drawSect ); } } break; case btObjectTypes.CONETWIST_CONSTRAINT_TYPE: { btConeTwistConstraint pCT = (btConeTwistConstraint)constraint; btTransform tr; pCT.getRigidBodyA().m_worldTransform.Apply( ref pCT.m_rbAFrame, out tr ); if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize ); pCT.m_rbB.m_worldTransform.Apply( ref pCT.m_rbBFrame, out tr ); if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize ); if( drawLimits ) { //double length = (double)(5); double length = dbgDrawSize; int nSegments = 8 * 4; double fAngleInRadians = btScalar.SIMD_2_PI * (double)( nSegments - 1 ) / (double)( nSegments ); btVector3 tmp; btVector3 pPrev; pCT.GetPointForAngle( fAngleInRadians, length, out tmp ); tr.Apply( ref tmp, out pPrev ); for( int i = 0; i < nSegments; i++ ) { fAngleInRadians = btScalar.SIMD_2_PI * (double)i / (double)( nSegments ); btVector3 pCur; pCT.GetPointForAngle( fAngleInRadians, length, out pCur ); //btVector3 tmp; tr.Apply( ref pCur, out tmp ); getDebugDrawer().drawLine( ref pPrev, ref tmp, ref btVector3.Zero ); if( i % ( nSegments / 8 ) == 0 ) getDebugDrawer().drawLine( ref tr.m_origin, ref pCur, ref btVector3.Zero ); pPrev = pCur; } double tws = pCT.getTwistSpan(); double twa = pCT.getTwistAngle(); bool useFrameB = ( pCT.getRigidBodyB().getInvMass() > (double)( 0 ) ); if( useFrameB ) { pCT.getRigidBodyB().m_worldTransform.Apply( ref pCT.m_rbBFrame, out tr ); } else { pCT.getRigidBodyA().m_worldTransform.Apply( ref pCT.m_rbBFrame, out tr ); } btVector3 pivot = tr.m_origin; btVector3 normal; tr.m_basis.getColumn( 0, out normal ); btVector3 axis1; tr.m_basis.getColumn( 1, out axis1 ); getDebugDrawer().drawArc( ref pivot, ref normal, ref axis1, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, ref btVector3.Zero, true ); } } break; case btObjectTypes.D6_SPRING_CONSTRAINT_TYPE: case btObjectTypes.D6_CONSTRAINT_TYPE: { btGeneric6DofConstraint p6DOF = (btGeneric6DofConstraint)constraint; btTransform tr;// = p6DOF.getCalculatedTransformA(); if( drawFrames ) getDebugDrawer().drawTransform( ref p6DOF.m_calculatedTransformA, dbgDrawSize ); //tr = p6DOF.getCalculatedTransformB(); if( drawFrames ) getDebugDrawer().drawTransform( ref p6DOF.m_calculatedTransformB, dbgDrawSize ); if( drawLimits ) { tr = p6DOF.m_calculatedTransformA; btVector3 center = p6DOF.m_calculatedTransformB.m_origin; btVector3 up = tr.m_basis.getColumn( 2 ); btVector3 axis = tr.m_basis.getColumn( 0 ); double minTh = p6DOF.getRotationalLimitMotor( 1 ).m_loLimit; double maxTh = p6DOF.getRotationalLimitMotor( 1 ).m_hiLimit; double minPs = p6DOF.getRotationalLimitMotor( 2 ).m_loLimit; double maxPs = p6DOF.getRotationalLimitMotor( 2 ).m_hiLimit; getDebugDrawer().drawSpherePatch( ref center, ref up, ref axis, dbgDrawSize * (double)( .9f ), minTh, maxTh, minPs, maxPs, ref btVector3.Zero ); axis = tr.m_basis.getColumn( 1 ); double ay = p6DOF.getAngle( 1 ); double az = p6DOF.getAngle( 2 ); double cy = btScalar.btCos( ay ); double sy = btScalar.btSin( ay ); double cz = btScalar.btCos( az ); double sz = btScalar.btSin( az ); btVector3 ref_point; ref_point.x = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2]; ref_point.y = -sz * axis[0] + cz * axis[1]; ref_point.z = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2]; ref_point.w = 0; tr = p6DOF.m_calculatedTransformB; btVector3 normal; tr.m_basis.getColumn( 0, out normal ); normal.Invert( out normal ); double minFi = p6DOF.getRotationalLimitMotor( 0 ).m_loLimit; double maxFi = p6DOF.getRotationalLimitMotor( 0 ).m_hiLimit; if( minFi > maxFi ) { getDebugDrawer().drawArc( ref center, ref normal, ref ref_point, dbgDrawSize, dbgDrawSize, -btScalar.SIMD_PI, btScalar.SIMD_PI, ref btVector3.Zero, false ); } else if( minFi < maxFi ) { getDebugDrawer().drawArc( ref center, ref normal, ref ref_point, dbgDrawSize, dbgDrawSize, minFi, maxFi, ref btVector3.Zero, true ); } tr = p6DOF.m_calculatedTransformA; btVector3 bbMin = p6DOF.getTranslationalLimitMotor().m_lowerLimit; btVector3 bbMax = p6DOF.getTranslationalLimitMotor().m_upperLimit; getDebugDrawer().drawBox( ref bbMin, ref bbMax, ref tr, ref btVector3.Zero ); } } break; ///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage case btObjectTypes.D6_SPRING_2_CONSTRAINT_TYPE: { { btGeneric6DofSpring2Constraint p6DOF = (btGeneric6DofSpring2Constraint)constraint; //btTransform tr;// = p6DOF.getCalculatedTransformA(); if( drawFrames ) getDebugDrawer().drawTransform( ref p6DOF.m_calculatedTransformA, dbgDrawSize ); //tr = p6DOF.getCalculatedTransformB(); if( drawFrames ) getDebugDrawer().drawTransform( ref p6DOF.m_calculatedTransformB, dbgDrawSize ); if( drawLimits ) { //tr = p6DOF.getCalculatedTransformA(); btVector3 center = p6DOF.m_calculatedTransformB.m_origin; btVector3 up = p6DOF.m_calculatedTransformA.m_basis.getColumn( 2 ); btVector3 axis = p6DOF.m_calculatedTransformA.m_basis.getColumn( 0 ); double minTh = p6DOF.getRotationalLimitMotor( 1 ).m_loLimit; double maxTh = p6DOF.getRotationalLimitMotor( 1 ).m_hiLimit; double minPs = p6DOF.getRotationalLimitMotor( 2 ).m_loLimit; double maxPs = p6DOF.getRotationalLimitMotor( 2 ).m_hiLimit; getDebugDrawer().drawSpherePatch( ref center, ref up, ref axis, dbgDrawSize * (double)( .9f ), minTh, maxTh, minPs, maxPs, ref btVector3.Zero ); axis = p6DOF.m_calculatedTransformA.m_basis.getColumn( 1 ); double ay = p6DOF.getAngle( 1 ); double az = p6DOF.getAngle( 2 ); double cy = btScalar.btCos( ay ); double sy = btScalar.btSin( ay ); double cz = btScalar.btCos( az ); double sz = btScalar.btSin( az ); btVector3 ref_point; ref_point.x = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2]; ref_point.y = -sz * axis[0] + cz * axis[1]; ref_point.z = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2]; ref_point.w = 0; //tr = p6DOF.getCalculatedTransformB(); btVector3 normal; p6DOF.m_calculatedTransformB.m_basis.getColumn( 0, out normal ); normal.Invert( out normal ); double minFi = p6DOF.getRotationalLimitMotor( 0 ).m_loLimit; double maxFi = p6DOF.getRotationalLimitMotor( 0 ).m_hiLimit; if( minFi > maxFi ) { getDebugDrawer().drawArc( ref center, ref normal, ref ref_point, dbgDrawSize, dbgDrawSize, -btScalar.SIMD_PI, btScalar.SIMD_PI, ref btVector3.Zero, false ); } else if( minFi < maxFi ) { getDebugDrawer().drawArc( ref center, ref normal, ref ref_point, dbgDrawSize, dbgDrawSize, minFi, maxFi, ref btVector3.Zero, true ); } //tr = p6DOF.getCalculatedTransformA(); btVector3 bbMin = p6DOF.getTranslationalLimitMotor().m_lowerLimit; btVector3 bbMax = p6DOF.getTranslationalLimitMotor().m_upperLimit; getDebugDrawer().drawBox( ref bbMin, ref bbMax, ref p6DOF.m_calculatedTransformA, ref btVector3.Zero ); } } break; } case btObjectTypes.SLIDER_CONSTRAINT_TYPE: { btSliderConstraint pSlider = (btSliderConstraint)constraint; //btTransform tr = pSlider.m_calculatedTransformA; if( drawFrames ) getDebugDrawer().drawTransform( ref pSlider.m_calculatedTransformA, dbgDrawSize ); //tr = pSlider.getCalculatedTransformB(); if( drawFrames ) getDebugDrawer().drawTransform( ref pSlider.m_calculatedTransformB, dbgDrawSize ); if( drawLimits ) { btTransform tr = pSlider.getUseLinearReferenceFrameA() ? pSlider.m_calculatedTransformA : pSlider.m_calculatedTransformB; btVector3 tmp = new btVector3( pSlider.getLowerLinLimit(), 0, 0 ); btVector3 li_min; tr.Apply( ref tmp, out li_min ); tmp = new btVector3( pSlider.getUpperLinLimit(), 0, 0 ); btVector3 li_max; tr.Apply( ref tmp, out li_max ); getDebugDrawer().drawLine( ref li_min, ref li_max, ref btVector3.Zero ); btVector3 normal = tr.m_basis.getColumn( 0 ); btVector3 axis = tr.m_basis.getColumn( 1 ); double a_min = pSlider.getLowerAngLimit(); double a_max = pSlider.getUpperAngLimit(); btVector3 center = pSlider.m_calculatedTransformB.m_origin; getDebugDrawer().drawArc( ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, a_min, a_max, ref btVector3.Zero, true ); } } break; default: break; } return; }
public virtual void addConstraint( btTypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies = false ) { //(void)constraint; (void)disableCollisionsBetweenLinkedBodies; }
protected virtual double solveGroupCacheFriendlySetup( btCollisionObject[] bodies, int numBodies , btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds , btTypedConstraint[] constraints, int startConstraint, int numConstraints, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer ) { if( m_fixedBody != null ) { BulletGlobals.SolverBodyPool.Free( m_fixedBody ); m_fixedBody = null; } //m_fixedBodyId = -1; CProfileSample sample = new CProfileSample( "solveGroupCacheFriendlySetup" ); //(void)debugDrawer; m_maxOverrideNumSolverIterations = 0; #if BT_ADDITIONAL_DEBUG //make sure that dynamic bodies exist for all (enabled)raints for (int i=0;i<numConstraints;i++) { btTypedConstraint* constraint = constraints[i]; if (constraint.isEnabled()) { if (!constraint.getRigidBodyA().isStaticOrKinematicObject()) { bool found=false; for (int b=0;b<numBodies;b++) { if (&constraint.getRigidBodyA()==bodies[b]) { found = true; break; } } Debug.Assert(found); } if (!constraint.getRigidBodyB().isStaticOrKinematicObject()) { bool found=false; for (int b=0;b<numBodies;b++) { if (&constraint.getRigidBodyB()==bodies[b]) { found = true; break; } } Debug.Assert(found); } } } //make sure that dynamic bodies exist for all contact manifolds for (int i=0;i<numManifolds;i++) { if (!manifoldPtr[i].getBody0().isStaticOrKinematicObject()) { bool found=false; for (int b=0;b<numBodies;b++) { if (manifoldPtr[i].getBody0()==bodies[b]) { found = true; break; } } Debug.Assert(found); } if (!manifoldPtr[i].getBody1().isStaticOrKinematicObject()) { bool found=false; for (int b=0;b<numBodies;b++) { if (manifoldPtr[i].getBody1()==bodies[b]) { found = true; break; } } Debug.Assert(found); } } #endif //BT_ADDITIONAL_DEBUG for( int i = 0; i < numBodies; i++ ) { bodies[i].setCompanionBody( null ); } m_tmpSolverBodyPool.Capacity = ( numBodies + 1 ); m_tmpSolverBodyPool.Count = ( 0 ); //btSolverBody fixedBody = m_tmpSolverBodyPool.expand(); //initSolverBody(&fixedBody,0); //convert all bodies for( int i = 0; i < numBodies; i++ ) { //int bodyId = getOrInitSolverBody( bodies[i], infoGlobal.m_timeStep ); btRigidBody body = btRigidBody.upcast( bodies[i] ); if( body != null && body.getInvMass() != 0 ) { btSolverBody solverBody = getOrInitSolverBody( bodies[i], infoGlobal.m_timeStep ); btVector3 gyroForce = btVector3.Zero; if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT ) != 0 ) { body.computeGyroscopicForceExplicit( infoGlobal.m_maxGyroscopicForce, out gyroForce ); btVector3 tmp; body.m_invInertiaTensorWorld.ApplyInverse( ref gyroForce, out tmp ); //solverBody.m_externalTorqueImpulse -= gyroForce * body.m_invInertiaTensorWorld * infoGlobal.m_timeStep; solverBody.m_externalTorqueImpulse.SubScale( ref tmp, infoGlobal.m_timeStep, out solverBody.m_externalTorqueImpulse ); } if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD ) != 0 ) { body.computeGyroscopicImpulseImplicit_World( infoGlobal.m_timeStep, out gyroForce ); solverBody.m_externalTorqueImpulse.Add( ref gyroForce, out solverBody.m_externalTorqueImpulse ); } if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY ) != 0 ) { body.computeGyroscopicImpulseImplicit_Body( infoGlobal.m_timeStep, out gyroForce ); btScalar.Dbg( "Gyroforce " + gyroForce ); solverBody.m_externalTorqueImpulse.Add( ref gyroForce, out solverBody.m_externalTorqueImpulse ); } } } if( true ) { int j; for( j = 0; j < numConstraints; j++ ) { btTypedConstraint constraint = constraints[j + startConstraint]; constraint.buildJacobian(); constraint.internalSetAppliedImpulse( 0.0f ); } } //btRigidBody rb0=0,*rb1=0; //if (1) { { int totalNumRows = 0; int i; m_tmpConstraintSizesPool.Capacity = ( numConstraints ); //calculate the total number of contraint rows for( i = 0; i < numConstraints; i++ ) { int infoNumConstraintRows = m_tmpConstraintSizesPool[i].m_numConstraintRows; //btTypedConstraint.btConstraintInfo1 info1 = m_tmpConstraintSizesPool[i]; btTypedConstraint.btJointFeedback fb = constraints[i + startConstraint].getJointFeedback(); if( fb != null ) { fb.m_appliedForceBodyA.setZero(); fb.m_appliedTorqueBodyA.setZero(); fb.m_appliedForceBodyB.setZero(); fb.m_appliedTorqueBodyB.setZero(); } if( constraints[i + startConstraint].isEnabled() ) { constraints[i + startConstraint].getInfo1( ref m_tmpConstraintSizesPool.InternalArray[i] ); } else { m_tmpConstraintSizesPool.InternalArray[i].m_numConstraintRows = 0; m_tmpConstraintSizesPool.InternalArray[i].nub = 0; } totalNumRows += m_tmpConstraintSizesPool.InternalArray[i].m_numConstraintRows; } m_tmpSolverNonContactConstraintPool.Count = ( totalNumRows ); for( i = 0; i < totalNumRows; i++ ) m_tmpSolverNonContactConstraintPool[i] = BulletGlobals.SolverConstraintPool.Get(); ///setup the btSolverConstraints int currentRow = 0; for( i = 0; i < numConstraints; i++ ) { int infoConstraintRows = m_tmpConstraintSizesPool[i].m_numConstraintRows; if( infoConstraintRows != 0 ) { Debug.Assert( currentRow < totalNumRows ); btSolverConstraint currentConstraintRow = m_tmpSolverNonContactConstraintPool[currentRow]; btTypedConstraint constraint = constraints[i + startConstraint]; btRigidBody rbA = constraint.getRigidBodyA(); btRigidBody rbB = constraint.getRigidBodyB(); //int solverBodyIdA = ; //int solverBodyIdB = ; btSolverBody bodyAPtr = getOrInitSolverBody( rbA, infoGlobal.m_timeStep ); btSolverBody bodyBPtr = getOrInitSolverBody( rbB, infoGlobal.m_timeStep ); int overrideNumSolverIterations = constraint.getOverrideNumSolverIterations() > 0 ? constraint.getOverrideNumSolverIterations() : infoGlobal.m_numIterations; if( overrideNumSolverIterations > m_maxOverrideNumSolverIterations ) m_maxOverrideNumSolverIterations = overrideNumSolverIterations; int j; for( j = 0; j < infoConstraintRows; j++ ) { btSolverConstraint current = m_tmpSolverNonContactConstraintPool[currentRow + j]; current.Clear(); //memset( ¤tConstraintRow[j], 0, sizeof( btSolverConstraint ) ); current.m_lowerLimit = btScalar.SIMD_NEG_INFINITY; current.m_upperLimit = btScalar.SIMD_INFINITY; current.m_appliedImpulse = 0; current.m_appliedPushImpulse = 0; current.m_solverBodyA = bodyAPtr; current.m_solverBodyB = bodyBPtr; current.m_overrideNumSolverIterations = overrideNumSolverIterations; } bodyAPtr.Clear(); btTypedConstraint.btConstraintInfo2 info2 = m_tmpConstraintInfo2Pool.Get();// new btTypedConstraint.btConstraintInfo2(); info2.m_numRows = infoConstraintRows; for( j = 0; j < infoConstraintRows; ++j ) { info2.m_solverConstraints[j] = m_tmpSolverNonContactConstraintPool[currentRow + j]; } info2.fps = 1 / infoGlobal.m_timeStep; info2.erp = infoGlobal.m_erp; #if OLD_CONSTRAINT_INFO_INIT info2.m_J1linearAxis = currentConstraintRow.m_contactNormal1; info2.m_J1angularAxis = currentConstraintRow.m_relpos1CrossNormal; info2.m_J2linearAxis = currentConstraintRow.m_contactNormal2; info2.m_J2angularAxis = currentConstraintRow.m_relpos2CrossNormal; info2.rowskip = 0;// sizeof( btSolverConstraint ) / sizeof( double );//check this ///the size of btSolverConstraint needs be a multiple of double //Debug.Assert( info2.rowskip * sizeof( double ) == sizeof( btSolverConstraint ) ); info2.m_constraintError = currentConstraintRow.m_rhs; info2.cfm = currentConstraintRow.m_cfm; info2.m_lowerLimit = currentConstraintRow.m_lowerLimit; info2.m_upperLimit = currentConstraintRow.m_upperLimit; #endif currentConstraintRow.m_cfm = infoGlobal.m_globalCfm; info2.m_damping = infoGlobal.m_damping; info2.m_numIterations = infoGlobal.m_numIterations; constraint.getInfo2( info2 ); ///finalize the constraint setup for( j = 0; j < infoConstraintRows; j++ ) { btSolverConstraint solverConstraint = m_tmpSolverNonContactConstraintPool[currentRow + j]; if( solverConstraint.m_upperLimit >= constraint.getBreakingImpulseThreshold() ) { solverConstraint.m_upperLimit = constraint.getBreakingImpulseThreshold(); } if( solverConstraint.m_lowerLimit <= -constraint.getBreakingImpulseThreshold() ) { solverConstraint.m_lowerLimit = -constraint.getBreakingImpulseThreshold(); } solverConstraint.m_originalContactPoint = constraint; btVector3 tmp; { //solverConstraint.m_angularComponentA = constraint.getRigidBodyA().m_invInertiaTensorWorld // *solverConstraint.m_relpos1CrossNormal * constraint.getRigidBodyA().getAngularFactor(); constraint.m_rbA.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out tmp ); tmp.Mult( ref constraint.m_rbA.m_angularFactor, out solverConstraint.m_angularComponentA ); } { //solverConstraint.m_angularComponentB = constraint.getRigidBodyB().m_invInertiaTensorWorld // * solverConstraint.m_relpos2CrossNormal * constraint.getRigidBodyB().getAngularFactor(); constraint.m_rbB.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out tmp ); tmp.Mult( ref constraint.m_rbB.m_angularFactor, out solverConstraint.m_angularComponentB ); } { btVector3 iMJlA; solverConstraint.m_contactNormal1.Mult( rbA.m_inverseMass, out iMJlA ); btVector3 iMJaA; rbA.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out iMJaA ); btVector3 iMJlB; solverConstraint.m_contactNormal2.Mult( rbB.m_inverseMass, out iMJlB );//sign of normal? btVector3 iMJaB; rbB.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out iMJaB ); double sum = iMJlA.dot( ref solverConstraint.m_contactNormal1 ); sum += iMJaA.dot( ref solverConstraint.m_relpos1CrossNormal ); sum += iMJlB.dot( ref solverConstraint.m_contactNormal2 ); sum += iMJaB.dot( ref solverConstraint.m_relpos2CrossNormal ); double fsum = btScalar.btFabs( sum ); Debug.Assert( fsum > btScalar.SIMD_EPSILON ); btScalar.Dbg( "m_jacDiagABInv 4 set to " + ( fsum > btScalar.SIMD_EPSILON ? btScalar.BT_ONE / sum : 0 ).ToString( "g17" ) ); solverConstraint.m_jacDiagABInv = fsum > btScalar.SIMD_EPSILON ? btScalar.BT_ONE / sum : 0; } { double rel_vel; btVector3 externalForceImpulseA = bodyAPtr.m_originalBody != null ? bodyAPtr.m_externalForceImpulse : btVector3.Zero; btVector3 externalTorqueImpulseA = bodyAPtr.m_originalBody != null ? bodyAPtr.m_externalTorqueImpulse : btVector3.Zero; btVector3 externalForceImpulseB = bodyBPtr.m_originalBody != null ? bodyBPtr.m_externalForceImpulse : btVector3.Zero; btVector3 externalTorqueImpulseB = bodyBPtr.m_originalBody != null ? bodyBPtr.m_externalTorqueImpulse : btVector3.Zero; btScalar.Dbg( "external torque2 impulses " + externalTorqueImpulseA + externalTorqueImpulseB ); double vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref rbA.m_linearVelocity, ref externalForceImpulseA ) + solverConstraint.m_relpos1CrossNormal.dotAdded( ref rbA.m_angularVelocity, ref externalTorqueImpulseA ); double vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref rbB.m_linearVelocity, ref externalForceImpulseB ) + solverConstraint.m_relpos2CrossNormal.dotAdded( ref rbB.m_angularVelocity, ref externalTorqueImpulseB ); rel_vel = vel1Dotn + vel2Dotn; double restitution = 0; double positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 double velocityError = restitution - rel_vel * info2.m_damping; double penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; btScalar.Dbg( "Constraint 5 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) ); solverConstraint.m_appliedImpulse = 0; } } } currentRow += m_tmpConstraintSizesPool[i].m_numConstraintRows; } } btScalar.Dbg( "About to convert contacts " + start_manifold + " " + numManifolds ); convertContacts( manifoldPtr, start_manifold, numManifolds, infoGlobal ); } // btContactSolverInfo info = infoGlobal; int numNonContactPool = m_tmpSolverNonContactConstraintPool.Count; int numConstraintPool = m_tmpSolverContactConstraintPool.Count; int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count; ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints m_orderNonContactConstraintPool.Capacity = ( numNonContactPool ); if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 ) m_orderTmpConstraintPool.Count = m_orderTmpConstraintPool.Capacity = ( numConstraintPool * 2 ); else m_orderTmpConstraintPool.Count = m_orderTmpConstraintPool.Capacity = ( numConstraintPool ); m_orderFrictionConstraintPool.Count = m_orderFrictionConstraintPool.Capacity = ( numFrictionPool ); { int i; for( i = 0; i < numNonContactPool; i++ ) { m_orderNonContactConstraintPool[i] = i; } for( i = 0; i < numConstraintPool; i++ ) { m_orderTmpConstraintPool[i] = i; } for( i = 0; i < numFrictionPool; i++ ) { m_orderFrictionConstraintPool[i] = i; } } return 0; }
/// btSequentialImpulseConstraintSolver Sequentially applies impulses internal override double solveGroup( btCollisionObject[] bodies, int numBodies , btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds , btTypedConstraint[] constraints, int startConstraint, int numConstraints , btContactSolverInfo infoGlobal , btIDebugDraw debugDrawer, btDispatcher dispatcher ) { CProfileSample sample = new CProfileSample( "solveGroup" ); //you need to provide at least some bodies solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer ); solveGroupCacheFriendlyIterations( bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer ); solveGroupCacheFriendlyFinish( bodies, numBodies, infoGlobal ); return 0; }
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). ///If no axis is provided, it uses the default axis for this constraint. public override void setParam( btTypedConstraint.btConstraintParams num, double value, int axis = -1 ) { if( ( axis == -1 ) || ( axis == 5 ) ) { switch( num ) { case btConstraintParams.BT_CONSTRAINT_STOP_ERP: m_stopERP = value; m_flags |= btHingeFlags.BT_HINGE_FLAGS_ERP_STOP; break; case btConstraintParams.BT_CONSTRAINT_STOP_CFM: m_stopCFM = value; m_flags |= btHingeFlags.BT_HINGE_FLAGS_CFM_STOP; break; case btConstraintParams.BT_CONSTRAINT_CFM: m_normalCFM = value; m_flags |= btHingeFlags.BT_HINGE_FLAGS_CFM_NORM; break; case btConstraintParams.BT_CONSTRAINT_ERP: m_normalERP = value; m_flags |= btHingeFlags.BT_HINGE_FLAGS_ERP_NORM; break; default: btAssertConstrParams( false ); break; } } else { btAssertConstrParams( false ); } }
protected virtual double solveGroupCacheFriendlyIterations( btCollisionObject[] bodies, int numBodies , btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds , btTypedConstraint[] constraints, int startConstraint, int numConstraints , btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer ) { CProfileSample sample = new CProfileSample( "solveGroupCacheFriendlyIterations" ); { ///this is a special step to resolve penetrations (just for contacts) solveGroupCacheFriendlySplitImpulseIterations( bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer ); int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; for( int iteration = 0; iteration < maxIterations; iteration++ ) //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) { solveSingleIteration( iteration, bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer ); } } return 0; }
protected virtual void solveGroupCacheFriendlySplitImpulseIterations( btCollisionObject[] bodies, int numBodies , btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds , btTypedConstraint[] constraints, int startConstraint, int numConstraints, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer ) { int iteration; if( infoGlobal.m_splitImpulse ) { if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_SIMD ) != 0 ) { for( iteration = 0; iteration < infoGlobal.m_numIterations; iteration++ ) { { int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; int j; for( j = 0; j < numPoolConstraints; j++ ) { btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; resolveSplitPenetrationSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); } } } } else { for( iteration = 0; iteration < infoGlobal.m_numIterations; iteration++ ) { { int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; int j; for( j = 0; j < numPoolConstraints; j++ ) { btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; resolveSplitPenetrationImpulseCacheFriendly( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); } } } } } }
protected virtual double solveSingleIteration( int iteration, btCollisionObject[] bodies, int numBodies , btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds , btTypedConstraint[] constraints, int startConstraint, int numConstraints , btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer ) { int numNonContactPool = m_tmpSolverNonContactConstraintPool.Count; int numConstraintPool = m_tmpSolverContactConstraintPool.Count; int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count; if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_RANDMIZE_ORDER ) != 0 ) { if( true ) // uncomment this for a bit less random ((iteration & 7) == 0) { for( int j = 0; j < numNonContactPool; ++j ) { int tmp = m_orderNonContactConstraintPool[j]; int swapi = btRandInt2( j + 1 ); m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; m_orderNonContactConstraintPool[swapi] = tmp; } //contact/friction constraints are not solved more than if( iteration < infoGlobal.m_numIterations ) { for( int j = 0; j < numConstraintPool; ++j ) { int tmp = m_orderTmpConstraintPool[j]; int swapi = btRandInt2( j + 1 ); m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; m_orderTmpConstraintPool[swapi] = tmp; } for( int j = 0; j < numFrictionPool; ++j ) { int tmp = m_orderFrictionConstraintPool[j]; int swapi = btRandInt2( j + 1 ); m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; m_orderFrictionConstraintPool[swapi] = tmp; } } } } if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_SIMD ) != 0 ) { ///solve all joint constraints, using SIMD, if available for( int j = 0; j < m_tmpSolverNonContactConstraintPool.Count; j++ ) { btSolverConstraint constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; if( iteration < constraint.m_overrideNumSolverIterations ) resolveSingleConstraintRowGenericSIMD( constraint.m_solverBodyA , constraint.m_solverBodyB, constraint ); } if( iteration < infoGlobal.m_numIterations ) { for( int j = 0; j < numConstraints; j++ ) { btTypedConstraint constraint = constraints[j + startConstraint]; if( constraint.isEnabled() ) { btSolverBody bodyA = getOrInitSolverBody( constraint.getRigidBodyA(), infoGlobal.m_timeStep ); btSolverBody bodyB = getOrInitSolverBody( constraint.getRigidBodyB(), infoGlobal.m_timeStep ); constraint.solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep ); } } ///solve all contact constraints using SIMD, if available if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS ) != 0 ) { int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; int multiplier = ( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 ) ? 2 : 1; for( int c = 0; c < numPoolConstraints; c++ ) { double totalImpulse = 0; { btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; resolveSingleConstraintRowLowerLimitSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); totalImpulse = solveManifold.m_appliedImpulse; } bool applyFriction = true; if( applyFriction ) { { btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]]; if( totalImpulse > (double)( 0 ) ) { solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse ); solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); } } if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 ) { btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]]; if( totalImpulse > (double)( 0 ) ) { solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse ); solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); } } } } } else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS { //solve the friction constraints after all contact constraints, don't interleave them int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; int j; for( j = 0; j < numPoolConstraints; j++ ) { btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; //resolveSingleConstraintRowLowerLimitSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); gResolveSingleConstraintRowLowerLimit_scalar_reference( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); } ///solve all friction constraints, using SIMD, if available int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.Count; for( j = 0; j < numFrictionPoolConstraints; j++ ) { btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; double totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; if( totalImpulse > (double)( 0 ) ) { solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse ); solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; btScalar.Dbg( "PreFriction Impulse?" + solveManifold.m_appliedImpulse.ToString( "g17" ) ); gResolveSingleConstraintRowGeneric_scalar_reference( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); //resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); btScalar.Dbg( "Friction Impulse?" + solveManifold.m_appliedImpulse.ToString( "g17" ) ); } } int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.Count; for( j = 0; j < numRollingFrictionPoolConstraints; j++ ) { btSolverConstraint rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; double totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; if( totalImpulse > (double)( 0 ) ) { double rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse; if( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction ) rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; gResolveSingleConstraintRowGeneric_scalar_reference( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint ); //resolveSingleConstraintRowGenericSIMD( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint ); } } } } } else { //non-SIMD version ///solve all joint constraints for( int j = 0; j < m_tmpSolverNonContactConstraintPool.Count; j++ ) { btSolverConstraint constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; if( iteration < constraint.m_overrideNumSolverIterations ) resolveSingleConstraintRowGeneric( constraint.m_solverBodyA, constraint.m_solverBodyB, constraint ); } if( iteration < infoGlobal.m_numIterations ) { for( int j = 0; j < numConstraints; j++ ) { btTypedConstraint constraint = constraints[j + startConstraint]; if( constraint.isEnabled() ) { //int bodyAid = ; //int bodyBid = ; btSolverBody bodyA = getOrInitSolverBody( constraint.getRigidBodyA(), infoGlobal.m_timeStep ); btSolverBody bodyB = getOrInitSolverBody( constraint.getRigidBodyB(), infoGlobal.m_timeStep ); constraint.solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep ); } } ///solve all contact constraints int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; for( int j = 0; j < numPoolConstraints; j++ ) { btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; resolveSingleConstraintRowLowerLimit( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); } ///solve all friction constraints int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.Count; for( int j = 0; j < numFrictionPoolConstraints; j++ ) { btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; double totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; if( totalImpulse > (double)( 0 ) ) { solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse ); solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; resolveSingleConstraintRowGeneric( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); } } int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.Count; for( int j = 0; j < numRollingFrictionPoolConstraints; j++ ) { btSolverConstraint rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; double totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; if( totalImpulse > (double)( 0 ) ) { double rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse; if( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction ) rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; resolveSingleConstraintRowGeneric( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint ); } } } } return 0; }
internal void addConstraintRef( btTypedConstraint c ) { ///disable collision with the 'other' body int index = m_constraintRefs.IndexOf( c ); //don't add constraints that are already referenced //Debug.Assert(index == m_constraintRefs.Count); if( index == -1 ) { m_constraintRefs.Add( c ); btCollisionObject colObjA = c.getRigidBodyA(); btCollisionObject colObjB = c.getRigidBodyB(); if( colObjA == this ) { colObjA.setIgnoreCollisionCheck( colObjB, true ); } else { colObjB.setIgnoreCollisionCheck( colObjA, true ); } } }
public virtual void removeConstraint( btTypedConstraint constraint ) { }
internal void removeConstraintRef( btTypedConstraint c ) { int index = m_constraintRefs.IndexOf( c ); //don't remove constraints that are not referenced if( index != -1 ) { m_constraintRefs.Remove( c ); btCollisionObject colObjA = c.getRigidBodyA(); btCollisionObject colObjB = c.getRigidBodyB(); if( colObjA == this ) { colObjA.setIgnoreCollisionCheck( colObjB, false ); } else { colObjB.setIgnoreCollisionCheck( colObjA, false ); } } }
///solve a group of constraints internal abstract double solveGroup( btCollisionObject[] bodies, int numBodies , btPersistentManifold[] manifold, int first_manifold, int numManifolds , btTypedConstraint[] constraints, int startConstraint, int numConstraints , btContactSolverInfo info , btIDebugDraw debugDrawer , btDispatcher dispatcher );