diff options
Diffstat (limited to 'servers/physics/joints')
| -rw-r--r-- | servers/physics/joints/SCsub | 5 | ||||
| -rw-r--r-- | servers/physics/joints/cone_twist_joint_sw.cpp | 366 | ||||
| -rw-r--r-- | servers/physics/joints/cone_twist_joint_sw.h | 142 | ||||
| -rw-r--r-- | servers/physics/joints/generic_6dof_joint_sw.cpp | 686 | ||||
| -rw-r--r-- | servers/physics/joints/generic_6dof_joint_sw.h | 401 | ||||
| -rw-r--r-- | servers/physics/joints/hinge_joint_sw.cpp | 450 | ||||
| -rw-r--r-- | servers/physics/joints/hinge_joint_sw.h | 117 | ||||
| -rw-r--r-- | servers/physics/joints/jacobian_entry_sw.h | 169 | ||||
| -rw-r--r-- | servers/physics/joints/pin_joint_sw.cpp | 167 | ||||
| -rw-r--r-- | servers/physics/joints/pin_joint_sw.h | 96 | ||||
| -rw-r--r-- | servers/physics/joints/slider_joint_sw.cpp | 443 | ||||
| -rw-r--r-- | servers/physics/joints/slider_joint_sw.h | 249 |
12 files changed, 0 insertions, 3291 deletions
diff --git a/servers/physics/joints/SCsub b/servers/physics/joints/SCsub deleted file mode 100644 index d730144861..0000000000 --- a/servers/physics/joints/SCsub +++ /dev/null @@ -1,5 +0,0 @@ -#!/usr/bin/env python - -Import('env') - -env.add_source_files(env.servers_sources, "*.cpp") diff --git a/servers/physics/joints/cone_twist_joint_sw.cpp b/servers/physics/joints/cone_twist_joint_sw.cpp deleted file mode 100644 index 58f66f7ea2..0000000000 --- a/servers/physics/joints/cone_twist_joint_sw.cpp +++ /dev/null @@ -1,366 +0,0 @@ -/*************************************************************************/ -/* cone_twist_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -Written by: Marcus Hennix -*/ - -#include "cone_twist_joint_sw.h" - -static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { - - if (Math::abs(n.z) > Math_SQRT12) { - // choose p in y-z plane - real_t a = n[1] * n[1] + n[2] * n[2]; - real_t k = 1.0 / Math::sqrt(a); - p = Vector3(0, -n[2] * k, n[1] * k); - // set q = n x p - q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]); - } else { - // choose p in x-y plane - real_t a = n.x * n.x + n.y * n.y; - real_t k = 1.0 / Math::sqrt(a); - p = Vector3(-n.y * k, n.x * k, 0); - // set q = n x p - q = Vector3(-n.z * p.y, n.z * p.x, a * k); - } -} - -static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { - real_t coeff_1 = Math_PI / 4.0f; - real_t coeff_2 = 3.0f * coeff_1; - real_t abs_y = Math::abs(y); - real_t angle; - if (x >= 0.0f) { - real_t r = (x - abs_y) / (x + abs_y); - angle = coeff_1 - coeff_1 * r; - } else { - real_t r = (x + abs_y) / (abs_y - x); - angle = coeff_2 - coeff_1 * r; - } - return (y < 0.0f) ? -angle : angle; -} - -ConeTwistJointSW::ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &rbBFrame) : - JointSW(_arr, 2) { - - A = rbA; - B = rbB; - - m_rbAFrame = rbAFrame; - m_rbBFrame = rbBFrame; - - m_swingSpan1 = Math_PI / 4.0; - m_swingSpan2 = Math_PI / 4.0; - m_twistSpan = Math_PI * 2; - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - - m_angularOnly = false; - m_solveTwistLimit = false; - m_solveSwingLimit = false; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); - - m_appliedImpulse = 0; -} - -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 (Math::is_zero_approx(relPos.length_squared())) { - normal[0] = Vector3(real_t(1.0), 0, 0); - } else { - normal[0] = relPos.normalized(); - } - - 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 b2Axis22 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1)); - Quat rotationArc = Quat(b2Axis1, b1Axis1); - Vector3 TwistRef = rotationArc.xform(b2Axis22); - 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; -} - -void ConeTwistJointSW::solve(real_t p_timestep) { - - Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); - Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); - - real_t tau = real_t(0.3); - - //linear part - if (!m_angularOnly) { - Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; - Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; - - Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); - Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); - Vector3 vel = vel1 - vel2; - - for (int i = 0; i < 3; i++) { - const Vector3 &normal = m_jac[i].m_linearJointAxis; - real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); - - real_t rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - Vector3 impulse_vector = normal * impulse; - A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); - B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); - } - } - - { - ///solve angular part - const Vector3 &angVelA = A->get_angular_velocity(); - const Vector3 &angVelB = B->get_angular_velocity(); - - // solve swing limit - if (m_solveSwingLimit) { - real_t amplitude = ((angVelB - angVelA).dot(m_swingAxis) * m_relaxationFactor * m_relaxationFactor + m_swingCorrection * (real_t(1.) / p_timestep) * m_biasFactor); - real_t impulseMag = amplitude * m_kSwing; - - // Clamp the accumulated impulse - real_t temp = m_accSwingLimitImpulse; - m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0)); - impulseMag = m_accSwingLimitImpulse - temp; - - Vector3 impulse = m_swingAxis * impulseMag; - - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); - } - - // solve twist limit - if (m_solveTwistLimit) { - real_t amplitude = ((angVelB - angVelA).dot(m_twistAxis) * m_relaxationFactor * m_relaxationFactor + m_twistCorrection * (real_t(1.) / p_timestep) * m_biasFactor); - real_t impulseMag = amplitude * m_kTwist; - - // Clamp the accumulated impulse - real_t temp = m_accTwistLimitImpulse; - m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0)); - impulseMag = m_accTwistLimitImpulse - temp; - - Vector3 impulse = m_twistAxis * impulseMag; - - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); - } - } -} - -void ConeTwistJointSW::set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value) { - - switch (p_param) { - case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: { - - m_swingSpan1 = p_value; - m_swingSpan2 = p_value; - } break; - case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: { - - m_twistSpan = p_value; - } break; - case PhysicsServer::CONE_TWIST_JOINT_BIAS: { - - m_biasFactor = p_value; - } break; - case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: { - - m_limitSoftness = p_value; - } break; - case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: { - - m_relaxationFactor = p_value; - } break; - case PhysicsServer::CONE_TWIST_MAX: break; // Can't happen, but silences warning - } -} - -real_t ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const { - - switch (p_param) { - case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: { - - return m_swingSpan1; - } break; - case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: { - - return m_twistSpan; - } break; - case PhysicsServer::CONE_TWIST_JOINT_BIAS: { - - return m_biasFactor; - } break; - case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: { - - return m_limitSoftness; - } break; - case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: { - - return m_relaxationFactor; - } break; - case PhysicsServer::CONE_TWIST_MAX: break; // Can't happen, but silences warning - } - - return 0; -} diff --git a/servers/physics/joints/cone_twist_joint_sw.h b/servers/physics/joints/cone_twist_joint_sw.h deleted file mode 100644 index 857aaa0d86..0000000000 --- a/servers/physics/joints/cone_twist_joint_sw.h +++ /dev/null @@ -1,142 +0,0 @@ -/*************************************************************************/ -/* cone_twist_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -Written by: Marcus Hennix -*/ - -#ifndef CONE_TWIST_JOINT_SW_H -#define CONE_TWIST_JOINT_SW_H - -#include "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_sw.h" - -///ConeTwistJointSW can be used to simulate ragdoll joints (upper arm, leg etc) -class ConeTwistJointSW : public JointSW { -#ifdef IN_PARALLELL_SOLVER -public: -#endif - - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints - - real_t m_appliedImpulse; - Transform m_rbAFrame; - Transform m_rbBFrame; - - real_t m_limitSoftness; - real_t m_biasFactor; - real_t m_relaxationFactor; - - real_t m_swingSpan1; - real_t m_swingSpan2; - real_t m_twistSpan; - - Vector3 m_swingAxis; - Vector3 m_twistAxis; - - real_t m_kSwing; - real_t m_kTwist; - - real_t m_twistLimitSign; - real_t m_swingCorrection; - real_t m_twistCorrection; - - real_t m_accSwingLimitImpulse; - real_t m_accTwistLimitImpulse; - - bool m_angularOnly; - bool m_solveTwistLimit; - bool m_solveSwingLimit; - -public: - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_CONE_TWIST; } - - virtual bool setup(real_t p_timestep); - virtual void solve(real_t p_timestep); - - ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &rbBFrame); - - void setAngularOnly(bool angularOnly) { - m_angularOnly = angularOnly; - } - - void setLimit(real_t _swingSpan1, real_t _swingSpan2, real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f) { - m_swingSpan1 = _swingSpan1; - m_swingSpan2 = _swingSpan2; - m_twistSpan = _twistSpan; - - m_limitSoftness = _softness; - m_biasFactor = _biasFactor; - m_relaxationFactor = _relaxationFactor; - } - - inline int getSolveTwistLimit() { - return m_solveTwistLimit; - } - - inline int getSolveSwingLimit() { - return m_solveTwistLimit; - } - - inline real_t getTwistLimitSign() { - return m_twistLimitSign; - } - - void set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer::ConeTwistJointParam p_param) const; -}; - -#endif // CONE_TWIST_JOINT_SW_H diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp deleted file mode 100644 index 8f0ccab7f7..0000000000 --- a/servers/physics/joints/generic_6dof_joint_sw.cpp +++ /dev/null @@ -1,686 +0,0 @@ -/*************************************************************************/ -/* generic_6dof_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -2007-09-09 -Generic6DOFJointSW Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - -#include "generic_6dof_joint_sw.h" - -#define GENERIC_D6_DISABLE_WARMSTARTING 1 - -//////////////////////////// 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()) 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 (Math::is_zero_approx(motor_relvel)) { - 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() { - Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis; - - m_calculatedAxisAngleDiff = relative_frame.get_euler_xyz(); - - // 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_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(), - 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_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().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(real_t p_timestep) { - - // 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 p_timestep) { - m_timeStep = p_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); -} // Generic6DOFJointSW::calcAnchorPos() - -void Generic6DOFJointSW::set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, real_t 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; - case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_MAX: break; // Can't happen, but silences warning - } -} - -real_t 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; - case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_MAX: break; // Can't happen, but silences warning - } - 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; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_MAX: break; // Can't happen, but silences warning - } -} -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; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { - // Not implemented in GodotPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_MAX: break; // Can't happen, but silences warning - } - - return 0; -} diff --git a/servers/physics/joints/generic_6dof_joint_sw.h b/servers/physics/joints/generic_6dof_joint_sw.h deleted file mode 100644 index 07626ffa97..0000000000 --- a/servers/physics/joints/generic_6dof_joint_sw.h +++ /dev/null @@ -1,401 +0,0 @@ -/*************************************************************************/ -/* generic_6dof_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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. -*/ - -#ifndef GENERIC_6DOF_JOINT_SW_H -#define GENERIC_6DOF_JOINT_SW_H - -#include "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_sw.h" - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -2007-09-09 -Generic6DOFJointSW Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - -//! Rotation Limit structure for generic joints -class G6DOFRotationalLimitMotorSW { -public: - //! limit_parameters - //!@{ - real_t m_loLimit; //!< joint limit - real_t m_hiLimit; //!< joint limit - real_t m_targetVelocity; //!< target motor velocity - real_t m_maxMotorForce; //!< max force on motor - real_t m_maxLimitForce; //!< max force on limit - real_t m_damping; //!< Damping. - real_t m_limitSoftness; //! Relaxation factor - real_t m_ERP; //!< Error tolerance factor when joint is at limit - real_t m_bounce; //!< restitution factor - bool m_enableMotor; - bool m_enableLimit; - - //!@} - - //! temp_variables - //!@{ - real_t m_currentLimitError; //! How much is violated this limit - int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit - real_t m_accumulatedImpulse; - //!@} - - G6DOFRotationalLimitMotorSW() { - m_accumulatedImpulse = 0.f; - m_targetVelocity = 0; - m_maxMotorForce = 0.1f; - m_maxLimitForce = 300.0f; - m_loLimit = -1e30; - m_hiLimit = 1e30; - m_ERP = 0.5f; - m_bounce = 0.0f; - m_damping = 1.0f; - m_limitSoftness = 0.5f; - m_currentLimit = 0; - m_currentLimitError = 0; - m_enableMotor = false; - m_enableLimit = false; - } - - G6DOFRotationalLimitMotorSW(const G6DOFRotationalLimitMotorSW &limot) { - m_targetVelocity = limot.m_targetVelocity; - m_maxMotorForce = limot.m_maxMotorForce; - m_limitSoftness = limot.m_limitSoftness; - m_loLimit = limot.m_loLimit; - m_hiLimit = limot.m_hiLimit; - m_ERP = limot.m_ERP; - m_bounce = limot.m_bounce; - m_currentLimit = limot.m_currentLimit; - m_currentLimitError = limot.m_currentLimitError; - m_enableMotor = limot.m_enableMotor; - } - - //! Is limited - bool isLimited() { - return (m_loLimit < m_hiLimit); - } - - //! Need apply correction - bool needApplyTorques() { - return (m_enableMotor || m_currentLimit != 0); - } - - //! calculates error - /*! - calculates m_currentLimit and m_currentLimitError. - */ - int testLimitValue(real_t test_value); - - //! apply the correction impulses for two bodies - real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, BodySW *body0, BodySW *body1); -}; - -class G6DOFTranslationalLimitMotorSW { -public: - Vector3 m_lowerLimit; //!< the constraint lower limits - Vector3 m_upperLimit; //!< the constraint upper limits - Vector3 m_accumulatedImpulse; - //! Linear_Limit_parameters - //!@{ - Vector3 m_limitSoftness; //!< Softness for linear limit - Vector3 m_damping; //!< Damping for linear limit - Vector3 m_restitution; //! Bounce parameter for linear limit - //!@} - bool enable_limit[3]; - - G6DOFTranslationalLimitMotorSW() { - m_lowerLimit = Vector3(0.f, 0.f, 0.f); - m_upperLimit = Vector3(0.f, 0.f, 0.f); - m_accumulatedImpulse = Vector3(0.f, 0.f, 0.f); - - m_limitSoftness = Vector3(1, 1, 1) * 0.7f; - m_damping = Vector3(1, 1, 1) * real_t(1.0f); - m_restitution = Vector3(1, 1, 1) * real_t(0.5f); - - enable_limit[0] = true; - enable_limit[1] = true; - enable_limit[2] = true; - } - - G6DOFTranslationalLimitMotorSW(const G6DOFTranslationalLimitMotorSW &other) { - m_lowerLimit = other.m_lowerLimit; - m_upperLimit = other.m_upperLimit; - m_accumulatedImpulse = other.m_accumulatedImpulse; - - m_limitSoftness = other.m_limitSoftness; - m_damping = other.m_damping; - m_restitution = other.m_restitution; - } - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - inline bool isLimited(int limitIndex) { - return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); - } - - real_t 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); -}; - -class Generic6DOFJointSW : public JointSW { -protected: - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - //! relative_frames - //!@{ - Transform m_frameInA; //!< the constraint space w.r.t body A - Transform m_frameInB; //!< the constraint space w.r.t body B - //!@} - - //! Jacobians - //!@{ - JacobianEntrySW m_jacLinear[3]; //!< 3 orthogonal linear constraints - JacobianEntrySW m_jacAng[3]; //!< 3 orthogonal angular constraints - //!@} - - //! Linear_Limit_parameters - //!@{ - G6DOFTranslationalLimitMotorSW m_linearLimits; - //!@} - - //! hinge_parameters - //!@{ - G6DOFRotationalLimitMotorSW m_angularLimits[3]; - //!@} - -protected: - //! temporal variables - //!@{ - real_t m_timeStep; - Transform m_calculatedTransformA; - Transform m_calculatedTransformB; - Vector3 m_calculatedAxisAngleDiff; - Vector3 m_calculatedAxis[3]; - - Vector3 m_AnchorPos; // point between pivots of bodies A and B to solve linear axes - - bool m_useLinearReferenceFrameA; - - //!@} - - Generic6DOFJointSW &operator=(Generic6DOFJointSW &other) { - ERR_PRINT("pito"); - (void)other; - return *this; - } - - void buildLinearJacobian( - JacobianEntrySW &jacLinear, const Vector3 &normalWorld, - const Vector3 &pivotAInW, const Vector3 &pivotBInW); - - void buildAngularJacobian(JacobianEntrySW &jacAngular, const Vector3 &jointAxisW); - - //! calcs the euler angles between the two bodies. - void calculateAngleInfo(); - -public: - Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA); - - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; } - - virtual bool setup(real_t p_timestep); - virtual void solve(real_t p_timestep); - - //! Calcs global transform of the offsets - /*! - Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. - \sa Generic6DOFJointSW.getCalculatedTransformA , Generic6DOFJointSW.getCalculatedTransformB, Generic6DOFJointSW.calculateAngleInfo - */ - void calculateTransforms(); - - //! Gets the global transform of the offset for body A - /*! - \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo. - */ - const Transform &getCalculatedTransformA() const { - return m_calculatedTransformA; - } - - //! Gets the global transform of the offset for body B - /*! - \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo. - */ - const Transform &getCalculatedTransformB() const { - return m_calculatedTransformB; - } - - const Transform &getFrameOffsetA() const { - return m_frameInA; - } - - const Transform &getFrameOffsetB() const { - return m_frameInB; - } - - Transform &getFrameOffsetA() { - return m_frameInA; - } - - Transform &getFrameOffsetB() { - return m_frameInB; - } - - //! performs Jacobian calculation, and also calculates angle differences and axis - - void updateRHS(real_t timeStep); - - //! Get the rotation axis in global coordinates - /*! - \pre Generic6DOFJointSW.buildJacobian must be called previously. - */ - Vector3 getAxis(int axis_index) const; - - //! Get the relative Euler angle - /*! - \pre Generic6DOFJointSW.buildJacobian must be called previously. - */ - real_t getAngle(int axis_index) const; - - //! Test angular limit. - /*! - Calculates angular correction and returns true if limit needs to be corrected. - \pre Generic6DOFJointSW.buildJacobian must be called previously. - */ - bool testAngularLimitMotor(int axis_index); - - void setLinearLowerLimit(const Vector3 &linearLower) { - m_linearLimits.m_lowerLimit = linearLower; - } - - void setLinearUpperLimit(const Vector3 &linearUpper) { - m_linearLimits.m_upperLimit = linearUpper; - } - - void setAngularLowerLimit(const Vector3 &angularLower) { - m_angularLimits[0].m_loLimit = angularLower.x; - m_angularLimits[1].m_loLimit = angularLower.y; - m_angularLimits[2].m_loLimit = angularLower.z; - } - - void setAngularUpperLimit(const Vector3 &angularUpper) { - m_angularLimits[0].m_hiLimit = angularUpper.x; - m_angularLimits[1].m_hiLimit = angularUpper.y; - m_angularLimits[2].m_hiLimit = angularUpper.z; - } - - //! Retrieves the angular limit informacion - G6DOFRotationalLimitMotorSW *getRotationalLimitMotor(int index) { - return &m_angularLimits[index]; - } - - //! Retrieves the limit informacion - G6DOFTranslationalLimitMotorSW *getTranslationalLimitMotor() { - return &m_linearLimits; - } - - //first 3 are linear, next 3 are angular - void setLimit(int axis, real_t lo, real_t hi) { - if (axis < 3) { - m_linearLimits.m_lowerLimit[axis] = lo; - m_linearLimits.m_upperLimit[axis] = hi; - } else { - m_angularLimits[axis - 3].m_loLimit = lo; - m_angularLimits[axis - 3].m_hiLimit = hi; - } - } - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - bool isLimited(int limitIndex) { - if (limitIndex < 3) { - return m_linearLimits.isLimited(limitIndex); - } - return m_angularLimits[limitIndex - 3].isLimited(); - } - - const BodySW *getRigidBodyA() const { - return A; - } - const BodySW *getRigidBodyB() const { - return B; - } - - virtual void calcAnchorPos(void); // overridable - - void set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value); - real_t get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const; - - void set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value); - bool get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const; -}; - -#endif // GENERIC_6DOF_JOINT_SW_H diff --git a/servers/physics/joints/hinge_joint_sw.cpp b/servers/physics/joints/hinge_joint_sw.cpp deleted file mode 100644 index 1ad3e738ba..0000000000 --- a/servers/physics/joints/hinge_joint_sw.cpp +++ /dev/null @@ -1,450 +0,0 @@ -/*************************************************************************/ -/* hinge_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "hinge_joint_sw.h" - -static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { - - if (Math::abs(n.z) > Math_SQRT12) { - // choose p in y-z plane - real_t a = n[1] * n[1] + n[2] * n[2]; - real_t k = 1.0 / Math::sqrt(a); - p = Vector3(0, -n[2] * k, n[1] * k); - // set q = n x p - q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]); - } else { - // choose p in x-y plane - real_t a = n.x * n.x + n.y * n.y; - real_t k = 1.0 / Math::sqrt(a); - p = Vector3(-n.y * k, n.x * k, 0); - // set q = n x p - q = Vector3(-n.z * p.y, n.z * p.x, a * k); - } -} - -HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB) : - JointSW(_arr, 2) { - - A = rbA; - B = rbB; - - m_rbAFrame = frameA; - m_rbBFrame = frameB; - // flip axis - m_rbBFrame.basis[0][2] *= real_t(-1.); - m_rbBFrame.basis[1][2] *= real_t(-1.); - m_rbBFrame.basis[2][2] *= real_t(-1.); - - //start with free - m_lowerLimit = Math_PI; - m_upperLimit = -Math_PI; - - m_useLimit = false; - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; - - tau = 0.3; - - m_angularOnly = false; - m_enableAngularMotor = false; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, - const Vector3 &axisInA, const Vector3 &axisInB) : - JointSW(_arr, 2) { - - A = rbA; - B = rbB; - - m_rbAFrame.origin = pivotInA; - - // since no frame is given, assume this to be zero angle and just pick rb transform axis - Vector3 rbAxisA1 = rbA->get_transform().basis.get_axis(0); - - Vector3 rbAxisA2; - real_t projection = axisInA.dot(rbAxisA1); - if (projection >= 1.0f - CMP_EPSILON) { - rbAxisA1 = -rbA->get_transform().basis.get_axis(2); - rbAxisA2 = rbA->get_transform().basis.get_axis(1); - } else if (projection <= -1.0f + CMP_EPSILON) { - rbAxisA1 = rbA->get_transform().basis.get_axis(2); - rbAxisA2 = rbA->get_transform().basis.get_axis(1); - } else { - rbAxisA2 = axisInA.cross(rbAxisA1); - rbAxisA1 = rbAxisA2.cross(axisInA); - } - - m_rbAFrame.basis = Basis(rbAxisA1.x, rbAxisA2.x, axisInA.x, - rbAxisA1.y, rbAxisA2.y, axisInA.y, - rbAxisA1.z, rbAxisA2.z, axisInA.z); - - Quat rotationArc = Quat(axisInA, axisInB); - Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1); - Vector3 rbAxisB2 = axisInB.cross(rbAxisB1); - - m_rbBFrame.origin = pivotInB; - m_rbBFrame.basis = Basis(rbAxisB1.x, rbAxisB2.x, -axisInB.x, - rbAxisB1.y, rbAxisB2.y, -axisInB.y, - rbAxisB1.z, rbAxisB2.z, -axisInB.z); - - //start with free - m_lowerLimit = Math_PI; - m_upperLimit = -Math_PI; - - m_useLimit = false; - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; - - tau = 0.3; - - m_angularOnly = false; - m_enableAngularMotor = false; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -bool HingeJointSW::setup(real_t 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 (Math::is_zero_approx(relPos.length_squared())) { - normal[0] = Vector3(real_t(1.0), 0, 0); - } else { - normal[0] = relPos.normalized(); - } - - 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())); - } - } - - //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); - - 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_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); - - memnew_placement(&m_jacAng[1], JacobianEntrySW(jointAxis1, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); - - memnew_placement(&m_jacAng[2], JacobianEntrySW(hingeAxisWorld, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); - - // Compute limit information - real_t hingeAngle = get_hinge_angle(); - - //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_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; -} - -void HingeJointSW::solve(real_t p_step) { - - Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); - Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); - - //real_t tau = real_t(0.3); - - //linear part - if (!m_angularOnly) { - Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; - Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; - - Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); - Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); - Vector3 vel = vel1 - vel2; - - for (int i = 0; i < 3; i++) { - const Vector3 &normal = m_jac[i].m_linearJointAxis; - real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); - - real_t rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - Vector3 impulse_vector = normal * impulse; - A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); - B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); - } - } - - { - ///solve angular part - - // get axes in world space - Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); - Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(2)); - - const Vector3 &angVelA = A->get_angular_velocity(); - const Vector3 &angVelB = B->get_angular_velocity(); - - Vector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); - Vector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); - - Vector3 angAorthog = angVelA - angVelAroundHingeAxisA; - Vector3 angBorthog = angVelB - angVelAroundHingeAxisB; - Vector3 velrelOrthog = angAorthog - angBorthog; - { - //solve orthogonal angular velocity correction - real_t relaxation = real_t(1.); - real_t len = velrelOrthog.length(); - if (len > real_t(0.00001)) { - Vector3 normal = velrelOrthog.normalized(); - real_t denom = A->compute_angular_impulse_denominator(normal) + - B->compute_angular_impulse_denominator(normal); - // scale for mass and relaxation - velrelOrthog *= (real_t(1.) / denom) * m_relaxationFactor; - } - - //solve angular positional correction - Vector3 angularError = -axisA.cross(axisB) * (real_t(1.) / p_step); - real_t len2 = angularError.length(); - if (len2 > real_t(0.00001)) { - Vector3 normal2 = angularError.normalized(); - real_t denom2 = A->compute_angular_impulse_denominator(normal2) + - B->compute_angular_impulse_denominator(normal2); - angularError *= (real_t(1.) / denom2) * relaxation; - } - - A->apply_torque_impulse(-velrelOrthog + angularError); - B->apply_torque_impulse(velrelOrthog - angularError); - - // solve limit - if (m_solveLimit) { - real_t amplitude = ((angVelB - angVelA).dot(axisA) * m_relaxationFactor + m_correction * (real_t(1.) / p_step) * m_biasFactor) * m_limitSign; - - real_t impulseMag = amplitude * m_kHinge; - - // Clamp the accumulated impulse - real_t temp = m_accLimitImpulse; - m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0)); - impulseMag = m_accLimitImpulse - temp; - - Vector3 impulse = axisA * impulseMag * m_limitSign; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); - } - } - - //apply motor - if (m_enableAngularMotor) { - //todo: add limits too - Vector3 angularLimit(0, 0, 0); - - Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; - real_t projRelVel = velrel.dot(axisA); - - real_t desiredMotorVel = m_motorTargetVelocity; - real_t motor_relvel = desiredMotorVel - projRelVel; - - real_t unclippedMotorImpulse = m_kHinge * motor_relvel; - //todo: should clip against accumulated impulse - real_t clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; - clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; - Vector3 motorImp = clippedMotorImpulse * axisA; - - A->apply_torque_impulse(motorImp + angularLimit); - B->apply_torque_impulse(-motorImp - angularLimit); - } - } -} -/* -void HingeJointSW::updateRHS(real_t timeStep) -{ - (void)timeStep; - -} -*/ - -static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { - real_t coeff_1 = Math_PI / 4.0f; - real_t coeff_2 = 3.0f * coeff_1; - real_t abs_y = Math::abs(y); - real_t angle; - if (x >= 0.0f) { - real_t r = (x - abs_y) / (x + abs_y); - angle = coeff_1 - coeff_1 * r; - } else { - real_t r = (x + abs_y) / (abs_y - x); - angle = coeff_2 - coeff_1 * r; - } - return (y < 0.0f) ? -angle : angle; -} - -real_t HingeJointSW::get_hinge_angle() { - const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(0)); - const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(1)); - const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(1)); - - return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); -} - -void HingeJointSW::set_param(PhysicsServer::HingeJointParam p_param, real_t p_value) { - - switch (p_param) { - - case PhysicsServer::HINGE_JOINT_BIAS: tau = p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: m_upperLimit = p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: m_lowerLimit = p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: m_biasFactor = p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: m_limitSoftness = p_value; break; - case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: m_relaxationFactor = p_value; break; - case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: m_motorTargetVelocity = p_value; break; - case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: m_maxMotorImpulse = p_value; break; - case PhysicsServer::HINGE_JOINT_MAX: break; // Can't happen, but silences warning - } -} - -real_t HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const { - - switch (p_param) { - - case PhysicsServer::HINGE_JOINT_BIAS: return tau; - case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: return m_upperLimit; - case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: return m_lowerLimit; - case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: return m_biasFactor; - case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: return m_limitSoftness; - case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: return m_relaxationFactor; - case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: return m_motorTargetVelocity; - case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: return m_maxMotorImpulse; - case PhysicsServer::HINGE_JOINT_MAX: break; // Can't happen, but silences warning - } - - return 0; -} - -void HingeJointSW::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value) { - - switch (p_flag) { - case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: m_useLimit = p_value; break; - case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: m_enableAngularMotor = p_value; break; - case PhysicsServer::HINGE_JOINT_FLAG_MAX: break; // Can't happen, but silences warning - } -} -bool HingeJointSW::get_flag(PhysicsServer::HingeJointFlag p_flag) const { - - switch (p_flag) { - case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: return m_useLimit; - case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: return m_enableAngularMotor; - case PhysicsServer::HINGE_JOINT_FLAG_MAX: break; // Can't happen, but silences warning - } - - return false; -} diff --git a/servers/physics/joints/hinge_joint_sw.h b/servers/physics/joints/hinge_joint_sw.h deleted file mode 100644 index 1c160cfc09..0000000000 --- a/servers/physics/joints/hinge_joint_sw.h +++ /dev/null @@ -1,117 +0,0 @@ -/*************************************************************************/ -/* hinge_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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. -*/ - -#ifndef HINGE_JOINT_SW_H -#define HINGE_JOINT_SW_H - -#include "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_sw.h" - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -class HingeJointSW : public JointSW { - - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints - JacobianEntrySW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor - - Transform m_rbAFrame; // constraint axii. Assumes z is hinge axis. - Transform m_rbBFrame; - - real_t m_motorTargetVelocity; - real_t m_maxMotorImpulse; - - real_t m_limitSoftness; - real_t m_biasFactor; - real_t m_relaxationFactor; - - real_t m_lowerLimit; - real_t m_upperLimit; - - real_t m_kHinge; - - real_t m_limitSign; - real_t m_correction; - - real_t m_accLimitImpulse; - - real_t tau; - - bool m_useLimit; - bool m_angularOnly; - bool m_enableAngularMotor; - bool m_solveLimit; - - real_t m_appliedImpulse; - -public: - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_HINGE; } - - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); - - real_t get_hinge_angle(); - - void set_param(PhysicsServer::HingeJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer::HingeJointParam p_param) const; - - void set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value); - bool get_flag(PhysicsServer::HingeJointFlag p_flag) const; - - HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB); - HingeJointSW(BodySW *rbA, BodySW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB); -}; - -#endif // HINGE_JOINT_SW_H diff --git a/servers/physics/joints/jacobian_entry_sw.h b/servers/physics/joints/jacobian_entry_sw.h deleted file mode 100644 index a17175e6de..0000000000 --- a/servers/physics/joints/jacobian_entry_sw.h +++ /dev/null @@ -1,169 +0,0 @@ -/*************************************************************************/ -/* jacobian_entry_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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. -*/ - -#ifndef JACOBIAN_ENTRY_SW_H -#define JACOBIAN_ENTRY_SW_H - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "core/math/transform.h" - -class JacobianEntrySW { -public: - JacobianEntrySW(){}; - //constraint between two different rigidbodies - JacobianEntrySW( - const Basis &world2A, - const Basis &world2B, - const Vector3 &rel_pos1, const Vector3 &rel_pos2, - const Vector3 &jointAxis, - const Vector3 &inertiaInvA, - const real_t massInvA, - const Vector3 &inertiaInvB, - const real_t massInvB) : - m_linearJointAxis(jointAxis) { - m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis)); - m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); - - ERR_FAIL_COND(m_Adiag <= real_t(0.0)); - } - - //angular constraint between two different rigidbodies - JacobianEntrySW(const Vector3 &jointAxis, - const Basis &world2A, - const Basis &world2B, - const Vector3 &inertiaInvA, - const Vector3 &inertiaInvB) : - m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))) { - m_aJ = world2A.xform(jointAxis); - m_bJ = world2B.xform(-jointAxis); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - - ERR_FAIL_COND(m_Adiag <= real_t(0.0)); - } - - //angular constraint between two different rigidbodies - JacobianEntrySW(const Vector3 &axisInA, - const Vector3 &axisInB, - const Vector3 &inertiaInvA, - const Vector3 &inertiaInvB) : - m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))), - m_aJ(axisInA), - m_bJ(-axisInB) { - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - - ERR_FAIL_COND(m_Adiag <= real_t(0.0)); - } - - //constraint on one rigidbody - JacobianEntrySW( - const Basis &world2A, - const Vector3 &rel_pos1, const Vector3 &rel_pos2, - const Vector3 &jointAxis, - const Vector3 &inertiaInvA, - const real_t massInvA) : - m_linearJointAxis(jointAxis) { - m_aJ = world2A.xform(rel_pos1.cross(jointAxis)); - m_bJ = world2A.xform(rel_pos2.cross(-jointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = Vector3(real_t(0.), real_t(0.), real_t(0.)); - m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); - - ERR_FAIL_COND(m_Adiag <= real_t(0.0)); - } - - real_t getDiagonal() const { return m_Adiag; } - - // for two constraints on the same rigidbody (for example vehicle friction) - real_t getNonDiagonal(const JacobianEntrySW &jacB, const real_t massInvA) const { - const JacobianEntrySW &jacA = *this; - real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); - real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ); - return lin + ang; - } - - // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) - real_t getNonDiagonal(const JacobianEntrySW &jacB, const real_t massInvA, const real_t massInvB) const { - const JacobianEntrySW &jacA = *this; - Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; - Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; - Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; - Vector3 lin0 = massInvA * lin; - Vector3 lin1 = massInvB * lin; - Vector3 sum = ang0 + ang1 + lin0 + lin1; - return sum[0] + sum[1] + sum[2]; - } - - real_t getRelativeVelocity(const Vector3 &linvelA, const Vector3 &angvelA, const Vector3 &linvelB, const Vector3 &angvelB) { - Vector3 linrel = linvelA - linvelB; - Vector3 angvela = angvelA * m_aJ; - Vector3 angvelb = angvelB * m_bJ; - linrel *= m_linearJointAxis; - angvela += angvelb; - angvela += linrel; - real_t rel_vel2 = angvela[0] + angvela[1] + angvela[2]; - return rel_vel2 + CMP_EPSILON; - } - //private: - - Vector3 m_linearJointAxis; - Vector3 m_aJ; - Vector3 m_bJ; - Vector3 m_0MinvJt; - Vector3 m_1MinvJt; - //Optimization: can be stored in the w/last component of one of the vectors - real_t m_Adiag; -}; - -#endif // JACOBIAN_ENTRY_SW_H diff --git a/servers/physics/joints/pin_joint_sw.cpp b/servers/physics/joints/pin_joint_sw.cpp deleted file mode 100644 index fe994aa172..0000000000 --- a/servers/physics/joints/pin_joint_sw.cpp +++ /dev/null @@ -1,167 +0,0 @@ -/*************************************************************************/ -/* pin_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "pin_joint_sw.h" - -bool PinJointSW::setup(real_t 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_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(), - B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(), - normal, - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); - normal[i] = 0; - } - - return true; -} - -void PinJointSW::solve(real_t p_step) { - - Vector3 pivotAInW = A->get_transform().xform(m_pivotInA); - Vector3 pivotBInW = B->get_transform().xform(m_pivotInB); - - Vector3 normal(0, 0, 0); - - //Vector3 angvelA = A->get_transform().origin.getBasis().transpose() * A->getAngularVelocity(); - //Vector3 angvelB = B->get_transform().origin.getBasis().transpose() * B->getAngularVelocity(); - - for (int i = 0; i < 3; i++) { - normal[i] = 1; - real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); - - Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; - Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; - //this jacobian entry could be re-used for all iterations - - Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); - Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); - Vector3 vel = vel1 - vel2; - - real_t rel_vel; - rel_vel = normal.dot(vel); - - /* - //velocity error (first order error) - real_t rel_vel = m_jac[i].getRelativeVelocity(A->getLinearVelocity(),angvelA, - B->getLinearVelocity(),angvelB); - */ - - //positional error (zeroth order error) - real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - - real_t impulse = depth * m_tau / p_step * jacDiagABInv - m_damping * rel_vel * jacDiagABInv; - - real_t impulseClamp = m_impulseClamp; - if (impulseClamp > 0) { - if (impulse < -impulseClamp) - impulse = -impulseClamp; - if (impulse > impulseClamp) - impulse = impulseClamp; - } - - m_appliedImpulse += impulse; - Vector3 impulse_vector = normal * impulse; - A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); - B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); - - normal[i] = 0; - } -} - -void PinJointSW::set_param(PhysicsServer::PinJointParam p_param, real_t p_value) { - - switch (p_param) { - case PhysicsServer::PIN_JOINT_BIAS: m_tau = p_value; break; - case PhysicsServer::PIN_JOINT_DAMPING: m_damping = p_value; break; - case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: m_impulseClamp = p_value; break; - } -} - -real_t PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const { - - switch (p_param) { - case PhysicsServer::PIN_JOINT_BIAS: return m_tau; - case PhysicsServer::PIN_JOINT_DAMPING: return m_damping; - case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: return m_impulseClamp; - } - - return 0; -} - -PinJointSW::PinJointSW(BodySW *p_body_a, const Vector3 &p_pos_a, BodySW *p_body_b, const Vector3 &p_pos_b) : - JointSW(_arr, 2) { - - A = p_body_a; - B = p_body_b; - m_pivotInA = p_pos_a; - m_pivotInB = p_pos_b; - - m_tau = 0.3; - m_damping = 1; - m_impulseClamp = 0; - m_appliedImpulse = 0; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -PinJointSW::~PinJointSW() { -} diff --git a/servers/physics/joints/pin_joint_sw.h b/servers/physics/joints/pin_joint_sw.h deleted file mode 100644 index 42884e4940..0000000000 --- a/servers/physics/joints/pin_joint_sw.h +++ /dev/null @@ -1,96 +0,0 @@ -/*************************************************************************/ -/* pin_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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. -*/ - -#ifndef PIN_JOINT_SW_H -#define PIN_JOINT_SW_H - -#include "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_sw.h" - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -class PinJointSW : public JointSW { - - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - real_t m_tau; //bias - real_t m_damping; - real_t m_impulseClamp; - real_t m_appliedImpulse; - - JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints - - Vector3 m_pivotInA; - Vector3 m_pivotInB; - -public: - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; } - - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); - - void set_param(PhysicsServer::PinJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer::PinJointParam p_param) const; - - void set_pos_a(const Vector3 &p_pos) { m_pivotInA = p_pos; } - void set_pos_b(const Vector3 &p_pos) { m_pivotInB = p_pos; } - - Vector3 get_position_a() { return m_pivotInA; } - Vector3 get_position_b() { return m_pivotInB; } - - PinJointSW(BodySW *p_body_a, const Vector3 &p_pos_a, BodySW *p_body_b, const Vector3 &p_pos_b); - ~PinJointSW(); -}; - -#endif // PIN_JOINT_SW_H diff --git a/servers/physics/joints/slider_joint_sw.cpp b/servers/physics/joints/slider_joint_sw.cpp deleted file mode 100644 index 9963c7ae89..0000000000 --- a/servers/physics/joints/slider_joint_sw.cpp +++ /dev/null @@ -1,443 +0,0 @@ -/*************************************************************************/ -/* slider_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 - -*/ - -#include "slider_joint_sw.h" - -//----------------------------------------------------------------------------- - -static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { - real_t coeff_1 = Math_PI / 4.0f; - real_t coeff_2 = 3.0f * coeff_1; - real_t abs_y = Math::abs(y); - real_t angle; - if (x >= 0.0f) { - real_t r = (x - abs_y) / (x + abs_y); - angle = coeff_1 - coeff_1 * r; - } else { - real_t r = (x + abs_y) / (abs_y - x); - angle = coeff_2 - coeff_1 * r; - } - return (y < 0.0f) ? -angle : angle; -} - -void SliderJointSW::initParams() { - m_lowerLinLimit = real_t(1.0); - m_upperLinLimit = real_t(-1.0); - m_lowerAngLimit = real_t(0.); - m_upperAngLimit = real_t(0.); - m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirLin = real_t(0.); - m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirAng = real_t(0.); - m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; - - m_poweredLinMotor = false; - m_targetLinMotorVelocity = real_t(0.); - m_maxLinMotorForce = real_t(0.); - m_accumulatedLinMotorImpulse = real_t(0.0); - - m_poweredAngMotor = false; - m_targetAngMotorVelocity = real_t(0.); - m_maxAngMotorForce = real_t(0.); - m_accumulatedAngMotorImpulse = real_t(0.0); - -} // SliderJointSW::initParams() - -//----------------------------------------------------------------------------- - -//----------------------------------------------------------------------------- - -SliderJointSW::SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB) : - JointSW(_arr, 2), - m_frameInA(frameInA), - m_frameInB(frameInB) { - - A = rbA; - B = rbB; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); - - initParams(); -} // SliderJointSW::SliderJointSW() - -//----------------------------------------------------------------------------- - -bool SliderJointSW::setup(real_t 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_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - m_relPosA - A->get_center_of_mass(), - m_relPosB - B->get_center_of_mass(), - 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_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().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 SliderJointSW::solve(real_t p_step) { - - int i; - // linear - Vector3 velA = A->get_velocity_in_local_point(m_relPosA); - Vector3 velB = B->get_velocity_in_local_point(m_relPosB); - Vector3 vel = velA - velB; - for (i = 0; i < 3; i++) { - const Vector3 &normal = m_jacLin[i].m_linearJointAxis; - real_t rel_vel = normal.dot(vel); - // calculate positional error - real_t depth = m_depth[i]; - // get parameters - real_t softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); - real_t restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); - real_t damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); - // calcutate and apply impulse - real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i]; - Vector3 impulse_vector = normal * normalImpulse; - A->apply_impulse(m_relPosA, impulse_vector); - B->apply_impulse(m_relPosB, -impulse_vector); - if (m_poweredLinMotor && (!i)) { // apply linear motor - if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) { - real_t desiredMotorVel = m_targetLinMotorVelocity; - real_t motor_relvel = desiredMotorVel + rel_vel; - normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; - // clamp accumulated impulse - real_t new_acc = m_accumulatedLinMotorImpulse + Math::abs(normalImpulse); - if (new_acc > m_maxLinMotorForce) { - new_acc = m_maxLinMotorForce; - } - real_t del = new_acc - m_accumulatedLinMotorImpulse; - if (normalImpulse < real_t(0.0)) { - normalImpulse = -del; - } else { - normalImpulse = del; - } - m_accumulatedLinMotorImpulse = new_acc; - // apply clamped impulse - impulse_vector = normal * normalImpulse; - A->apply_impulse(m_relPosA, impulse_vector); - B->apply_impulse(m_relPosB, -impulse_vector); - } - } - } - // angular - // get axes in world space - Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); - Vector3 axisB = m_calculatedTransformB.basis.get_axis(0); - - const Vector3 &angVelA = A->get_angular_velocity(); - const Vector3 &angVelB = B->get_angular_velocity(); - - Vector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); - Vector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); - - Vector3 angAorthog = angVelA - angVelAroundAxisA; - Vector3 angBorthog = angVelB - angVelAroundAxisB; - Vector3 velrelOrthog = angAorthog - angBorthog; - //solve orthogonal angular velocity correction - real_t len = velrelOrthog.length(); - if (len > real_t(0.00001)) { - Vector3 normal = velrelOrthog.normalized(); - real_t denom = A->compute_angular_impulse_denominator(normal) + B->compute_angular_impulse_denominator(normal); - velrelOrthog *= (real_t(1.) / denom) * m_dampingOrthoAng * m_softnessOrthoAng; - } - //solve angular positional correction - Vector3 angularError = axisA.cross(axisB) * (real_t(1.) / p_step); - real_t len2 = angularError.length(); - if (len2 > real_t(0.00001)) { - Vector3 normal2 = angularError.normalized(); - real_t denom2 = A->compute_angular_impulse_denominator(normal2) + B->compute_angular_impulse_denominator(normal2); - angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; - } - // apply impulse - A->apply_torque_impulse(-velrelOrthog + angularError); - B->apply_torque_impulse(velrelOrthog - angularError); - real_t impulseMag; - //solve angular limits - if (m_solveAngLim) { - impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / p_step; - impulseMag *= m_kAngle * m_softnessLimAng; - } else { - impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / p_step; - impulseMag *= m_kAngle * m_softnessDirAng; - } - Vector3 impulse = axisA * impulseMag; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); - //apply angular motor - if (m_poweredAngMotor) { - if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) { - Vector3 velrel = angVelAroundAxisA - angVelAroundAxisB; - real_t projRelVel = velrel.dot(axisA); - - real_t desiredMotorVel = m_targetAngMotorVelocity; - real_t motor_relvel = desiredMotorVel - projRelVel; - - real_t angImpulse = m_kAngle * motor_relvel; - // clamp accumulated impulse - real_t new_acc = m_accumulatedAngMotorImpulse + Math::abs(angImpulse); - if (new_acc > m_maxAngMotorForce) { - new_acc = m_maxAngMotorForce; - } - real_t del = new_acc - m_accumulatedAngMotorImpulse; - if (angImpulse < real_t(0.0)) { - angImpulse = -del; - } else { - angImpulse = del; - } - m_accumulatedAngMotorImpulse = new_acc; - // apply clamped impulse - Vector3 motorImp = angImpulse * axisA; - A->apply_torque_impulse(motorImp); - B->apply_torque_impulse(-motorImp); - } - } -} // SliderJointSW::solveConstraint() - -//----------------------------------------------------------------------------- - -//----------------------------------------------------------------------------- - -void SliderJointSW::calculateTransforms(void) { - 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; - Vector3 normalWorld; - int i; - //linear part - for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); - m_depth[i] = m_delta.dot(normalWorld); - } -} // SliderJointSW::calculateTransforms() - -//----------------------------------------------------------------------------- - -void SliderJointSW::testLinLimits(void) { - m_solveLinLim = false; - m_linPos = m_depth[0]; - 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] = real_t(0.); - } - } else { - m_depth[0] = real_t(0.); - } -} // SliderJointSW::testLinLimits() - -//----------------------------------------------------------------------------- - -void SliderJointSW::testAngLimits(void) { - m_angDepth = real_t(0.); - m_solveAngLim = false; - if (m_lowerAngLimit <= m_upperAngLimit) { - const Vector3 axisA0 = m_calculatedTransformA.basis.get_axis(1); - const Vector3 axisA1 = m_calculatedTransformA.basis.get_axis(2); - const Vector3 axisB0 = m_calculatedTransformB.basis.get_axis(1); - real_t rot = atan2fast(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; - } - } -} // SliderJointSW::testAngLimits() - -//----------------------------------------------------------------------------- - -Vector3 SliderJointSW::getAncorInA(void) { - Vector3 ancorInA; - ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis; - ancorInA = A->get_transform().inverse().xform(ancorInA); - return ancorInA; -} // SliderJointSW::getAncorInA() - -//----------------------------------------------------------------------------- - -Vector3 SliderJointSW::getAncorInB(void) { - Vector3 ancorInB; - ancorInB = m_frameInB.origin; - return ancorInB; -} // SliderJointSW::getAncorInB(); - -void SliderJointSW::set_param(PhysicsServer::SliderJointParam p_param, real_t p_value) { - - switch (p_param) { - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: m_upperLinLimit = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: m_lowerLinLimit = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: m_softnessLimLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: m_restitutionLimLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: m_dampingLimLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: m_softnessDirLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: m_restitutionDirLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: m_dampingDirLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoLin = p_value; break; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: m_dampingOrthoLin = p_value; break; - - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: m_upperAngLimit = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: m_lowerAngLimit = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: m_softnessLimAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: m_restitutionLimAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: m_dampingLimAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: m_softnessDirAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: m_restitutionDirAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: m_dampingDirAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoAng = p_value; break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: m_dampingOrthoAng = p_value; break; - - case PhysicsServer::SLIDER_JOINT_MAX: break; // Can't happen, but silences warning - } -} - -real_t SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const { - - switch (p_param) { - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: return m_upperLinLimit; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: return m_lowerLinLimit; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: return m_softnessLimLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: return m_restitutionLimLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: return m_dampingLimLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: return m_softnessDirLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: return m_restitutionDirLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: return m_dampingDirLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: return m_dampingOrthoLin; - - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: return m_upperAngLimit; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: return m_lowerAngLimit; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: return m_softnessLimAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: return m_restitutionLimAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: return m_dampingLimAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: return m_softnessDirAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: return m_restitutionDirAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: return m_dampingDirAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: return m_dampingOrthoAng; - - case PhysicsServer::SLIDER_JOINT_MAX: break; // Can't happen, but silences warning - } - - return 0; -} diff --git a/servers/physics/joints/slider_joint_sw.h b/servers/physics/joints/slider_joint_sw.h deleted file mode 100644 index 8b416eafc9..0000000000 --- a/servers/physics/joints/slider_joint_sw.h +++ /dev/null @@ -1,249 +0,0 @@ -/*************************************************************************/ -/* slider_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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. -*/ - -#ifndef SLIDER_JOINT_SW_H -#define SLIDER_JOINT_SW_H - -#include "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_sw.h" - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 - -*/ - -#define SLIDER_CONSTRAINT_DEF_SOFTNESS (real_t(1.0)) -#define SLIDER_CONSTRAINT_DEF_DAMPING (real_t(1.0)) -#define SLIDER_CONSTRAINT_DEF_RESTITUTION (real_t(0.7)) - -//----------------------------------------------------------------------------- - -class SliderJointSW : public JointSW { -protected: - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - Transform m_frameInA; - Transform m_frameInB; - - // linear limits - real_t m_lowerLinLimit; - real_t m_upperLinLimit; - // angular limits - real_t m_lowerAngLimit; - real_t m_upperAngLimit; - // softness, restitution and damping for different cases - // DirLin - moving inside linear limits - // LimLin - hitting linear limit - // DirAng - moving inside angular limits - // LimAng - hitting angular limit - // OrthoLin, OrthoAng - against constraint axis - real_t m_softnessDirLin; - real_t m_restitutionDirLin; - real_t m_dampingDirLin; - real_t m_softnessDirAng; - real_t m_restitutionDirAng; - real_t m_dampingDirAng; - real_t m_softnessLimLin; - real_t m_restitutionLimLin; - real_t m_dampingLimLin; - real_t m_softnessLimAng; - real_t m_restitutionLimAng; - real_t m_dampingLimAng; - real_t m_softnessOrthoLin; - real_t m_restitutionOrthoLin; - real_t m_dampingOrthoLin; - real_t m_softnessOrthoAng; - real_t m_restitutionOrthoAng; - real_t m_dampingOrthoAng; - - // for interlal use - bool m_solveLinLim; - bool m_solveAngLim; - - JacobianEntrySW m_jacLin[3]; - real_t m_jacLinDiagABInv[3]; - - JacobianEntrySW m_jacAng[3]; - - real_t m_timeStep; - Transform m_calculatedTransformA; - Transform m_calculatedTransformB; - - Vector3 m_sliderAxis; - Vector3 m_realPivotAInW; - Vector3 m_realPivotBInW; - Vector3 m_projPivotInW; - Vector3 m_delta; - Vector3 m_depth; - Vector3 m_relPosA; - Vector3 m_relPosB; - - real_t m_linPos; - - real_t m_angDepth; - real_t m_kAngle; - - bool m_poweredLinMotor; - real_t m_targetLinMotorVelocity; - real_t m_maxLinMotorForce; - real_t m_accumulatedLinMotorImpulse; - - bool m_poweredAngMotor; - real_t m_targetAngMotorVelocity; - real_t m_maxAngMotorForce; - real_t m_accumulatedAngMotorImpulse; - - //------------------------ - void initParams(); - -public: - // constructors - SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB); - //SliderJointSW(); - // overrides - - // access - const BodySW *getRigidBodyA() const { return A; } - const BodySW *getRigidBodyB() const { return B; } - const Transform &getCalculatedTransformA() const { return m_calculatedTransformA; } - const Transform &getCalculatedTransformB() const { return m_calculatedTransformB; } - const Transform &getFrameOffsetA() const { return m_frameInA; } - const Transform &getFrameOffsetB() const { return m_frameInB; } - Transform &getFrameOffsetA() { return m_frameInA; } - Transform &getFrameOffsetB() { return m_frameInB; } - real_t getLowerLinLimit() { return m_lowerLinLimit; } - void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; } - real_t getUpperLinLimit() { return m_upperLinLimit; } - void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; } - real_t getLowerAngLimit() { return m_lowerAngLimit; } - void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; } - real_t getUpperAngLimit() { return m_upperAngLimit; } - void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; } - - real_t getSoftnessDirLin() { return m_softnessDirLin; } - real_t getRestitutionDirLin() { return m_restitutionDirLin; } - real_t getDampingDirLin() { return m_dampingDirLin; } - real_t getSoftnessDirAng() { return m_softnessDirAng; } - real_t getRestitutionDirAng() { return m_restitutionDirAng; } - real_t getDampingDirAng() { return m_dampingDirAng; } - real_t getSoftnessLimLin() { return m_softnessLimLin; } - real_t getRestitutionLimLin() { return m_restitutionLimLin; } - real_t getDampingLimLin() { return m_dampingLimLin; } - real_t getSoftnessLimAng() { return m_softnessLimAng; } - real_t getRestitutionLimAng() { return m_restitutionLimAng; } - real_t getDampingLimAng() { return m_dampingLimAng; } - real_t getSoftnessOrthoLin() { return m_softnessOrthoLin; } - real_t getRestitutionOrthoLin() { return m_restitutionOrthoLin; } - real_t getDampingOrthoLin() { return m_dampingOrthoLin; } - real_t getSoftnessOrthoAng() { return m_softnessOrthoAng; } - real_t getRestitutionOrthoAng() { return m_restitutionOrthoAng; } - real_t getDampingOrthoAng() { return m_dampingOrthoAng; } - void setSoftnessDirLin(real_t softnessDirLin) { m_softnessDirLin = softnessDirLin; } - void setRestitutionDirLin(real_t restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } - void setDampingDirLin(real_t dampingDirLin) { m_dampingDirLin = dampingDirLin; } - void setSoftnessDirAng(real_t softnessDirAng) { m_softnessDirAng = softnessDirAng; } - void setRestitutionDirAng(real_t restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } - void setDampingDirAng(real_t dampingDirAng) { m_dampingDirAng = dampingDirAng; } - void setSoftnessLimLin(real_t softnessLimLin) { m_softnessLimLin = softnessLimLin; } - void setRestitutionLimLin(real_t restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } - void setDampingLimLin(real_t dampingLimLin) { m_dampingLimLin = dampingLimLin; } - void setSoftnessLimAng(real_t softnessLimAng) { m_softnessLimAng = softnessLimAng; } - void setRestitutionLimAng(real_t restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } - void setDampingLimAng(real_t dampingLimAng) { m_dampingLimAng = dampingLimAng; } - void setSoftnessOrthoLin(real_t softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } - void setRestitutionOrthoLin(real_t restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } - void setDampingOrthoLin(real_t dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } - void setSoftnessOrthoAng(real_t softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } - void setRestitutionOrthoAng(real_t restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } - void setDampingOrthoAng(real_t dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } - void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } - bool getPoweredLinMotor() { return m_poweredLinMotor; } - void setTargetLinMotorVelocity(real_t targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } - real_t getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } - void setMaxLinMotorForce(real_t maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } - real_t getMaxLinMotorForce() { return m_maxLinMotorForce; } - void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } - bool getPoweredAngMotor() { return m_poweredAngMotor; } - void setTargetAngMotorVelocity(real_t targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } - real_t getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } - void setMaxAngMotorForce(real_t maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } - real_t getMaxAngMotorForce() { return m_maxAngMotorForce; } - real_t getLinearPos() { return m_linPos; } - - // access for ODE solver - bool getSolveLinLimit() { return m_solveLinLim; } - real_t getLinDepth() { return m_depth[0]; } - bool getSolveAngLimit() { return m_solveAngLim; } - real_t getAngDepth() { return m_angDepth; } - // shared code used by ODE solver - void calculateTransforms(void); - void testLinLimits(void); - void testAngLimits(void); - // access for PE Solver - Vector3 getAncorInA(void); - Vector3 getAncorInB(void); - - void set_param(PhysicsServer::SliderJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer::SliderJointParam p_param) const; - - bool setup(real_t p_step); - void solve(real_t p_step); - - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_SLIDER; } -}; - -#endif // SLIDER_JOINT_SW_H |