/*************************************************************************/ /* generic_6dof_joint_sw.cpp */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ /* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ /* "Software"), to deal in the Software without restriction, including */ /* without limitation the rights to use, copy, modify, merge, publish, */ /* distribute, sublicense, and/or sell copies of the Software, and to */ /* permit persons to whom the Software is furnished to do so, subject to */ /* the following conditions: */ /* */ /* The above copyright notice and this permission notice shall be */ /* included in all copies or substantial portions of the Software. */ /* */ /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ /* Adapted to Godot from the Bullet library. See corresponding header file for licensing info. */ #include "generic_6dof_joint_sw.h" #define GENERIC_D6_DISABLE_WARMSTARTING 1 real_t btGetMatrixElem(const Matrix3 &mat, int index); real_t btGetMatrixElem(const Matrix3 &mat, int index) { int i = index % 3; int j = index / 3; return mat[i][j]; } ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html bool matrixToEulerXYZ(const Matrix3 &mat, Vector3 &xyz); bool matrixToEulerXYZ(const Matrix3 &mat, Vector3 &xyz) { // // rot = cy*cz -cy*sz sy // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy // if (btGetMatrixElem(mat, 2) < real_t(1.0)) { if (btGetMatrixElem(mat, 2) > real_t(-1.0)) { xyz[0] = Math::atan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8)); xyz[1] = Math::asin(btGetMatrixElem(mat, 2)); xyz[2] = Math::atan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0)); return true; } else { // WARNING. Not unique. XA - ZA = -atan2(r10,r11) xyz[0] = -Math::atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4)); xyz[1] = -Math_PI * 0.5; xyz[2] = real_t(0.0); return false; } } else { // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) xyz[0] = Math::atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4)); xyz[1] = Math_PI * 0.5; xyz[2] = 0.0; } return false; } //////////////////////////// G6DOFRotationalLimitMotorSW //////////////////////////////////// int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value) { if (m_loLimit > m_hiLimit) { m_currentLimit = 0; //Free from violation return 0; } if (test_value < m_loLimit) { m_currentLimit = 1; //low limit violation m_currentLimitError = test_value - m_loLimit; return 1; } else if (test_value > m_hiLimit) { m_currentLimit = 2; //High limit violation m_currentLimitError = test_value - m_hiLimit; return 2; }; m_currentLimit = 0; //Free from violation return 0; } real_t G6DOFRotationalLimitMotorSW::solveAngularLimits( real_t timeStep, Vector3 &axis, real_t jacDiagABInv, BodySW *body0, BodySW *body1) { if (needApplyTorques() == false) return 0.0f; real_t target_velocity = m_targetVelocity; real_t maxMotorForce = m_maxMotorForce; //current error correction if (m_currentLimit != 0) { target_velocity = -m_ERP * m_currentLimitError / (timeStep); maxMotorForce = m_maxLimitForce; } maxMotorForce *= timeStep; // current velocity difference Vector3 vel_diff = body0->get_angular_velocity(); if (body1) { vel_diff -= body1->get_angular_velocity(); } real_t rel_vel = axis.dot(vel_diff); // correction velocity real_t motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel); if (motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON) { return 0.0f; //no need for applying force } // correction impulse real_t unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv; // clip correction impulse real_t clippedMotorImpulse; ///@todo: should clip against accumulated impulse if (unclippedMotorImpulse > 0.0f) { clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse; } else { clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse; } // sort with accumulated impulses real_t lo = real_t(-1e30); real_t hi = real_t(1e30); real_t oldaccumImpulse = m_accumulatedImpulse; real_t sum = oldaccumImpulse + clippedMotorImpulse; m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; Vector3 motorImp = clippedMotorImpulse * axis; body0->apply_torque_impulse(motorImp); if (body1) body1->apply_torque_impulse(-motorImp); return clippedMotorImpulse; } //////////////////////////// End G6DOFRotationalLimitMotorSW //////////////////////////////////// //////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// real_t G6DOFTranslationalLimitMotorSW::solveLinearAxis( real_t timeStep, real_t jacDiagABInv, BodySW *body1, const Vector3 &pointInA, BodySW *body2, const Vector3 &pointInB, int limit_index, const Vector3 &axis_normal_on_a, const Vector3 &anchorPos) { ///find relative velocity // Vector3 rel_pos1 = pointInA - body1->get_transform().origin; // Vector3 rel_pos2 = pointInB - body2->get_transform().origin; Vector3 rel_pos1 = anchorPos - body1->get_transform().origin; Vector3 rel_pos2 = anchorPos - body2->get_transform().origin; Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1); Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2); Vector3 vel = vel1 - vel2; real_t rel_vel = axis_normal_on_a.dot(vel); /// apply displacement correction //positional error (zeroth order error) real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a); real_t lo = real_t(-1e30); real_t hi = real_t(1e30); real_t minLimit = m_lowerLimit[limit_index]; real_t maxLimit = m_upperLimit[limit_index]; //handle the limits if (minLimit < maxLimit) { { if (depth > maxLimit) { depth -= maxLimit; lo = real_t(0.); } else { if (depth < minLimit) { depth -= minLimit; hi = real_t(0.); } else { return 0.0f; } } } } real_t normalImpulse = m_limitSoftness[limit_index] * (m_restitution[limit_index] * depth / timeStep - m_damping[limit_index] * rel_vel) * jacDiagABInv; real_t oldNormalImpulse = m_accumulatedImpulse[limit_index]; real_t sum = oldNormalImpulse + normalImpulse; m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; Vector3 impulse_vector = axis_normal_on_a * normalImpulse; body1->apply_impulse(rel_pos1, impulse_vector); body2->apply_impulse(rel_pos2, -impulse_vector); return normalImpulse; } //////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// Generic6DOFJointSW::Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA) : JointSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA) { A = rbA; B = rbB; A->add_constraint(this, 0); B->add_constraint(this, 1); } void Generic6DOFJointSW::calculateAngleInfo() { Matrix3 relative_frame = m_calculatedTransformA.basis.inverse() * m_calculatedTransformB.basis; matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff); // in euler angle mode we do not actually constrain the angular velocity // along the axes axis[0] and axis[2] (although we do use axis[1]) : // // to get constrain w2-w1 along ...not // ------ --------------------- ------ // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] // d(angle[1])/dt = 0 ax[1] // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] // // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. // to prove the result for angle[0], write the expression for angle[0] from // GetInfo1 then take the derivative. to prove this for angle[2] it is // easier to take the euler rate expression for d(angle[2])/dt with respect // to the components of w and set that to 0. Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0); Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2); m_calculatedAxis[1] = axis2.cross(axis0); m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); // if(m_debugDrawer) // { // // char buff[300]; // sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", // m_calculatedAxisAngleDiff[0], // m_calculatedAxisAngleDiff[1], // m_calculatedAxisAngleDiff[2]); // m_debugDrawer->reportErrorWarning(buff); // } } void Generic6DOFJointSW::calculateTransforms() { m_calculatedTransformA = A->get_transform() * m_frameInA; m_calculatedTransformB = B->get_transform() * m_frameInB; calculateAngleInfo(); } 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())); } 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())); } bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index) { real_t angle = m_calculatedAxisAngleDiff[axis_index]; //test limits m_angularLimits[axis_index].testLimitValue(angle); return m_angularLimits[axis_index].needApplyTorques(); } bool Generic6DOFJointSW::setup(float p_step) { // Clear accumulated impulses for the next simulation step m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.)); int i; for (i = 0; i < 3; i++) { m_angularLimits[i].m_accumulatedImpulse = real_t(0.); } //calculates transform calculateTransforms(); // const Vector3& pivotAInW = m_calculatedTransformA.origin; // const Vector3& pivotBInW = m_calculatedTransformB.origin; calcAnchorPos(); Vector3 pivotAInW = m_AnchorPos; Vector3 pivotBInW = m_AnchorPos; // not used here // Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; // Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; Vector3 normalWorld; //linear part for (i = 0; i < 3; i++) { if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { if (m_useLinearReferenceFrameA) normalWorld = m_calculatedTransformA.basis.get_axis(i); else normalWorld = m_calculatedTransformB.basis.get_axis(i); buildLinearJacobian( m_jacLinear[i], normalWorld, pivotAInW, pivotBInW); } } // angular part for (i = 0; i < 3; i++) { //calculates error angle if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) { normalWorld = this->getAxis(i); // Create angular atom buildAngularJacobian(m_jacAng[i], normalWorld); } } return true; } void Generic6DOFJointSW::solve(real_t timeStep) { m_timeStep = timeStep; //calculateTransforms(); int i; // linear Vector3 pointInA = m_calculatedTransformA.origin; Vector3 pointInB = m_calculatedTransformB.origin; real_t jacDiagABInv; Vector3 linear_axis; for (i = 0; i < 3; i++) { if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal(); if (m_useLinearReferenceFrameA) linear_axis = m_calculatedTransformA.basis.get_axis(i); else linear_axis = m_calculatedTransformB.basis.get_axis(i); m_linearLimits.solveLinearAxis( m_timeStep, jacDiagABInv, A, pointInA, B, pointInB, i, linear_axis, m_AnchorPos); } } // angular Vector3 angular_axis; real_t angularJacDiagABInv; for (i = 0; i < 3; i++) { if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) { // get axis angular_axis = getAxis(i); angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal(); m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B); } } } void Generic6DOFJointSW::updateRHS(real_t timeStep) { (void)timeStep; } Vector3 Generic6DOFJointSW::getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; } real_t Generic6DOFJointSW::getAngle(int axis_index) const { return m_calculatedAxisAngleDiff[axis_index]; } void Generic6DOFJointSW::calcAnchorPos(void) { real_t imA = A->get_inv_mass(); real_t imB = B->get_inv_mass(); real_t weight; if (imB == real_t(0.0)) { weight = real_t(1.0); } else { weight = imA / (imA + imB); } const Vector3 &pA = m_calculatedTransformA.origin; const Vector3 &pB = m_calculatedTransformB.origin; m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight); return; } // Generic6DOFJointSW::calcAnchorPos() void Generic6DOFJointSW::set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, float p_value) { ERR_FAIL_INDEX(p_axis, 3); switch (p_param) { case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { m_linearLimits.m_lowerLimit[p_axis] = p_value; } break; case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { m_linearLimits.m_upperLimit[p_axis] = p_value; } break; case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { m_linearLimits.m_limitSoftness[p_axis] = p_value; } break; case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: { m_linearLimits.m_restitution[p_axis] = p_value; } break; case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: { m_linearLimits.m_damping[p_axis] = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { m_angularLimits[p_axis].m_loLimit = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { m_angularLimits[p_axis].m_hiLimit = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { m_angularLimits[p_axis].m_limitSoftness = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: { m_angularLimits[p_axis].m_damping = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: { m_angularLimits[p_axis].m_bounce = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { m_angularLimits[p_axis].m_maxLimitForce = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: { m_angularLimits[p_axis].m_ERP = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { m_angularLimits[p_axis].m_targetVelocity = p_value; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { m_angularLimits[p_axis].m_maxLimitForce = p_value; } break; } } float Generic6DOFJointSW::get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const { ERR_FAIL_INDEX_V(p_axis, 3, 0); switch (p_param) { case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { return m_linearLimits.m_lowerLimit[p_axis]; } break; case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { return m_linearLimits.m_upperLimit[p_axis]; } break; case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { return m_linearLimits.m_limitSoftness[p_axis]; } break; case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: { return m_linearLimits.m_restitution[p_axis]; } break; case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: { return m_linearLimits.m_damping[p_axis]; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { return m_angularLimits[p_axis].m_loLimit; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { return m_angularLimits[p_axis].m_hiLimit; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { return m_angularLimits[p_axis].m_limitSoftness; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: { return m_angularLimits[p_axis].m_damping; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: { return m_angularLimits[p_axis].m_bounce; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { return m_angularLimits[p_axis].m_maxLimitForce; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: { return m_angularLimits[p_axis].m_ERP; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { return m_angularLimits[p_axis].m_targetVelocity; } break; case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { return m_angularLimits[p_axis].m_maxMotorForce; } break; } return 0; } void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value) { ERR_FAIL_INDEX(p_axis, 3); switch (p_flag) { case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { m_linearLimits.enable_limit[p_axis] = p_value; } break; case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { m_angularLimits[p_axis].m_enableLimit = p_value; } break; case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { m_angularLimits[p_axis].m_enableMotor = p_value; } break; } } bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const { ERR_FAIL_INDEX_V(p_axis, 3, 0); switch (p_flag) { case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { return m_linearLimits.enable_limit[p_axis]; } break; case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { return m_angularLimits[p_axis].m_enableLimit; } break; case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { return m_angularLimits[p_axis].m_enableMotor; } break; } return 0; }