// constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2) : btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), m_anchor(anchor), m_axis1(axis1), m_axis2(axis2) { // build frame basis // 6DOF constraint uses Euler angles and to define limits // it is assumed that rotational order is : // Z - first, allowed limits are (-PI,PI); // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number // used to prevent constraint from instability on poles; // new position of X, allowed limits are (-PI,PI); // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs // Build the frame in world coordinate system first btVector3 zAxis = m_axis1.normalize(); btVector3 yAxis = m_axis2.normalize(); btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system btTransform frameInW; frameInW.setIdentity(); frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], xAxis[1], yAxis[1], zAxis[1], xAxis[2], yAxis[2], zAxis[2]); frameInW.setOrigin(anchor); // now get constraint frame in local coordinate systems m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW; // sei limits setLinearLowerLimit(btVector3(0., 0., 0.)); setLinearUpperLimit(btVector3(0., 0., 0.)); setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS)); setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS)); }
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false) { // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace btVector3 rbAxisA1, rbAxisA2; btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA; btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); //start with free m_lowerLimit = btScalar(1e30); m_upperLimit = btScalar(-1e30); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; }
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0),m_limit() { // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace btVector3 rbAxisA1, rbAxisA2; btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); }
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), #ifdef _BT_USE_CENTER_LIMIT_ m_limit(), #endif m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0), m_normalCFM(0), m_normalERP(0), m_stopCFM(0), m_stopERP(0) { m_rbAFrame.getOrigin() = pivotInA; // since no frame is given, assume this to be zero angle and just pick rb transform axis btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); btVector3 rbAxisA2; btScalar projection = axisInA.dot(rbAxisA1); if (projection >= 1.0f - SIMD_EPSILON) { rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); } else if (projection <= -1.0f + SIMD_EPSILON) { rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); } else { rbAxisA2 = axisInA.cross(rbAxisA1); rbAxisA1 = rbAxisA2.cross(axisInA); } m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = pivotInB; m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); #ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; #endif m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); }
//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 }
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA) :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), m_flags(0), m_useSolveConstraintObsolete(false) { }
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), m_angularOnly(false), m_enableAngularMotor(false) { m_rbAFrame.getOrigin() = pivotInA; // since no frame is given, assume this to be zero angle and just pick rb transform axis btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); btVector3 rbAxisA2; btScalar projection = axisInA.dot(rbAxisA1); if (projection >= 1.0f - SIMD_EPSILON) { rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); } else if (projection <= -1.0f + SIMD_EPSILON) { rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); } else { rbAxisA2 = axisInA.cross(rbAxisA1); rbAxisA1 = rbAxisA2.cross(axisInA); } m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = pivotInB; m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() ); //start with free m_lowerLimit = btScalar(1e30); m_upperLimit = btScalar(-1e30); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; }
btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder) : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB) , m_frameInB(frameInB) , m_rotateOrder(rotOrder) , m_flags(0) { ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; calculateTransforms(); }
btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameB), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0), m_useSolveConstraintObsolete(false) { ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; calculateTransforms(); }
btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA) : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB), m_useSolveConstraintObsolete(false), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA) { ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; // m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin()); initParams(); }
// constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2) : btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), m_anchor(anchor), m_axis1(axis1), m_axis2(axis2) { // build frame basis // 6DOF constraint uses Euler angles and to define limits // it is assumed that rotational order is : // Z - first, allowed limits are (-PI,PI); // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number // used to prevent constraint from instability on poles; // new position of X, allowed limits are (-PI,PI); // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs // Build the frame in world coordinate system first btVector3 zAxis = axis1.normalize(); btVector3 xAxis = axis2.normalize(); btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system btTransform frameInW; frameInW.setIdentity(); frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], xAxis[1], yAxis[1], zAxis[1], xAxis[2], yAxis[2], zAxis[2]); frameInW.setOrigin(anchor); // now get constraint frame in local coordinate systems m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW; // sei limits setLinearLowerLimit(btVector3(0.f, 0.f, -1.f)); setLinearUpperLimit(btVector3(0.f, 0.f, 1.f)); // like front wheels of a car setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f)); setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f)); // enable suspension enableSpring(2, true); setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-) setDamping(2, 0.01f); setEquilibriumPoint(); }
//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); } 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); }
void addPickingConstraint(const btVector3& rayFrom, const btVector3& rayTo) { if (!dynamicsWorld) { return; } removePickingConstraint(); if (pickedObjectIndex <= 0 || pickedObjectIndex >= dynamicsWorld->getNumCollisionObjects()) { return; } pickedBody = btRigidBody::upcast(dynamicsWorld->getCollisionObjectArray()[pickedObjectIndex]); btVector3 pickPos = rayTo; btVector3 localPivot = pickedBody->getCenterOfMassTransform().inverse() * pickPos; pickConstraint = new btPoint2PointConstraint(*pickedBody,localPivot); pickedBody->setActivationState(DISABLE_DEACTIVATION); dynamicsWorld->addConstraint(pickConstraint,true); pickingDistance = (rayFrom-rayTo).length(); pickConstraint->m_setting.m_impulseClamp = 3.0f; pickConstraint->m_setting.m_tau = 0.001f; }
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()