void btHingeConstraint::solveConstraint(btScalar timeStep) { btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); btScalar tau = btScalar(0.3); //linear part if (!m_angularOnly) { btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); btVector3 vel = vel1 - vel2; for (int i=0;i<3;i++) { const btVector3& normal = m_jac[i].m_linearJointAxis; btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); btScalar rel_vel; rel_vel = normal.dot(vel); //positional error (zeroth order error) btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; btVector3 impulse_vector = normal * impulse; m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); } } { ///solve angular part // get axes in world space btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; btVector3 velrelOrthog = angAorthog-angBorthog; { //solve orthogonal angular velocity correction btScalar relaxation = btScalar(1.); btScalar len = velrelOrthog.length(); if (len > btScalar(0.00001)) { btVector3 normal = velrelOrthog.normalized(); btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + getRigidBodyB().computeAngularImpulseDenominator(normal); // scale for mass and relaxation velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; } //solve angular positional correction btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep); btScalar len2 = angularError.length(); if (len2>btScalar(0.00001)) { btVector3 normal2 = angularError.normalized(); btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + getRigidBodyB().computeAngularImpulseDenominator(normal2); angularError *= (btScalar(1.)/denom2) * relaxation; } m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); m_rbB.applyTorqueImpulse(velrelOrthog-angularError); // solve limit if (m_solveLimit) { btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; btScalar impulseMag = amplitude * m_kHinge; // Clamp the accumulated impulse btScalar temp = m_accLimitImpulse; m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); impulseMag = m_accLimitImpulse - temp; btVector3 impulse = axisA * impulseMag * m_limitSign; m_rbA.applyTorqueImpulse(impulse); m_rbB.applyTorqueImpulse(-impulse); } } //apply motor if (m_enableAngularMotor) { //todo: add limits too btVector3 angularLimit(0,0,0); btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; btScalar projRelVel = velrel.dot(axisA); btScalar desiredMotorVel = m_motorTargetVelocity; btScalar motor_relvel = desiredMotorVel - projRelVel; btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; //todo: should clip against accumulated impulse btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; btVector3 motorImp = clippedMotorImpulse * axisA; m_rbA.applyTorqueImpulse(motorImp+angularLimit); m_rbB.applyTorqueImpulse(-motorImp-angularLimit); } } }
void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { ///for backwards compatibility during the transition to 'getInfo/getInfo2' if (m_useSolveConstraintObsolete) { btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); btScalar tau = btScalar(0.3); //linear part if (!m_angularOnly) { btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); btVector3 vel1,vel2; bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); btVector3 vel = vel1 - vel2; for (int i=0;i<3;i++) { const btVector3& normal = m_jac[i].m_linearJointAxis; btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); btScalar rel_vel; rel_vel = normal.dot(vel); //positional error (zeroth order error) btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; btVector3 impulse_vector = normal * impulse; btVector3 ftorqueAxis1 = rel_pos1.cross(normal); btVector3 ftorqueAxis2 = rel_pos2.cross(normal); bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); } } { ///solve angular part // get axes in world space btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); btVector3 angVelA; bodyA.getAngularVelocity(angVelA); btVector3 angVelB; bodyB.getAngularVelocity(angVelB); btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; btVector3 velrelOrthog = angAorthog-angBorthog; { //solve orthogonal angular velocity correction btScalar len = velrelOrthog.length(); if (len > btScalar(0.00001)) { btVector3 normal = velrelOrthog.normalized(); btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + getRigidBodyB().computeAngularImpulseDenominator(normal); // scale for mass and relaxation //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom)); bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom)); } //solve angular positional correction btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep); btScalar len2 = angularError.length(); if (len2>btScalar(0.00001)) { btVector3 normal2 = angularError.normalized(); btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + getRigidBodyB().computeAngularImpulseDenominator(normal2); //angularError *= (btScalar(1.)/denom2) * relaxation; bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2)); bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2)); } // solve limit if (m_solveLimit) { btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; btScalar impulseMag = amplitude * m_kHinge; // Clamp the accumulated impulse btScalar temp = m_accLimitImpulse; m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); impulseMag = m_accLimitImpulse - temp; bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign); bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign)); } } //apply motor if (m_enableAngularMotor) { //todo: add limits too btVector3 angularLimit(0,0,0); btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; btScalar projRelVel = velrel.dot(axisA); btScalar desiredMotorVel = m_motorTargetVelocity; btScalar motor_relvel = desiredMotorVel - projRelVel; btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; // accumulated impulse clipping: btScalar fMaxImpulse = m_maxMotorImpulse; btScalar newAccImpulse = m_accMotorImpulse + unclippedMotorImpulse; btScalar clippedMotorImpulse = unclippedMotorImpulse; if (newAccImpulse > fMaxImpulse) { newAccImpulse = fMaxImpulse; clippedMotorImpulse = newAccImpulse - m_accMotorImpulse; } else if (newAccImpulse < -fMaxImpulse) { newAccImpulse = -fMaxImpulse; clippedMotorImpulse = newAccImpulse - m_accMotorImpulse; } m_accMotorImpulse += clippedMotorImpulse; bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse); bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse); } } } }
void HingeJointSW::solve(float p_step) { Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); //real_t tau = real_t(0.3); //linear part if (!m_angularOnly) { Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); Vector3 vel = vel1 - vel2; for (int i = 0; i < 3; i++) { const Vector3 &normal = m_jac[i].m_linearJointAxis; real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); real_t rel_vel; rel_vel = normal.dot(vel); //positional error (zeroth order error) real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); } } { ///solve angular part // get axes in world space Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(2)); const Vector3 &angVelA = A->get_angular_velocity(); const Vector3 &angVelB = B->get_angular_velocity(); Vector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); Vector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); Vector3 angAorthog = angVelA - angVelAroundHingeAxisA; Vector3 angBorthog = angVelB - angVelAroundHingeAxisB; Vector3 velrelOrthog = angAorthog - angBorthog; { //solve orthogonal angular velocity correction real_t relaxation = real_t(1.); real_t len = velrelOrthog.length(); if (len > real_t(0.00001)) { Vector3 normal = velrelOrthog.normalized(); real_t denom = A->compute_angular_impulse_denominator(normal) + B->compute_angular_impulse_denominator(normal); // scale for mass and relaxation velrelOrthog *= (real_t(1.) / denom) * m_relaxationFactor; } //solve angular positional correction Vector3 angularError = -axisA.cross(axisB) * (real_t(1.) / p_step); real_t len2 = angularError.length(); if (len2 > real_t(0.00001)) { Vector3 normal2 = angularError.normalized(); real_t denom2 = A->compute_angular_impulse_denominator(normal2) + B->compute_angular_impulse_denominator(normal2); angularError *= (real_t(1.) / denom2) * relaxation; } A->apply_torque_impulse(-velrelOrthog + angularError); B->apply_torque_impulse(velrelOrthog - angularError); // solve limit if (m_solveLimit) { real_t amplitude = ((angVelB - angVelA).dot(axisA) * m_relaxationFactor + m_correction * (real_t(1.) / p_step) * m_biasFactor) * m_limitSign; real_t impulseMag = amplitude * m_kHinge; // Clamp the accumulated impulse real_t temp = m_accLimitImpulse; m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0)); impulseMag = m_accLimitImpulse - temp; Vector3 impulse = axisA * impulseMag * m_limitSign; A->apply_torque_impulse(impulse); B->apply_torque_impulse(-impulse); } } //apply motor if (m_enableAngularMotor) { //todo: add limits too Vector3 angularLimit(0, 0, 0); Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; real_t projRelVel = velrel.dot(axisA); real_t desiredMotorVel = m_motorTargetVelocity; real_t motor_relvel = desiredMotorVel - projRelVel; real_t unclippedMotorImpulse = m_kHinge * motor_relvel; //todo: should clip against accumulated impulse real_t clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; Vector3 motorImp = clippedMotorImpulse * axisA; A->apply_torque_impulse(motorImp + angularLimit); B->apply_torque_impulse(-motorImp - angularLimit); } } }