internal void setFrictionConstraintImpulse( btSolverConstraint solverConstraint, btSolverBody bodyA, btSolverBody bodyB, btManifoldPoint cp, btContactSolverInfo infoGlobal ) { btRigidBody rb0 = bodyA.m_originalBody; btRigidBody rb1 = bodyB.m_originalBody; { btSolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_WARMSTARTING ) != 0 ) { frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; btScalar.Dbg( "New Applied source is " + cp.m_appliedImpulseLateral1.ToString( "g17" ) ); if( rb0 != null ) { btVector3 tmp; frictionConstraint1.m_contactNormal1.Mult2( ref rb0.m_linearFactor, rb0.getInvMass(), out tmp ); bodyA.applyImpulse( ref tmp, ref frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse ); } if( rb1 != null ) { btVector3 tmp; frictionConstraint1.m_contactNormal2.Mult2( ref rb1.m_linearFactor, -rb1.getInvMass(), out tmp ); btVector3 tmp2; frictionConstraint1.m_angularComponentB.Invert( out tmp2 ); bodyB.applyImpulse( ref tmp, ref tmp2, -(double)frictionConstraint1.m_appliedImpulse ); } } else { frictionConstraint1.m_appliedImpulse = 0; } } if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 ) { btSolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_WARMSTARTING ) != 0 ) { frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; if( rb0 != null ) { btVector3 tmp; frictionConstraint2.m_contactNormal1.Mult( rb0.getInvMass(), out tmp ); bodyA.applyImpulse( ref tmp, ref frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse ); } if( rb1 != null ) { btVector3 tmp; frictionConstraint2.m_contactNormal2.Mult( -rb1.getInvMass(), out tmp ); btVector3 tmp2; frictionConstraint2.m_angularComponentB.Invert( out tmp2 ); bodyB.applyImpulse( ref tmp, ref tmp2, -(double)frictionConstraint2.m_appliedImpulse ); } } else { frictionConstraint2.m_appliedImpulse = 0; } } }
///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver ///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check. public static double gResolveSingleConstraintRowGeneric_scalar_reference( btSolverBody body1, btSolverBody body2, btSolverConstraint c ) { double deltaImpulse = c.m_rhs - (double)( c.m_appliedImpulse ) * c.m_cfm; double deltaVel1Dotn = c.m_contactNormal1.dot( ref body1.m_deltaLinearVelocity ) + c.m_relpos1CrossNormal.dot( ref body1.m_deltaAngularVelocity ); double deltaVel2Dotn = c.m_contactNormal2.dot( ref body2.m_deltaLinearVelocity ) + c.m_relpos2CrossNormal.dot( ref body2.m_deltaAngularVelocity ); // double delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv; double sum = (double)( c.m_appliedImpulse ) + deltaImpulse; if( sum < c.m_lowerLimit ) { deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse; c.m_appliedImpulse = c.m_lowerLimit; } else if( sum > c.m_upperLimit ) { deltaImpulse = c.m_upperLimit - c.m_appliedImpulse; c.m_appliedImpulse = c.m_upperLimit; } else { c.m_appliedImpulse = sum; } btScalar.Dbg( "Constraint applied impulse is " + c.m_appliedImpulse.ToString( "g17" ) ); btVector3 mass, val; body1.internalGetInvMass( out mass ); mass.Mult( ref c.m_contactNormal1, out val ); body1.applyImpulse( ref val, ref c.m_angularComponentA, deltaImpulse ); body2.internalGetInvMass( out mass ); mass.Mult( ref c.m_contactNormal2, out val ); body2.applyImpulse( ref val, ref c.m_angularComponentB, deltaImpulse ); return deltaImpulse; }
internal void setupRollingFrictionConstraint( btSolverConstraint solverConstraint, ref btVector3 normalAxis1 , btSolverBody solverBodyA, btSolverBody solverBodyB, btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2, btCollisionObject colObj0, btCollisionObject colObj1, double relaxation, double desiredVelocity = 0, double cfmSlip = 0.0 ) { btVector3 normalAxis = btVector3.Zero; solverConstraint.m_contactNormal1 = normalAxis; normalAxis.Invert( out solverConstraint.m_contactNormal2 ); //btSolverBody solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; //btSolverBody solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; btRigidBody body0 = solverBodyA.m_originalBody; btRigidBody body1 = solverBodyB.m_originalBody; solverConstraint.m_solverBodyA = solverBodyA; solverConstraint.m_solverBodyB = solverBodyB; solverConstraint.m_friction = cp.m_combinedRollingFriction; solverConstraint.m_originalContactPoint = null; solverConstraint.m_appliedImpulse = 0; solverConstraint.m_appliedPushImpulse = 0; btVector3 iMJaA; btVector3 iMJaB; { btVector3 ftorqueAxis1; normalAxis1.Invert( out ftorqueAxis1 ); solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; if( body0 != null ) { body0.m_invInertiaTensorWorld.Apply( ref ftorqueAxis1, out iMJaA ); iMJaA.Mult( ref body0.m_angularFactor, out solverConstraint.m_angularComponentA ); } else iMJaA = btVector3.Zero; //solverConstraint.m_angularComponentA = body0 != null ? body0.getInvInertiaTensorWorld() * ftorqueAxis1 * body0.getAngularFactor() : btVector3.Zero; } { btVector3 ftorqueAxis1 = normalAxis1; solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; if( body1 != null ) { body1.m_invInertiaTensorWorld.Apply( ref ftorqueAxis1, out iMJaB ); iMJaB.Mult( ref body1.m_angularFactor, out solverConstraint.m_angularComponentB ); } else iMJaB = btVector3.Zero; //solverConstraint.m_angularComponentB = body1 != null ? body1.getInvInertiaTensorWorld() * ftorqueAxis1 * body1.getAngularFactor() : btVector3.Zero; } { //btVector3 iMJaA = body0 != null ? body0.getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3.Zero; //btVector3 iMJaB = body1 != null ? body1.getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3.Zero; double sum = 0; sum += iMJaA.dot( ref solverConstraint.m_relpos1CrossNormal ); sum += iMJaB.dot( ref solverConstraint.m_relpos2CrossNormal ); btScalar.Dbg( "m_jacDiagABInv 2 set to " + ( btScalar.BT_ONE / sum ).ToString( "g17" ) ); solverConstraint.m_jacDiagABInv = btScalar.BT_ONE / sum; } { double rel_vel; double vel1Dotn; double vel2Dotn; //double vel1Dotn = solverConstraint.m_contactNormal1.dot( body0 != null ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3.Zero ) // + solverConstraint.m_relpos1CrossNormal.dot( body0 != null ? solverBodyA.m_angularVelocity : btVector3.Zero ); //double vel2Dotn = solverConstraint.m_contactNormal2.dot( body1 != null ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3.Zero ) // + solverConstraint.m_relpos2CrossNormal.dot( body1 != null ? solverBodyB.m_angularVelocity : btVector3.Zero ); if( body0 != null ) vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyA.m_linearVelocity, ref solverBodyA.m_externalForceImpulse ) + solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyA.m_angularVelocity ); else vel1Dotn = 0; if( body1 != null ) vel2Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyB.m_linearVelocity, ref solverBodyB.m_externalForceImpulse ) + solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyB.m_angularVelocity ); else vel2Dotn = 0; rel_vel = vel1Dotn + vel2Dotn; // double positionalError = 0; double velocityError = desiredVelocity - rel_vel; double velocityImpulse = velocityError * ( solverConstraint.m_jacDiagABInv ); solverConstraint.m_rhs = velocityImpulse; btScalar.Dbg( "Constraint 2 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) ); solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; } }
internal void setupContactConstraint( btSolverConstraint solverConstraint, btSolverBody bodyA, btSolverBody bodyB, btManifoldPoint cp, btContactSolverInfo infoGlobal, out double relaxation, ref btVector3 rel_pos1, ref btVector3 rel_pos2 ) { // ref btVector3 pos1 = cp.getPositionWorldOnA(); // ref btVector3 pos2 = cp.getPositionWorldOnB(); btRigidBody rb0 = bodyA.m_originalBody; btRigidBody rb1 = bodyB.m_originalBody; // btVector3 rel_pos1 = pos1 - colObj0.getWorldTransform().getOrigin(); // btVector3 rel_pos2 = pos2 - colObj1.getWorldTransform().getOrigin(); //rel_pos1 = pos1 - bodyA.getWorldTransform().getOrigin(); //rel_pos2 = pos2 - bodyB.getWorldTransform().getOrigin(); relaxation = 1; btVector3 torqueAxis0; rel_pos1.cross( ref cp.m_normalWorldOnB, out torqueAxis0 ); btVector3 tmp; //solverConstraint.m_angularComponentA = rb0 != null ? rb0.m_invInertiaTensorWorld * torqueAxis0 * rb0.getAngularFactor() : btVector3.Zero; if( rb0 != null ) { rb0.m_invInertiaTensorWorld.Apply( ref torqueAxis0, out tmp ); tmp.Mult( ref rb0.m_angularFactor, out solverConstraint.m_angularComponentA ); } else solverConstraint.m_angularComponentA = btVector3.Zero; btVector3 torqueAxis1; rel_pos2.cross( ref cp.m_normalWorldOnB, out torqueAxis1 ); torqueAxis1.Invert( out torqueAxis1 ); //solverConstraint.m_angularComponentB = rb1 != null ? rb1.m_invInertiaTensorWorld * -torqueAxis1 * rb1.getAngularFactor() : btVector3.Zero; if( rb1 != null ) { rb1.m_invInertiaTensorWorld.Apply( ref torqueAxis1, out tmp ); tmp.Mult( ref rb1.m_angularFactor, out solverConstraint.m_angularComponentB ); } else solverConstraint.m_angularComponentB = btVector3.Zero; { #if COMPUTE_IMPULSE_DENOM double denom0 = rb0.computeImpulseDenominator(pos1,cp.m_normalWorldOnB); double denom1 = rb1.computeImpulseDenominator(pos2,cp.m_normalWorldOnB); #else btVector3 vec; double denom0 = 0; double denom1 = 0; if( rb0 != null ) { ( solverConstraint.m_angularComponentA ).cross( ref rel_pos1, out vec ); denom0 = rb0.getInvMass() + cp.m_normalWorldOnB.dot( vec ); } if( rb1 != null ) { solverConstraint.m_angularComponentB.Invert( out tmp ); tmp.cross( ref rel_pos2, out vec ); denom1 = rb1.getInvMass() + cp.m_normalWorldOnB.dot( vec ); } #endif //COMPUTE_IMPULSE_DENOM double denom = relaxation / ( denom0 + denom1 ); btScalar.Dbg( "m_jacDiagABInv 3 set to " + denom.ToString( "g17" ) ); solverConstraint.m_jacDiagABInv = denom; } if( rb0 != null ) { solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB; solverConstraint.m_relpos1CrossNormal = torqueAxis0; btScalar.Dbg( "Torque Axis to relpos1 " + torqueAxis0 ); } else { solverConstraint.m_contactNormal1 = btVector3.Zero; solverConstraint.m_relpos1CrossNormal = btVector3.Zero; } if( rb1 != null ) { cp.m_normalWorldOnB.Invert( out solverConstraint.m_contactNormal2 ); solverConstraint.m_relpos2CrossNormal = torqueAxis1; btScalar.Dbg( "Torque Axis to relpos2 " + torqueAxis1 ); } else { solverConstraint.m_contactNormal2 = btVector3.Zero; solverConstraint.m_relpos2CrossNormal = btVector3.Zero; } double restitution = 0; double penetration = cp.getDistance() + infoGlobal.m_linearSlop; { btVector3 vel1, vel2; vel1 = rb0 != null ? rb0.getVelocityInLocalPoint( ref rel_pos1 ) : btVector3.Zero; vel2 = rb1 != null ? rb1.getVelocityInLocalPoint( ref rel_pos2 ) : btVector3.Zero; // btVector3 vel2 = rb1 ? rb1.getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); btVector3 vel; vel1.Sub( ref vel2, out vel ); double rel_vel = cp.m_normalWorldOnB.dot( ref vel ); solverConstraint.m_friction = cp.m_combinedFriction; restitution = restitutionCurve( rel_vel, cp.m_combinedRestitution ); if( restitution <= btScalar.BT_ZERO ) { restitution = 0; }; } ///warm starting (or zero if disabled) if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_WARMSTARTING ) != 0 ) { solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; if( rb0 != null ) { solverConstraint.m_contactNormal1.Mult2( ref bodyA.m_invMass, ref rb0.m_linearFactor, out tmp ); bodyA.applyImpulse( ref tmp, ref solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse ); } if( rb1 != null ) { solverConstraint.m_contactNormal2.Mult2( ref rb1.m_linearFactor, ref bodyB.m_invMass, out tmp ); tmp.Invert( out tmp ); btVector3 tmp2; solverConstraint.m_angularComponentB.Invert( out tmp2 ); bodyB.applyImpulse( ref tmp, ref tmp2, -(double)solverConstraint.m_appliedImpulse ); } } else { solverConstraint.m_appliedImpulse = 0; } solverConstraint.m_appliedPushImpulse = 0; { btVector3 externalForceImpulseA = bodyA.m_originalBody != null ? bodyA.m_externalForceImpulse : btVector3.Zero; btVector3 externalTorqueImpulseA = bodyA.m_originalBody != null ? bodyA.m_externalTorqueImpulse : btVector3.Zero; btVector3 externalForceImpulseB = bodyB.m_originalBody != null ? bodyB.m_externalForceImpulse : btVector3.Zero; btVector3 externalTorqueImpulseB = bodyB.m_originalBody != null ? bodyB.m_externalTorqueImpulse : btVector3.Zero; btScalar.Dbg( "external torque impulses " + externalTorqueImpulseA + externalTorqueImpulseB ); double vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref bodyA.m_linearVelocity, ref externalForceImpulseA ) + solverConstraint.m_relpos1CrossNormal.dotAdded( ref bodyA.m_angularVelocity, ref externalTorqueImpulseA ); double vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref bodyB.m_linearVelocity, ref externalForceImpulseB ) + solverConstraint.m_relpos2CrossNormal.dotAdded( ref bodyB.m_angularVelocity, ref externalTorqueImpulseB ); double rel_vel = vel1Dotn + vel2Dotn; double positionalError = 0; double velocityError = restitution - rel_vel;// * damping; double erp = infoGlobal.m_erp2; if( !infoGlobal.m_splitImpulse || ( penetration > infoGlobal.m_splitImpulsePenetrationThreshold ) ) { erp = infoGlobal.m_erp; } if( penetration > 0 ) { positionalError = 0; velocityError -= penetration / infoGlobal.m_timeStep; } else { positionalError = -penetration * erp / infoGlobal.m_timeStep; } double penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; if( !infoGlobal.m_splitImpulse || ( penetration > infoGlobal.m_splitImpulsePenetrationThreshold ) ) { //combine position and velocity into rhs solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA.m_externalForce*bodyA.m_invMass-bodyB.m_externalForce/bodyB.m_invMass)*solverConstraint.m_jacDiagABInv; btScalar.Dbg( "Constraint 3 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) ); solverConstraint.m_rhsPenetration = 0; } else { //split position and velocity into rhs and m_rhsPenetration solverConstraint.m_rhs = velocityImpulse; btScalar.Dbg( "Constraint 4 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) ); solverConstraint.m_rhsPenetration = penetrationImpulse; } solverConstraint.m_cfm = 0; solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; } }
protected void resolveSplitPenetrationSIMD( btSolverBody body1, btSolverBody body2, btSolverConstraint c ) { resolveSplitPenetrationImpulseCacheFriendly( body1, body2, c ); }
internal void setupFrictionConstraint( btSolverConstraint solverConstraint, ref btVector3 normalAxis //, int solverBodyIdA, int solverBodyIdB , btSolverBody solverBodyA, btSolverBody solverBodyB , btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2, btCollisionObject colObj0, btCollisionObject colObj1, double relaxation, double desiredVelocity = 0, double cfmSlip = 0.0 ) { //btSolverBody solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; //btSolverBody solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; btRigidBody body0 = solverBodyA.m_originalBody; btRigidBody body1 = solverBodyB.m_originalBody; solverConstraint.m_solverBodyA = solverBodyA; solverConstraint.m_solverBodyB = solverBodyB; solverConstraint.m_friction = cp.m_combinedFriction; solverConstraint.m_originalContactPoint = null; solverConstraint.m_appliedImpulse = 0; solverConstraint.m_appliedPushImpulse = 0; if( body0 != null ) { solverConstraint.m_contactNormal1 = normalAxis; rel_pos1.cross( ref solverConstraint.m_contactNormal1, out solverConstraint.m_relpos1CrossNormal ); btVector3 tmp; body0.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out tmp ); tmp.Mult( ref body0.m_angularFactor, out solverConstraint.m_angularComponentA ); } else { solverConstraint.m_contactNormal1.setZero(); solverConstraint.m_relpos1CrossNormal.setZero(); solverConstraint.m_angularComponentA.setZero(); } if( body1 != null ) { normalAxis.Invert( out solverConstraint.m_contactNormal2 ); rel_pos2.cross( ref solverConstraint.m_contactNormal2, out solverConstraint.m_relpos2CrossNormal ); btVector3 tmp; body1.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out tmp ); tmp.Mult( ref body1.m_angularFactor, out solverConstraint.m_angularComponentB ); } else { solverConstraint.m_contactNormal2 = btVector3.Zero; solverConstraint.m_relpos2CrossNormal = btVector3.Zero; solverConstraint.m_angularComponentB = btVector3.Zero; } { btVector3 vec; double denom0 = 0; double denom1 = 0; if( body0 != null ) { solverConstraint.m_angularComponentA.cross( ref rel_pos1, out vec ); denom0 = body0.getInvMass() + normalAxis.dot( ref vec ); } if( body1 != null ) { btVector3 tmp; solverConstraint.m_angularComponentB.Invert( out tmp ); tmp.cross( ref rel_pos2, out vec ); denom1 = body1.getInvMass() + normalAxis.dot( ref vec ); } double denom = relaxation / ( denom0 + denom1 ); btScalar.Dbg( "m_jacDiagABInv 1 set to " + denom.ToString( "g17" ) ); solverConstraint.m_jacDiagABInv = denom; } { double rel_vel; double vel1Dotn; double vel2Dotn; //double vel1Dotn = solverConstraint.m_contactNormal1.dot( body0 != null ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3.Zero ) // + solverConstraint.m_relpos1CrossNormal.dot( body0 != null ? solverBodyA.m_angularVelocity : btVector3.Zero ); if( body0 != null ) vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyA.m_linearVelocity, ref solverBodyA.m_externalForceImpulse ) + solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyA.m_angularVelocity ); else vel1Dotn = 0; //double vel2Dotn = solverConstraint.m_contactNormal2.dot( body1 != null ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3.Zero ) // + solverConstraint.m_relpos2CrossNormal.dot( body1 != null ? solverBodyB.m_angularVelocity : btVector3.Zero ); if( body1 != null ) vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref solverBodyB.m_linearVelocity, ref solverBodyB.m_externalForceImpulse ) + solverConstraint.m_relpos2CrossNormal.dot( ref solverBodyB.m_angularVelocity ); else vel2Dotn = 0; rel_vel = vel1Dotn + vel2Dotn; // double positionalError = 0; double velocityError = desiredVelocity - rel_vel; double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; solverConstraint.m_rhs = velocityImpulse; btScalar.Dbg( "Constraint 1 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) ); solverConstraint.m_rhsPenetration = 0; solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; } }
protected void resolveSplitPenetrationImpulseCacheFriendly( btSolverBody body1, btSolverBody body2, btSolverConstraint c ) { if( c.m_rhsPenetration != 0 ) { gNumSplitImpulseRecoveries++; double deltaImpulse = c.m_rhsPenetration - (double)( c.m_appliedPushImpulse ) * c.m_cfm; btVector3 tmplin, tmpang; body1.internalGetPushVelocity( out tmplin ); body1.internalGetTurnVelocity( out tmpang ); double deltaVel1Dotn = c.m_contactNormal1.dot( ref tmplin ) + c.m_relpos1CrossNormal.dot( ref tmpang ); body2.internalGetPushVelocity( out tmplin ); body2.internalGetTurnVelocity( out tmpang ); double deltaVel2Dotn = c.m_contactNormal2.dot( ref tmplin ) + c.m_relpos2CrossNormal.dot( ref tmpang ); deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv; double sum = (double)( c.m_appliedPushImpulse ) + deltaImpulse; if( sum < c.m_lowerLimit ) { deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse; c.m_appliedPushImpulse = c.m_lowerLimit; } else { c.m_appliedPushImpulse = sum; } btVector3 mass, val; if( !c.m_contactNormal1.isZero() && !c.m_angularComponentA.isZero() ) { btScalar.Dbg( "push impulse setup from " + deltaImpulse.ToString( "g17" ) + " * " + c.m_contactNormal1.ToString() + " and " + c.m_angularComponentA.ToString() ); body1.internalGetInvMass( out mass ); mass.Mult( ref c.m_contactNormal1, out val ); body1.applyPushImpulse( ref val, ref c.m_angularComponentA, deltaImpulse ); } if( !c.m_contactNormal2.isZero() && !c.m_angularComponentB.isZero() ) { btScalar.Dbg( "push impulse setup from " + deltaImpulse.ToString( "g17" ) + " * " + c.m_contactNormal2.ToString() + " and " + c.m_angularComponentB.ToString() ); body2.internalGetInvMass( out mass ); mass.Mult( ref c.m_contactNormal2, out val ); body2.applyPushImpulse( ref val, ref c.m_angularComponentB, deltaImpulse ); } } }
protected double resolveSingleConstraintRowLowerLimit( btSolverBody body1, btSolverBody body2, btSolverConstraint c ) { return gResolveSingleConstraintRowLowerLimit_scalar_reference( body1, body2, c ); }
protected double resolveSingleConstraintRowLowerLimitSIMD( btSolverBody body1, btSolverBody body2, btSolverConstraint c ) { #if USE_SIMD return m_resolveSingleConstraintRowLowerLimit(body1, body2, c); #else return resolveSingleConstraintRowLowerLimit( body1, body2, c ); #endif }
// Project Gauss Seidel or the equivalent Sequential Impulse protected double resolveSingleConstraintRowGeneric( btSolverBody body1, btSolverBody body2, btSolverConstraint c ) { return gResolveSingleConstraintRowGeneric_scalar_reference( body1, body2, c ); }
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(btSolverConstraint obj) { return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr); }