//bilateral constraint between two dynamic objects void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, btRigidBody& body2, const btVector3& pos2, btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep) { (void)timeStep; (void)distance; btScalar normalLenSqr = normal.length2(); btAssert(btFabs(normalLenSqr) < btScalar(1.1)); if (normalLenSqr > btScalar(1.1)) { impulse = btScalar(0.); return; } btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); //this jacobian entry could be re-used for all iterations btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); btVector3 vel = vel1 - vel2; btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), body2.getCenterOfMassTransform().getBasis().transpose(), rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), body2.getInvInertiaDiagLocal(),body2.getInvMass()); btScalar jacDiagAB = jac.getDiagonal(); btScalar jacDiagABInv = btScalar(1.) / jacDiagAB; btScalar rel_vel = jac.getRelativeVelocity( body1.getLinearVelocity(), body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(), body2.getLinearVelocity(), body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); btScalar a; a=jacDiagABInv; rel_vel = normal.dot(vel); //todo: move this into proper structure btScalar contactDamping = btScalar(0.2); #ifdef ONLY_USE_LINEAR_MASS btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass()); impulse = - contactDamping * rel_vel * massTerm; #else btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; #endif }
void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo) { (void)raycastInfo; if (m_raycastInfo.m_isInContact) { btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); btVector3 chassis_velocity_at_contactPoint; btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); if ( project >= btScalar(-0.1)) { m_suspensionRelativeVelocity = btScalar(0.0); m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); } else { btScalar inv = btScalar(-1.) / project; m_suspensionRelativeVelocity = projVel * inv; m_clippedInvContactDotSuspension = inv; } } else // Not in contact : position wheel in a nice (rest length) position { m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); m_suspensionRelativeVelocity = btScalar(0.0); m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; m_clippedInvContactDotSuspension = btScalar(1.0); } }
btScalar btTranslationalLimitMotor::solveLinearAxis( btScalar timeStep, btScalar jacDiagABInv, btRigidBody& body1,const btVector3 &pointInA, btRigidBody& body2,const btVector3 &pointInB, int limit_index, const btVector3 & axis_normal_on_a, const btVector3 & anchorPos) { ///find relative velocity // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); btVector3 vel = vel1 - vel2; btScalar rel_vel = axis_normal_on_a.dot(vel); /// apply displacement correction //positional error (zeroth order error) btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); btScalar lo = btScalar(-BT_LARGE_FLOAT); btScalar hi = btScalar(BT_LARGE_FLOAT); btScalar minLimit = m_lowerLimit[limit_index]; btScalar maxLimit = m_upperLimit[limit_index]; //handle the limits if (minLimit < maxLimit) { { if (depth > maxLimit) { depth -= maxLimit; lo = btScalar(0.); } else { if (depth < minLimit) { depth -= minLimit; hi = btScalar(0.); } else { return 0.0f; } } } } btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; btScalar sum = oldNormalImpulse + normalImpulse; m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; btVector3 impulse_vector = axis_normal_on_a * normalImpulse; body1.applyImpulse( impulse_vector, rel_pos1); body2.applyImpulse(-impulse_vector, rel_pos2); return normalImpulse; }
void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) { int i; // linear btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA); btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB); btVector3 vel = velA - velB; for(i = 0; i < 3; i++) { const btVector3& normal = m_jacLin[i].m_linearJointAxis; btScalar rel_vel = normal.dot(vel); // calculate positional error btScalar depth = m_depth[i]; // get parameters btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); // calcutate and apply impulse btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; btVector3 impulse_vector = normal * normalImpulse; rbA.applyImpulse( impulse_vector, m_relPosA); rbB.applyImpulse(-impulse_vector, m_relPosB); if(m_poweredLinMotor && (!i)) { // apply linear motor if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) { btScalar desiredMotorVel = m_targetLinMotorVelocity; btScalar motor_relvel = desiredMotorVel + rel_vel; normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; // clamp accumulated impulse btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse); if(new_acc > m_maxLinMotorForce) { new_acc = m_maxLinMotorForce; } btScalar del = new_acc - m_accumulatedLinMotorImpulse; if(normalImpulse < btScalar(0.0)) { normalImpulse = -del; } else { normalImpulse = del; } m_accumulatedLinMotorImpulse = new_acc; // apply clamped impulse impulse_vector = normal * normalImpulse; rbA.applyImpulse( impulse_vector, m_relPosA); rbB.applyImpulse(-impulse_vector, m_relPosB); } } } // angular // get axes in world space btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); const btVector3& angVelA = rbA.getAngularVelocity(); const btVector3& angVelB = rbB.getAngularVelocity(); btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); btVector3 angAorthog = angVelA - angVelAroundAxisA; btVector3 angBorthog = angVelB - angVelAroundAxisB; btVector3 velrelOrthog = angAorthog-angBorthog; //solve orthogonal angular velocity correction btScalar len = velrelOrthog.length(); if (len > btScalar(0.00001)) { btVector3 normal = velrelOrthog.normalized(); btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; } //solve angular positional correction btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); btScalar len2 = angularError.length(); if (len2>btScalar(0.00001)) { btVector3 normal2 = angularError.normalized(); btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; } // apply impulse rbA.applyTorqueImpulse(-velrelOrthog+angularError); rbB.applyTorqueImpulse(velrelOrthog-angularError); btScalar impulseMag; //solve angular limits if(m_solveAngLim) { impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep; impulseMag *= m_kAngle * m_softnessLimAng; } else { impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep; impulseMag *= m_kAngle * m_softnessDirAng; } btVector3 impulse = axisA * impulseMag; rbA.applyTorqueImpulse(impulse); rbB.applyTorqueImpulse(-impulse); //apply angular motor if(m_poweredAngMotor) { if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce) { btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB; btScalar projRelVel = velrel.dot(axisA); btScalar desiredMotorVel = m_targetAngMotorVelocity; btScalar motor_relvel = desiredMotorVel - projRelVel; btScalar angImpulse = m_kAngle * motor_relvel; // clamp accumulated impulse btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse); if(new_acc > m_maxAngMotorForce) { new_acc = m_maxAngMotorForce; } btScalar del = new_acc - m_accumulatedAngMotorImpulse; if(angImpulse < btScalar(0.0)) { angImpulse = -del; } else { angImpulse = del; } m_accumulatedAngMotorImpulse = new_acc; // apply clamped impulse btVector3 motorImp = angImpulse * axisA; m_rbA.applyTorqueImpulse(motorImp); m_rbB.applyTorqueImpulse(-motorImp); } } } // btSliderConstraint::solveConstraint()