void btSliderConstraint::getInfo1(btConstraintInfo1* info) { if (m_useSolveConstraintObsolete) { info->m_numConstraintRows = 0; info->nub = 0; } else { info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular info->nub = 2; //prepare constraint calculateTransforms(); testLinLimits(); if(getSolveLinLimit() || getPoweredLinMotor()) { info->m_numConstraintRows++; // limit 3rd linear as well info->nub--; } testAngLimits(); if(getSolveAngLimit() || getPoweredAngMotor()) { info->m_numConstraintRows++; // limit 3rd angular as well info->nub--; } } }
bool SliderJointSW::setup(float p_step) { //calculate transforms m_calculatedTransformA = A->get_transform() * m_frameInA; m_calculatedTransformB = B->get_transform() * m_frameInB; m_realPivotAInW = m_calculatedTransformA.origin; m_realPivotBInW = m_calculatedTransformB.origin; m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X m_delta = m_realPivotBInW - m_realPivotAInW; m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; m_relPosA = m_projPivotInW - A->get_transform().origin; m_relPosB = m_realPivotBInW - B->get_transform().origin; Vector3 normalWorld; int i; //linear part for(i = 0; i < 3; i++) { normalWorld = m_calculatedTransformA.basis.get_axis(i); memnew_placement(&m_jacLin[i], JacobianEntrySW( A->get_transform().basis.transposed(), B->get_transform().basis.transposed(), m_relPosA, m_relPosB, normalWorld, A->get_inv_inertia(), A->get_inv_mass(), B->get_inv_inertia(), B->get_inv_mass() )); m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal(); m_depth[i] = m_delta.dot(normalWorld); } testLinLimits(); // angular part for(i = 0; i < 3; i++) { normalWorld = m_calculatedTransformA.basis.get_axis(i); memnew_placement(&m_jacAng[i], JacobianEntrySW( normalWorld, A->get_transform().basis.transposed(), B->get_transform().basis.transposed(), A->get_inv_inertia(), B->get_inv_inertia() )); } testAngLimits(); Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); m_kAngle = real_t(1.0 )/ (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); // clear accumulator for motors m_accumulatedLinMotorImpulse = real_t(0.0); m_accumulatedAngMotorImpulse = real_t(0.0); return true; } // SliderJointSW::buildJacobianInt()
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); }