//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::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()