//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 btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) { //calculate transforms m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA; m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB; m_realPivotAInW = m_calculatedTransformA.getOrigin(); m_realPivotBInW = m_calculatedTransformB.getOrigin(); m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X m_delta = m_realPivotBInW - m_realPivotAInW; m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition(); m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition(); btVector3 normalWorld; int i; //linear part for(i = 0; i < 3; i++) { normalWorld = m_calculatedTransformA.getBasis().getColumn(i); new (&m_jacLin[i]) btJacobianEntry( rbA.getCenterOfMassTransform().getBasis().transpose(), rbB.getCenterOfMassTransform().getBasis().transpose(), m_relPosA, m_relPosB, normalWorld, rbA.getInvInertiaDiagLocal(), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(), rbB.getInvMass() ); m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal(); m_depth[i] = m_delta.dot(normalWorld); } testLinLimits(); // angular part for(i = 0; i < 3; i++) { normalWorld = m_calculatedTransformA.getBasis().getColumn(i); new (&m_jacAng[i]) btJacobianEntry( normalWorld, rbA.getCenterOfMassTransform().getBasis().transpose(), rbB.getCenterOfMassTransform().getBasis().transpose(), rbA.getInvInertiaDiagLocal(), rbB.getInvInertiaDiagLocal() ); } testAngLimits(); btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA)); // clear accumulator for motors m_accumulatedLinMotorImpulse = btScalar(0.0); m_accumulatedAngMotorImpulse = btScalar(0.0); }
//bilateral constraint between two dynamic objects void RaycastCar::resolveSingleBilateral(btRigidBody & body1, const btVector3 & pos1, btRigidBody & body2, const btVector3 & pos2, const btVector3 & normal, btScalar & impulse) { btScalar normalLenSqr = normal.length2(); btAssert(btFabs(normalLenSqr) < btScalar(1.1f)); if (normalLenSqr > btScalar(1.1f)) { impulse = btScalar(0.0f); return; } btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); 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.0f) / jacDiagAB; btScalar rel_vel = jac.getRelativeVelocity (body1.getLinearVelocity(), body1.getCenterOfMassTransform().getBasis().transpose()*body1.getAngularVelocity(), body2.getLinearVelocity(), body2.getCenterOfMassTransform().getBasis().transpose()*body2.getAngularVelocity()); btScalar velocityImpulse = -1.0f * rel_vel * jacDiagABInv; impulse = velocityImpulse; }
void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB) { int i; // linear btVector3 velA; bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA); btVector3 velB; bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB); 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); { btVector3 ftorqueAxis1 = m_relPosA.cross(normal); btVector3 ftorqueAxis2 = m_relPosB.cross(normal); bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); } 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); { btVector3 ftorqueAxis1 = m_relPosA.cross(normal); btVector3 ftorqueAxis2 = m_relPosB.cross(normal); bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); } } } } // angular // get axes in world space btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); btVector3 angVelA; bodyA.getAngularVelocity(angVelA); btVector3 angVelB; bodyB.getAngularVelocity(angVelB); 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(); btScalar orthorImpulseMag = 0.f; 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; orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; } //solve angular positional correction btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); btVector3 angularAxis = angularError; btScalar angularImpulseMag = 0; btScalar len2 = angularError.length(); if (len2>btScalar(0.00001)) { btVector3 normal2 = angularError.normalized(); btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; angularError *= angularImpulseMag; } // apply impulse //rbA.applyTorqueImpulse(-velrelOrthog+angularError); //rbB.applyTorqueImpulse(velrelOrthog-angularError); bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag); bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag); bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag); bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag); 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); bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag); bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag); //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; //rbA.applyTorqueImpulse(motorImp); //rbB.applyTorqueImpulse(-motorImp); bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse); bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse); } } }
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.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); btVector3 vel2; body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); 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); btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a); btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a); body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); return normalImpulse; }
void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) { //calculate transforms m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA; m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB; m_realPivotAInW = m_calculatedTransformA.getOrigin(); m_realPivotBInW = m_calculatedTransformB.getOrigin(); m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X m_delta = m_realPivotBInW - m_realPivotAInW; m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition(); m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition(); btVector3 normalWorld; int i; //linear part for(i = 0; i < 3; i++) { normalWorld = m_calculatedTransformA.getBasis().getColumn(i); new (&m_jacLin[i]) btJacobianEntry( rbA.getCenterOfMassTransform().getBasis().transpose(), rbB.getCenterOfMassTransform().getBasis().transpose(), m_relPosA, m_relPosB, normalWorld, rbA.getInvInertiaDiagLocal(), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(), rbB.getInvMass() ); m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal(); m_depth[i] = m_delta.dot(normalWorld); } m_solveLinLim = false; if(m_lowerLinLimit <= m_upperLinLimit) { if(m_depth[0] > m_upperLinLimit) { m_depth[0] -= m_upperLinLimit; m_solveLinLim = true; } else if(m_depth[0] < m_lowerLinLimit) { m_depth[0] -= m_lowerLinLimit; m_solveLinLim = true; } else { m_depth[0] = btScalar(0.); } } else { m_depth[0] = btScalar(0.); } // angular part for(i = 0; i < 3; i++) { normalWorld = m_calculatedTransformA.getBasis().getColumn(i); new (&m_jacAng[i]) btJacobianEntry( normalWorld, rbA.getCenterOfMassTransform().getBasis().transpose(), rbB.getCenterOfMassTransform().getBasis().transpose(), rbA.getInvInertiaDiagLocal(), rbB.getInvInertiaDiagLocal() ); } m_angDepth = btScalar(0.); m_solveAngLim = false; if(m_lowerAngLimit <= m_upperAngLimit) { const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1); const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2); const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1); btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); if(rot < m_lowerAngLimit) { m_angDepth = rot - m_lowerAngLimit; m_solveAngLim = true; } else if(rot > m_upperAngLimit) { m_angDepth = rot - m_upperAngLimit; m_solveAngLim = true; } } btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA)); } // btSliderConstraint::buildJacobianInt()