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 Generic6DOFJointSW::buildAngularJacobian( JacobianEntrySW & jacAngular,const Vector3 & jointAxisW) { memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW, A->get_transform().basis.transposed(), B->get_transform().basis.transposed(), A->get_inv_inertia(), B->get_inv_inertia())); }
void Generic6DOFJointSW::buildLinearJacobian( JacobianEntrySW & jacLinear,const Vector3 & normalWorld, const Vector3 & pivotAInW,const Vector3 & pivotBInW) { memnew_placement(&jacLinear, JacobianEntrySW( A->get_transform().basis.transposed(), B->get_transform().basis.transposed(), pivotAInW - A->get_transform().origin, pivotBInW - B->get_transform().origin, normalWorld, A->get_inv_inertia(), A->get_inv_mass(), B->get_inv_inertia(), B->get_inv_mass())); }
bool PinJointSW::setup(float p_step) { m_appliedImpulse = real_t(0.); Vector3 normal(0, 0, 0); for (int i = 0; i < 3; i++) { normal[i] = 1; memnew_placement(&m_jac[i], JacobianEntrySW( A->get_transform().basis.transposed(), B->get_transform().basis.transposed(), A->get_transform().xform(m_pivotInA) - A->get_transform().origin, B->get_transform().xform(m_pivotInB) - B->get_transform().origin, normal, A->get_inv_inertia(), A->get_inv_mass(), B->get_inv_inertia(), B->get_inv_mass())); normal[i] = 0; } return true; }
bool ConeTwistJointSW::setup(real_t p_timestep) { m_appliedImpulse = real_t(0.); //set bias, sign, clear accumulator m_swingCorrection = real_t(0.); m_twistLimitSign = real_t(0.); m_solveTwistLimit = false; m_solveSwingLimit = false; m_accTwistLimitImpulse = real_t(0.); m_accSwingLimitImpulse = real_t(0.); if (!m_angularOnly) { Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); Vector3 relPos = pivotBInW - pivotAInW; Vector3 normal[3]; if (relPos.length_squared() > CMP_EPSILON) { normal[0] = relPos.normalized(); } else { normal[0] = Vector3(real_t(1.0), 0, 0); } plane_space(normal[0], normal[1], normal[2]); for (int i = 0; i < 3; i++) { memnew_placement(&m_jac[i], JacobianEntrySW( A->get_principal_inertia_axes().transposed(), B->get_principal_inertia_axes().transposed(), pivotAInW - A->get_transform().origin - A->get_center_of_mass(), pivotBInW - B->get_transform().origin - B->get_center_of_mass(), normal[i], A->get_inv_inertia(), A->get_inv_mass(), B->get_inv_inertia(), B->get_inv_mass())); } } Vector3 b1Axis1, b1Axis2, b1Axis3; Vector3 b2Axis1, b2Axis2; b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(0)); b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(0)); real_t swing1 = real_t(0.), swing2 = real_t(0.); real_t swx = real_t(0.), swy = real_t(0.); real_t thresh = real_t(10.); real_t fact; // Get Frame into world space if (m_swingSpan1 >= real_t(0.05f)) { b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(1)); //swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis2); swing1 = atan2fast(swy, swx); fact = (swy * swy + swx * swx) * thresh * thresh; fact = fact / (fact + real_t(1.0)); swing1 *= fact; } if (m_swingSpan2 >= real_t(0.05f)) { b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(2)); //swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis3); swing2 = atan2fast(swy, swx); fact = (swy * swy + swx * swx) * thresh * thresh; fact = fact / (fact + real_t(1.0)); swing2 *= fact; } real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1); real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2); real_t EllipseAngle = Math::abs(swing1 * swing1) * RMaxAngle1Sq + Math::abs(swing2 * swing2) * RMaxAngle2Sq; if (EllipseAngle > 1.0f) { m_swingCorrection = EllipseAngle - 1.0f; m_solveSwingLimit = true; // Calculate necessary axis & factors m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3)); m_swingAxis.normalize(); real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; m_swingAxis *= swingAxisSign; m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) + B->compute_angular_impulse_denominator(m_swingAxis)); } // Twist limits if (m_twistSpan >= real_t(0.)) { Vector3 b2Axis2 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1)); Quat rotationArc = Quat(b2Axis1, b1Axis1); Vector3 TwistRef = rotationArc.xform(b2Axis2); real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2)); real_t lockedFreeFactor = (m_twistSpan > real_t(0.05f)) ? m_limitSoftness : real_t(0.); if (twist <= -m_twistSpan * lockedFreeFactor) { m_twistCorrection = -(twist + m_twistSpan); m_solveTwistLimit = true; m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis.normalize(); m_twistAxis *= -1.0f; m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + B->compute_angular_impulse_denominator(m_twistAxis)); } else if (twist > m_twistSpan * lockedFreeFactor) { m_twistCorrection = (twist - m_twistSpan); m_solveTwistLimit = true; m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis.normalize(); m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + B->compute_angular_impulse_denominator(m_twistAxis)); } } return true; }
bool HingeJointSW::setup(float p_step) { m_appliedImpulse = real_t(0.); if (!m_angularOnly) { Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); Vector3 relPos = pivotBInW - pivotAInW; Vector3 normal[3]; if (relPos.length_squared() > CMP_EPSILON) { normal[0] = relPos.normalized(); } else { normal[0] = Vector3(real_t(1.0), 0, 0); } plane_space(normal[0], normal[1], normal[2]); for (int i = 0; i < 3; i++) { memnew_placement(&m_jac[i], JacobianEntrySW( A->get_transform().basis.transposed(), B->get_transform().basis.transposed(), pivotAInW - A->get_transform().origin, pivotBInW - B->get_transform().origin, normal[i], A->get_inv_inertia(), A->get_inv_mass(), B->get_inv_inertia(), B->get_inv_mass())); } } //calculate two perpendicular jointAxis, orthogonal to hingeAxis //these two jointAxis require equal angular velocities for both bodies //this is unused for now, it's a todo Vector3 jointAxis0local; Vector3 jointAxis1local; plane_space(m_rbAFrame.basis.get_axis(2), jointAxis0local, jointAxis1local); A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); Vector3 jointAxis0 = A->get_transform().basis.xform(jointAxis0local); Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local); Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); memnew_placement(&m_jacAng[0], JacobianEntrySW(jointAxis0, A->get_transform().basis.transposed(), B->get_transform().basis.transposed(), A->get_inv_inertia(), B->get_inv_inertia())); memnew_placement(&m_jacAng[1], JacobianEntrySW(jointAxis1, A->get_transform().basis.transposed(), B->get_transform().basis.transposed(), A->get_inv_inertia(), B->get_inv_inertia())); memnew_placement(&m_jacAng[2], JacobianEntrySW(hingeAxisWorld, A->get_transform().basis.transposed(), B->get_transform().basis.transposed(), A->get_inv_inertia(), B->get_inv_inertia())); // Compute limit information real_t hingeAngle = get_hinge_angle(); // print_line("angle: "+rtos(hingeAngle)); //set bias, sign, clear accumulator m_correction = real_t(0.); m_limitSign = real_t(0.); m_solveLimit = false; m_accLimitImpulse = real_t(0.); /*if (m_useLimit) { print_line("low: "+rtos(m_lowerLimit)); print_line("hi: "+rtos(m_upperLimit)); }*/ // if (m_lowerLimit < m_upperLimit) if (m_useLimit && m_lowerLimit <= m_upperLimit) { // if (hingeAngle <= m_lowerLimit*m_limitSoftness) if (hingeAngle <= m_lowerLimit) { m_correction = (m_lowerLimit - hingeAngle); m_limitSign = 1.0f; m_solveLimit = true; } // else if (hingeAngle >= m_upperLimit*m_limitSoftness) else if (hingeAngle >= m_upperLimit) { m_correction = m_upperLimit - hingeAngle; m_limitSign = -1.0f; m_solveLimit = true; } } //Compute K = J*W*J' for hinge axis Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); return true; }