diff options
Diffstat (limited to 'servers/physics_3d/joints/pin_joint_3d_sw.cpp')
| -rw-r--r-- | servers/physics_3d/joints/pin_joint_3d_sw.cpp | 167 |
1 files changed, 167 insertions, 0 deletions
diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp new file mode 100644 index 0000000000..67f2d78d54 --- /dev/null +++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp @@ -0,0 +1,167 @@ +/*************************************************************************/ +/* 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_3d_sw.h" + +bool PinJoint3DSW::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], JacobianEntry3DSW( + 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 PinJoint3DSW::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 PinJoint3DSW::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) { + + switch (p_param) { + case PhysicsServer3D::PIN_JOINT_BIAS: m_tau = p_value; break; + case PhysicsServer3D::PIN_JOINT_DAMPING: m_damping = p_value; break; + case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP: m_impulseClamp = p_value; break; + } +} + +real_t PinJoint3DSW::get_param(PhysicsServer3D::PinJointParam p_param) const { + + switch (p_param) { + case PhysicsServer3D::PIN_JOINT_BIAS: return m_tau; + case PhysicsServer3D::PIN_JOINT_DAMPING: return m_damping; + case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP: return m_impulseClamp; + } + + return 0; +} + +PinJoint3DSW::PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW *p_body_b, const Vector3 &p_pos_b) : + Joint3DSW(_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); +} + +PinJoint3DSW::~PinJoint3DSW() { +} |