initial commit, 4.5 stable

modules/godot_physics_3d/joints/SCsub

@@ -0,0 +1,6 @@
+#!/usr/bin/env python
+from misc.utility.scons_hints import *
+
+Import("env")
+
+env.add_source_files(env.modules_sources, "*.cpp")

modules/godot_physics_3d/joints/godot_cone_twist_joint_3d.cpp

@@ -0,0 +1,326 @@
+/**************************************************************************/
+/*  godot_cone_twist_joint_3d.cpp                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+/*
+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 "godot_cone_twist_joint_3d.h"
+
+GodotConeTwistJoint3D::GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) :
+		GodotJoint3D(_arr, 2) {
+	A = rbA;
+	B = rbB;
+
+	m_rbAFrame = rbAFrame;
+	m_rbBFrame = rbBFrame;
+
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+}
+
+bool GodotConeTwistJoint3D::setup(real_t p_timestep) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
+	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],
+					GodotJacobianEntry3D(
+							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;
+
+	b1Axis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(0));
+	b2Axis1 = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(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(m_rbAFrame.basis.get_column(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(m_rbAFrame.basis.get_column(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(m_rbBFrame.basis.get_column(1));
+		Quaternion rotationArc = Quaternion(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 GodotConeTwistJoint3D::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;
+			if (dynamic_A) {
+				A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
+			}
+			if (dynamic_B) {
+				B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
+			}
+		}
+	}
+
+	{
+		///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;
+
+			if (dynamic_A) {
+				A->apply_torque_impulse(impulse);
+			}
+			if (dynamic_B) {
+				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;
+
+			if (dynamic_A) {
+				A->apply_torque_impulse(impulse);
+			}
+			if (dynamic_B) {
+				B->apply_torque_impulse(-impulse);
+			}
+		}
+	}
+}
+
+void GodotConeTwistJoint3D::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: {
+			m_swingSpan1 = p_value;
+			m_swingSpan2 = p_value;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: {
+			m_twistSpan = p_value;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: {
+			m_biasFactor = p_value;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: {
+			m_limitSoftness = p_value;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: {
+			m_relaxationFactor = p_value;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_MAX:
+			break; // Can't happen, but silences warning
+	}
+}
+
+real_t GodotConeTwistJoint3D::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: {
+			return m_swingSpan1;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: {
+			return m_twistSpan;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: {
+			return m_biasFactor;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: {
+			return m_limitSoftness;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: {
+			return m_relaxationFactor;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_MAX:
+			break; // Can't happen, but silences warning
+	}
+
+	return 0;
+}

modules/godot_physics_3d/joints/godot_cone_twist_joint_3d.h

@@ -0,0 +1,139 @@
+/**************************************************************************/
+/*  godot_cone_twist_joint_3d.h                                           */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+GodotConeTwistJoint3D 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 "../godot_joint_3d.h"
+#include "godot_jacobian_entry_3d.h"
+
+// GodotConeTwistJoint3D can be used to simulate ragdoll joints (upper arm, leg etc).
+class GodotConeTwistJoint3D : public GodotJoint3D {
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
+
+	union {
+		struct {
+			GodotBody3D *A;
+			GodotBody3D *B;
+		};
+
+		GodotBody3D *_arr[2] = { nullptr, nullptr };
+	};
+
+	GodotJacobianEntry3D m_jac[3] = {}; //3 orthogonal linear constraints
+
+	real_t m_appliedImpulse = 0.0;
+	Transform3D m_rbAFrame;
+	Transform3D m_rbBFrame;
+
+	real_t m_limitSoftness = 0.0;
+	real_t m_biasFactor = 0.3;
+	real_t m_relaxationFactor = 1.0;
+
+	real_t m_swingSpan1 = Math::TAU / 8.0;
+	real_t m_swingSpan2 = 0.0;
+	real_t m_twistSpan = 0.0;
+
+	Vector3 m_swingAxis;
+	Vector3 m_twistAxis;
+
+	real_t m_kSwing = 0.0;
+	real_t m_kTwist = 0.0;
+
+	real_t m_twistLimitSign = 0.0;
+	real_t m_swingCorrection = 0.0;
+	real_t m_twistCorrection = 0.0;
+
+	real_t m_accSwingLimitImpulse = 0.0;
+	real_t m_accTwistLimitImpulse = 0.0;
+
+	bool m_angularOnly = false;
+	bool m_solveTwistLimit = false;
+	bool m_solveSwingLimit = false;
+
+public:
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; }
+
+	virtual bool setup(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &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(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer3D::ConeTwistJointParam p_param) const;
+};

modules/godot_physics_3d/joints/godot_generic_6dof_joint_3d.cpp

@@ -0,0 +1,675 @@
+/**************************************************************************/
+/*  godot_generic_6dof_joint_3d.cpp                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+/*
+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
+GodotGeneric6DOFJoint3D Refactored by Francisco Le?n
+email: projectileman@yahoo.com
+http://gimpact.sf.net
+*/
+
+#include "godot_generic_6dof_joint_3d.h"
+
+#define GENERIC_D6_DISABLE_WARMSTARTING 1
+
+//////////////////////////// GodotG6DOFRotationalLimitMotor3D ////////////////////////////////////
+
+int GodotG6DOFRotationalLimitMotor3D::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 GodotG6DOFRotationalLimitMotor3D::solveAngularLimits(
+		real_t timeStep, Vector3 &axis, real_t jacDiagABInv,
+		GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic) {
+	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;
+
+	if (p_body0_dynamic) {
+		body0->apply_torque_impulse(motorImp);
+	}
+	if (body1 && p_body1_dynamic) {
+		body1->apply_torque_impulse(-motorImp);
+	}
+
+	return clippedMotorImpulse;
+}
+
+//////////////////////////// GodotG6DOFTranslationalLimitMotor3D ////////////////////////////////////
+
+real_t GodotG6DOFTranslationalLimitMotor3D::solveLinearAxis(
+		real_t timeStep,
+		real_t jacDiagABInv,
+		GodotBody3D *body1, const Vector3 &pointInA,
+		GodotBody3D *body2, const Vector3 &pointInB,
+		bool p_body1_dynamic, bool p_body2_dynamic,
+		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;
+	if (p_body1_dynamic) {
+		body1->apply_impulse(impulse_vector, rel_pos1);
+	}
+	if (p_body2_dynamic) {
+		body2->apply_impulse(-impulse_vector, rel_pos2);
+	}
+	return normalImpulse;
+}
+
+//////////////////////////// GodotGeneric6DOFJoint3D ////////////////////////////////////
+
+GodotGeneric6DOFJoint3D::GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA) :
+		GodotJoint3D(_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 GodotGeneric6DOFJoint3D::calculateAngleInfo() {
+	Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis;
+
+	m_calculatedAxisAngleDiff = relative_frame.get_euler(EulerOrder::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_column(0);
+	Vector3 axis2 = m_calculatedTransformA.basis.get_column(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 GodotGeneric6DOFJoint3D::calculateTransforms() {
+	m_calculatedTransformA = A->get_transform() * m_frameInA;
+	m_calculatedTransformB = B->get_transform() * m_frameInB;
+
+	calculateAngleInfo();
+}
+
+void GodotGeneric6DOFJoint3D::buildLinearJacobian(
+		GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld,
+		const Vector3 &pivotAInW, const Vector3 &pivotBInW) {
+	memnew_placement(
+			&jacLinear,
+			GodotJacobianEntry3D(
+					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 GodotGeneric6DOFJoint3D::buildAngularJacobian(
+		GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW) {
+	memnew_placement(
+			&jacAngular,
+			GodotJacobianEntry3D(
+					jointAxisW,
+					A->get_principal_inertia_axes().transposed(),
+					B->get_principal_inertia_axes().transposed(),
+					A->get_inv_inertia(),
+					B->get_inv_inertia()));
+}
+
+bool GodotGeneric6DOFJoint3D::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 GodotGeneric6DOFJoint3D::setup(real_t p_timestep) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
+	// 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_column(i);
+			} else {
+				normalWorld = m_calculatedTransformB.basis.get_column(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 = getAxis(i);
+			// Create angular atom
+			buildAngularJacobian(m_jacAng[i], normalWorld);
+		}
+	}
+
+	return true;
+}
+
+void GodotGeneric6DOFJoint3D::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_column(i);
+			} else {
+				linear_axis = m_calculatedTransformB.basis.get_column(i);
+			}
+
+			m_linearLimits.solveLinearAxis(
+					m_timeStep,
+					jacDiagABInv,
+					A, pointInA,
+					B, pointInB,
+					dynamic_A, dynamic_B,
+					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, dynamic_A, dynamic_B);
+		}
+	}
+}
+
+void GodotGeneric6DOFJoint3D::updateRHS(real_t timeStep) {
+	(void)timeStep;
+}
+
+Vector3 GodotGeneric6DOFJoint3D::getAxis(int axis_index) const {
+	return m_calculatedAxis[axis_index];
+}
+
+real_t GodotGeneric6DOFJoint3D::getAngle(int axis_index) const {
+	return m_calculatedAxisAngleDiff[axis_index];
+}
+
+void GodotGeneric6DOFJoint3D::calcAnchorPos() {
+	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);
+}
+
+void GodotGeneric6DOFJoint3D::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) {
+	ERR_FAIL_INDEX(p_axis, 3);
+	switch (p_param) {
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
+			m_linearLimits.m_lowerLimit[p_axis] = p_value;
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
+			m_linearLimits.m_upperLimit[p_axis] = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
+			m_linearLimits.m_limitSoftness[p_axis] = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: {
+			m_linearLimits.m_restitution[p_axis] = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: {
+			m_linearLimits.m_damping[p_axis] = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
+			m_angularLimits[p_axis].m_loLimit = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
+			m_angularLimits[p_axis].m_hiLimit = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
+			m_angularLimits[p_axis].m_limitSoftness = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: {
+			m_angularLimits[p_axis].m_damping = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: {
+			m_angularLimits[p_axis].m_bounce = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
+			m_angularLimits[p_axis].m_maxLimitForce = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: {
+			m_angularLimits[p_axis].m_ERP = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
+			m_angularLimits[p_axis].m_targetVelocity = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
+			m_angularLimits[p_axis].m_maxLimitForce = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_MAX:
+			break; // Can't happen, but silences warning
+	}
+}
+
+real_t GodotGeneric6DOFJoint3D::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const {
+	ERR_FAIL_INDEX_V(p_axis, 3, 0);
+	switch (p_param) {
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
+			return m_linearLimits.m_lowerLimit[p_axis];
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
+			return m_linearLimits.m_upperLimit[p_axis];
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
+			return m_linearLimits.m_limitSoftness[p_axis];
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: {
+			return m_linearLimits.m_restitution[p_axis];
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: {
+			return m_linearLimits.m_damping[p_axis];
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
+			return m_angularLimits[p_axis].m_loLimit;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
+			return m_angularLimits[p_axis].m_hiLimit;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
+			return m_angularLimits[p_axis].m_limitSoftness;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: {
+			return m_angularLimits[p_axis].m_damping;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: {
+			return m_angularLimits[p_axis].m_bounce;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
+			return m_angularLimits[p_axis].m_maxLimitForce;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: {
+			return m_angularLimits[p_axis].m_ERP;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
+			return m_angularLimits[p_axis].m_targetVelocity;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
+			return m_angularLimits[p_axis].m_maxMotorForce;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_MAX:
+			break; // Can't happen, but silences warning
+	}
+	return 0;
+}
+
+void GodotGeneric6DOFJoint3D::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) {
+	ERR_FAIL_INDEX(p_axis, 3);
+
+	switch (p_flag) {
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
+			m_linearLimits.enable_limit[p_axis] = p_value;
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
+			m_angularLimits[p_axis].m_enableLimit = p_value;
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
+			m_angularLimits[p_axis].m_enableMotor = p_value;
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX:
+			break; // Can't happen, but silences warning
+	}
+}
+
+bool GodotGeneric6DOFJoint3D::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const {
+	ERR_FAIL_INDEX_V(p_axis, 3, false);
+	switch (p_flag) {
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
+			return m_linearLimits.enable_limit[p_axis];
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
+			return m_angularLimits[p_axis].m_enableLimit;
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
+			return m_angularLimits[p_axis].m_enableMotor;
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX:
+			break; // Can't happen, but silences warning
+	}
+
+	return false;
+}

modules/godot_physics_3d/joints/godot_generic_6dof_joint_3d.h

@@ -0,0 +1,319 @@
+/**************************************************************************/
+/*  godot_generic_6dof_joint_3d.h                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+#include "../godot_joint_3d.h"
+#include "godot_jacobian_entry_3d.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
+GodotGeneric6DOFJoint3D Refactored by Francisco Le?n
+email: projectileman@yahoo.com
+http://gimpact.sf.net
+*/
+
+//! Rotation Limit structure for generic joints
+class GodotG6DOFRotationalLimitMotor3D {
+public:
+	//! limit_parameters
+	//!@{
+	real_t m_loLimit = -1e30; //!< joint limit
+	real_t m_hiLimit = 1e30; //!< joint limit
+	real_t m_targetVelocity = 0.0; //!< target motor velocity
+	real_t m_maxMotorForce = 0.1; //!< max force on motor
+	real_t m_maxLimitForce = 300.0; //!< max force on limit
+	real_t m_damping = 1.0; //!< Damping.
+	real_t m_limitSoftness = 0.5; //! Relaxation factor
+	real_t m_ERP = 0.5; //!< Error tolerance factor when joint is at limit
+	real_t m_bounce = 0.0; //!< restitution factor
+	bool m_enableMotor = false;
+	bool m_enableLimit = false;
+
+	//!@}
+
+	//! temp_variables
+	//!@{
+	real_t m_currentLimitError = 0.0; //!< How much is violated this limit
+	int m_currentLimit = 0; //!< 0=free, 1=at lo limit, 2=at hi limit
+	real_t m_accumulatedImpulse = 0.0;
+	//!@}
+
+	GodotG6DOFRotationalLimitMotor3D() {}
+
+	bool isLimited() {
+		return (m_loLimit < m_hiLimit);
+	}
+
+	// Need apply correction.
+	bool needApplyTorques() {
+		return (m_enableMotor || m_currentLimit != 0);
+	}
+
+	// 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, GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic);
+};
+
+class GodotG6DOFTranslationalLimitMotor3D {
+public:
+	Vector3 m_lowerLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint lower limits
+	Vector3 m_upperLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint upper limits
+	Vector3 m_accumulatedImpulse = Vector3(0.0, 0.0, 0.0);
+	//! Linear_Limit_parameters
+	//!@{
+	Vector3 m_limitSoftness = Vector3(0.7, 0.7, 0.7); //!< Softness for linear limit
+	Vector3 m_damping = Vector3(1.0, 1.0, 1.0); //!< Damping for linear limit
+	Vector3 m_restitution = Vector3(0.5, 0.5, 0.5); //! Bounce parameter for linear limit
+	//!@}
+	bool enable_limit[3] = { true, true, true };
+
+	//! 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,
+			GodotBody3D *body1, const Vector3 &pointInA,
+			GodotBody3D *body2, const Vector3 &pointInB,
+			bool p_body1_dynamic, bool p_body2_dynamic,
+			int limit_index,
+			const Vector3 &axis_normal_on_a,
+			const Vector3 &anchorPos);
+};
+
+class GodotGeneric6DOFJoint3D : public GodotJoint3D {
+protected:
+	union {
+		struct {
+			GodotBody3D *A;
+			GodotBody3D *B;
+		};
+
+		GodotBody3D *_arr[2] = { nullptr, nullptr };
+	};
+
+	//! relative_frames
+	//!@{
+	Transform3D m_frameInA; //!< the constraint space w.r.t body A
+	Transform3D m_frameInB; //!< the constraint space w.r.t body B
+	//!@}
+
+	//! Jacobians
+	//!@{
+	GodotJacobianEntry3D m_jacLinear[3]; //!< 3 orthogonal linear constraints
+	GodotJacobianEntry3D m_jacAng[3]; //!< 3 orthogonal angular constraints
+	//!@}
+
+	//! Linear_Limit_parameters
+	//!@{
+	GodotG6DOFTranslationalLimitMotor3D m_linearLimits;
+	//!@}
+
+	//! hinge_parameters
+	//!@{
+	GodotG6DOFRotationalLimitMotor3D m_angularLimits[3];
+	//!@}
+
+protected:
+	//! temporal variables
+	//!@{
+	real_t m_timeStep = 0.0;
+	Transform3D m_calculatedTransformA;
+	Transform3D 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 = false;
+
+	//!@}
+
+	GodotGeneric6DOFJoint3D(GodotGeneric6DOFJoint3D const &) = delete;
+	void operator=(GodotGeneric6DOFJoint3D const &) = delete;
+
+	void buildLinearJacobian(
+			GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld,
+			const Vector3 &pivotAInW, const Vector3 &pivotBInW);
+
+	void buildAngularJacobian(GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW);
+
+	//! calcs the euler angles between the two bodies.
+	void calculateAngleInfo();
+
+public:
+	GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA);
+
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; }
+
+	virtual bool setup(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	// Calcs the global transform for the joint offset for body A an B, and also calcs the angle differences between the bodies.
+	void calculateTransforms();
+
+	// Gets the global transform of the offset for body A.
+	const Transform3D &getCalculatedTransformA() const {
+		return m_calculatedTransformA;
+	}
+
+	// Gets the global transform of the offset for body B.
+	const Transform3D &getCalculatedTransformB() const {
+		return m_calculatedTransformB;
+	}
+
+	const Transform3D &getFrameOffsetA() const {
+		return m_frameInA;
+	}
+
+	const Transform3D &getFrameOffsetB() const {
+		return m_frameInB;
+	}
+
+	Transform3D &getFrameOffsetA() {
+		return m_frameInA;
+	}
+
+	Transform3D &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.
+	Vector3 getAxis(int axis_index) const;
+
+	// Get the relative Euler angle.
+	real_t getAngle(int axis_index) const;
+
+	// Calculates angular correction and returns true if limit needs to be corrected.
+	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 information.
+	GodotG6DOFRotationalLimitMotor3D *getRotationalLimitMotor(int index) {
+		return &m_angularLimits[index];
+	}
+
+	// Retrieves the limit information.
+	GodotG6DOFTranslationalLimitMotor3D *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 GodotBody3D *getRigidBodyA() const {
+		return A;
+	}
+	const GodotBody3D *getRigidBodyB() const {
+		return B;
+	}
+
+	virtual void calcAnchorPos(); // overridable
+
+	void set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value);
+	real_t get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const;
+
+	void set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value);
+	bool get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const;
+};

modules/godot_physics_3d/joints/godot_hinge_joint_3d.cpp

@@ -0,0 +1,441 @@
+/**************************************************************************/
+/*  godot_hinge_joint_3d.cpp                                              */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+/*
+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 "godot_hinge_joint_3d.h"
+
+GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB) :
+		GodotJoint3D(_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.);
+
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+}
+
+GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB,
+		const Vector3 &axisInA, const Vector3 &axisInB) :
+		GodotJoint3D(_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_column(0);
+
+	Vector3 rbAxisA2;
+	real_t projection = axisInA.dot(rbAxisA1);
+	if (projection >= 1.0f - CMP_EPSILON) {
+		rbAxisA1 = -rbA->get_transform().basis.get_column(2);
+		rbAxisA2 = rbA->get_transform().basis.get_column(1);
+	} else if (projection <= -1.0f + CMP_EPSILON) {
+		rbAxisA1 = rbA->get_transform().basis.get_column(2);
+		rbAxisA2 = rbA->get_transform().basis.get_column(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);
+
+	Quaternion rotationArc = Quaternion(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);
+
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+}
+
+bool GodotHingeJoint3D::setup(real_t p_step) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
+	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],
+					GodotJacobianEntry3D(
+							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_column(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_column(2));
+
+	memnew_placement(
+			&m_jacAng[0],
+			GodotJacobianEntry3D(
+					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],
+			GodotJacobianEntry3D(
+					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],
+			GodotJacobianEntry3D(
+					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_useLimit && m_lowerLimit <= m_upperLimit) {
+		if (hingeAngle <= m_lowerLimit) {
+			m_correction = (m_lowerLimit - hingeAngle);
+			m_limitSign = 1.0f;
+			m_solveLimit = true;
+		} 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_column(2));
+	m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA));
+
+	return true;
+}
+
+void GodotHingeJoint3D::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;
+			if (dynamic_A) {
+				A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
+			}
+			if (dynamic_B) {
+				B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
+			}
+		}
+	}
+
+	{
+		///solve angular part
+
+		// get axes in world space
+		Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2));
+		Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(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;
+			}
+
+			if (dynamic_A) {
+				A->apply_torque_impulse(-velrelOrthog + angularError);
+			}
+			if (dynamic_B) {
+				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;
+				if (dynamic_A) {
+					A->apply_torque_impulse(impulse);
+				}
+				if (dynamic_B) {
+					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;
+
+			if (dynamic_A) {
+				A->apply_torque_impulse(motorImp + angularLimit);
+			}
+			if (dynamic_B) {
+				B->apply_torque_impulse(-motorImp - angularLimit);
+			}
+		}
+	}
+}
+
+/*
+void	HingeJointSW::updateRHS(real_t	timeStep)
+{
+	(void)timeStep;
+}
+
+*/
+
+real_t GodotHingeJoint3D::get_hinge_angle() {
+	const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(0));
+	const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(1));
+	const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(1));
+
+	return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+}
+
+void GodotHingeJoint3D::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::HINGE_JOINT_BIAS:
+			tau = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER:
+			m_upperLimit = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER:
+			m_lowerLimit = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS:
+			m_biasFactor = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS:
+			m_limitSoftness = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION:
+			m_relaxationFactor = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY:
+			m_motorTargetVelocity = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE:
+			m_maxMotorImpulse = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_MAX:
+			break; // Can't happen, but silences warning
+	}
+}
+
+real_t GodotHingeJoint3D::get_param(PhysicsServer3D::HingeJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::HINGE_JOINT_BIAS:
+			return tau;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER:
+			return m_upperLimit;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER:
+			return m_lowerLimit;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS:
+			return m_biasFactor;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS:
+			return m_limitSoftness;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION:
+			return m_relaxationFactor;
+		case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY:
+			return m_motorTargetVelocity;
+		case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE:
+			return m_maxMotorImpulse;
+		case PhysicsServer3D::HINGE_JOINT_MAX:
+			break; // Can't happen, but silences warning
+	}
+
+	return 0;
+}
+
+void GodotHingeJoint3D::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) {
+	switch (p_flag) {
+		case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT:
+			m_useLimit = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR:
+			m_enableAngularMotor = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_FLAG_MAX:
+			break; // Can't happen, but silences warning
+	}
+}
+
+bool GodotHingeJoint3D::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const {
+	switch (p_flag) {
+		case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT:
+			return m_useLimit;
+		case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR:
+			return m_enableAngularMotor;
+		case PhysicsServer3D::HINGE_JOINT_FLAG_MAX:
+			break; // Can't happen, but silences warning
+	}
+
+	return false;
+}

modules/godot_physics_3d/joints/godot_hinge_joint_3d.h

@@ -0,0 +1,113 @@
+/**************************************************************************/
+/*  godot_hinge_joint_3d.h                                                */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+#include "../godot_joint_3d.h"
+#include "godot_jacobian_entry_3d.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 GodotHingeJoint3D : public GodotJoint3D {
+	union {
+		struct {
+			GodotBody3D *A;
+			GodotBody3D *B;
+		};
+
+		GodotBody3D *_arr[2] = {};
+	};
+
+	GodotJacobianEntry3D m_jac[3]; //3 orthogonal linear constraints
+	GodotJacobianEntry3D m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
+
+	Transform3D m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	Transform3D m_rbBFrame;
+
+	real_t m_motorTargetVelocity = 0.0;
+	real_t m_maxMotorImpulse = 0.0;
+
+	real_t m_limitSoftness = 0.9;
+	real_t m_biasFactor = 0.3;
+	real_t m_relaxationFactor = 1.0;
+
+	real_t m_lowerLimit = Math::PI;
+	real_t m_upperLimit = -Math::PI;
+
+	real_t m_kHinge = 0.0;
+
+	real_t m_limitSign = 0.0;
+	real_t m_correction = 0.0;
+
+	real_t m_accLimitImpulse = 0.0;
+
+	real_t tau = 0.3;
+
+	bool m_useLimit = false;
+	bool m_angularOnly = false;
+	bool m_enableAngularMotor = false;
+	bool m_solveLimit = false;
+
+	real_t m_appliedImpulse = 0.0;
+
+public:
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; }
+
+	virtual bool setup(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	real_t get_hinge_angle();
+
+	void set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer3D::HingeJointParam p_param) const;
+
+	void set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value);
+	bool get_flag(PhysicsServer3D::HingeJointFlag p_flag) const;
+
+	GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB);
+	GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB);
+};

modules/godot_physics_3d/joints/godot_jacobian_entry_3d.h

@@ -0,0 +1,166 @@
+/**************************************************************************/
+/*  godot_jacobian_entry_3d.h                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+/*
+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 "core/math/transform_3d.h"
+
+class GodotJacobianEntry3D {
+public:
+	GodotJacobianEntry3D() {}
+	//constraint between two different rigidbodies
+	GodotJacobianEntry3D(
+			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
+	GodotJacobianEntry3D(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
+	GodotJacobianEntry3D(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
+	GodotJacobianEntry3D(
+			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 GodotJacobianEntry3D &jacB, const real_t massInvA) const {
+		const GodotJacobianEntry3D &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 GodotJacobianEntry3D &jacB, const real_t massInvA, const real_t massInvB) const {
+		const GodotJacobianEntry3D &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 = 1.0;
+};

modules/godot_physics_3d/joints/godot_pin_joint_3d.cpp

@@ -0,0 +1,181 @@
+/**************************************************************************/
+/*  godot_pin_joint_3d.cpp                                                */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+/*
+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 "godot_pin_joint_3d.h"
+
+bool GodotPinJoint3D::setup(real_t p_step) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
+	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],
+				GodotJacobianEntry3D(
+						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 GodotPinJoint3D::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 reused 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;
+		if (dynamic_A) {
+			A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
+		}
+		if (dynamic_B) {
+			B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
+		}
+
+		normal[i] = 0;
+	}
+}
+
+void GodotPinJoint3D::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 GodotPinJoint3D::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;
+}
+
+GodotPinJoint3D::GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b) :
+		GodotJoint3D(_arr, 2) {
+	A = p_body_a;
+	B = p_body_b;
+	m_pivotInA = p_pos_a;
+	m_pivotInB = p_pos_b;
+
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+}
+
+GodotPinJoint3D::~GodotPinJoint3D() {
+}

modules/godot_physics_3d/joints/godot_pin_joint_3d.h

@@ -0,0 +1,92 @@
+/**************************************************************************/
+/*  godot_pin_joint_3d.h                                                  */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+#include "../godot_joint_3d.h"
+#include "godot_jacobian_entry_3d.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 GodotPinJoint3D : public GodotJoint3D {
+	union {
+		struct {
+			GodotBody3D *A;
+			GodotBody3D *B;
+		};
+
+		GodotBody3D *_arr[2] = {};
+	};
+
+	real_t m_tau = 0.3; //bias
+	real_t m_damping = 1.0;
+	real_t m_impulseClamp = 0.0;
+	real_t m_appliedImpulse = 0.0;
+
+	GodotJacobianEntry3D m_jac[3] = {}; //3 orthogonal linear constraints
+
+	Vector3 m_pivotInA;
+	Vector3 m_pivotInB;
+
+public:
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_PIN; }
+
+	virtual bool setup(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer3D::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; }
+
+	GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b);
+	~GodotPinJoint3D();
+};

modules/godot_physics_3d/joints/godot_slider_joint_3d.cpp

@@ -0,0 +1,478 @@
+/**************************************************************************/
+/*  godot_slider_joint_3d.cpp                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+/*
+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 "godot_slider_joint_3d.h"
+
+//-----------------------------------------------------------------------------
+
+GodotSliderJoint3D::GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB) :
+		GodotJoint3D(_arr, 2),
+		m_frameInA(frameInA),
+		m_frameInB(frameInB) {
+	A = rbA;
+	B = rbB;
+
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+}
+
+//-----------------------------------------------------------------------------
+
+bool GodotSliderJoint3D::setup(real_t p_step) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
+	//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_column(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_column(i);
+		memnew_placement(
+				&m_jacLin[i],
+				GodotJacobianEntry3D(
+						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_column(i);
+		memnew_placement(
+				&m_jacAng[i],
+				GodotJacobianEntry3D(
+						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_column(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;
+}
+
+//-----------------------------------------------------------------------------
+
+void GodotSliderJoint3D::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);
+		// Calculate and apply impulse.
+		real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i];
+		Vector3 impulse_vector = normal * normalImpulse;
+		if (dynamic_A) {
+			A->apply_impulse(impulse_vector, m_relPosA);
+		}
+		if (dynamic_B) {
+			B->apply_impulse(-impulse_vector, m_relPosB);
+		}
+		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;
+				if (dynamic_A) {
+					A->apply_impulse(impulse_vector, m_relPosA);
+				}
+				if (dynamic_B) {
+					B->apply_impulse(-impulse_vector, m_relPosB);
+				}
+			}
+		}
+	}
+	// angular
+	// get axes in world space
+	Vector3 axisA = m_calculatedTransformA.basis.get_column(0);
+	Vector3 axisB = m_calculatedTransformB.basis.get_column(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
+	if (dynamic_A) {
+		A->apply_torque_impulse(-velrelOrthog + angularError);
+	}
+	if (dynamic_B) {
+		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;
+	if (dynamic_A) {
+		A->apply_torque_impulse(impulse);
+	}
+	if (dynamic_B) {
+		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;
+			if (dynamic_A) {
+				A->apply_torque_impulse(motorImp);
+			}
+			if (dynamic_B) {
+				B->apply_torque_impulse(-motorImp);
+			}
+		}
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+void GodotSliderJoint3D::calculateTransforms() {
+	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_column(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_column(i);
+		m_depth[i] = m_delta.dot(normalWorld);
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+void GodotSliderJoint3D::testLinLimits() {
+	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.);
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+void GodotSliderJoint3D::testAngLimits() {
+	m_angDepth = real_t(0.);
+	m_solveAngLim = false;
+	if (m_lowerAngLimit <= m_upperAngLimit) {
+		const Vector3 axisA0 = m_calculatedTransformA.basis.get_column(1);
+		const Vector3 axisA1 = m_calculatedTransformA.basis.get_column(2);
+		const Vector3 axisB0 = m_calculatedTransformB.basis.get_column(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;
+		}
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+Vector3 GodotSliderJoint3D::getAncorInA() {
+	Vector3 ancorInA;
+	ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis;
+	ancorInA = A->get_transform().inverse().xform(ancorInA);
+	return ancorInA;
+}
+
+//-----------------------------------------------------------------------------
+
+Vector3 GodotSliderJoint3D::getAncorInB() {
+	Vector3 ancorInB;
+	ancorInB = m_frameInB.origin;
+	return ancorInB;
+}
+
+void GodotSliderJoint3D::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
+			m_upperLinLimit = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER:
+			m_lowerLinLimit = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS:
+			m_softnessLimLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION:
+			m_restitutionLimLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING:
+			m_dampingLimLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS:
+			m_softnessDirLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION:
+			m_restitutionDirLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING:
+			m_dampingDirLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS:
+			m_softnessOrthoLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION:
+			m_restitutionOrthoLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING:
+			m_dampingOrthoLin = p_value;
+			break;
+
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER:
+			m_upperAngLimit = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER:
+			m_lowerAngLimit = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS:
+			m_softnessLimAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION:
+			m_restitutionLimAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING:
+			m_dampingLimAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS:
+			m_softnessDirAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION:
+			m_restitutionDirAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING:
+			m_dampingDirAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS:
+			m_softnessOrthoAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION:
+			m_restitutionOrthoAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING:
+			m_dampingOrthoAng = p_value;
+			break;
+
+		case PhysicsServer3D::SLIDER_JOINT_MAX:
+			break; // Can't happen, but silences warning
+	}
+}
+
+real_t GodotSliderJoint3D::get_param(PhysicsServer3D::SliderJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
+			return m_upperLinLimit;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER:
+			return m_lowerLinLimit;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS:
+			return m_softnessLimLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION:
+			return m_restitutionLimLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING:
+			return m_dampingLimLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS:
+			return m_softnessDirLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION:
+			return m_restitutionDirLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING:
+			return m_dampingDirLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS:
+			return m_softnessOrthoLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION:
+			return m_restitutionOrthoLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING:
+			return m_dampingOrthoLin;
+
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER:
+			return m_upperAngLimit;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER:
+			return m_lowerAngLimit;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS:
+			return m_softnessLimAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION:
+			return m_restitutionLimAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING:
+			return m_dampingLimAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS:
+			return m_softnessDirAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION:
+			return m_restitutionDirAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING:
+			return m_dampingDirAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS:
+			return m_softnessOrthoAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION:
+			return m_restitutionOrthoAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING:
+			return m_dampingOrthoAng;
+
+		case PhysicsServer3D::SLIDER_JOINT_MAX:
+			break; // Can't happen, but silences warning
+	}
+
+	return 0;
+}

modules/godot_physics_3d/joints/godot_slider_joint_3d.h

@@ -0,0 +1,243 @@
+/**************************************************************************/
+/*  godot_slider_joint_3d.h                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+#include "../godot_joint_3d.h"
+#include "godot_jacobian_entry_3d.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 GodotSliderJoint3D : public GodotJoint3D {
+protected:
+	union {
+		struct {
+			GodotBody3D *A;
+			GodotBody3D *B;
+		};
+
+		GodotBody3D *_arr[2] = { nullptr, nullptr };
+	};
+
+	Transform3D m_frameInA;
+	Transform3D m_frameInB;
+
+	// linear limits
+	real_t m_lowerLinLimit = 1.0;
+	real_t m_upperLinLimit = -1.0;
+	// angular limits
+	real_t m_lowerAngLimit = 0.0;
+	real_t m_upperAngLimit = 0.0;
+	// 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 = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingDirLin = 0.0;
+	real_t m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingDirAng = 0.0;
+	real_t m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+	real_t m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+	real_t m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+	real_t m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+
+	// for interlal use
+	bool m_solveLinLim = false;
+	bool m_solveAngLim = false;
+
+	GodotJacobianEntry3D m_jacLin[3] = {};
+	real_t m_jacLinDiagABInv[3] = {};
+
+	GodotJacobianEntry3D m_jacAng[3] = {};
+
+	real_t m_timeStep = 0.0;
+	Transform3D m_calculatedTransformA;
+	Transform3D 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 = 0.0;
+
+	real_t m_angDepth = 0.0;
+	real_t m_kAngle = 0.0;
+
+	bool m_poweredLinMotor = false;
+	real_t m_targetLinMotorVelocity = 0.0;
+	real_t m_maxLinMotorForce = 0.0;
+	real_t m_accumulatedLinMotorImpulse = 0.0;
+
+	bool m_poweredAngMotor = false;
+	real_t m_targetAngMotorVelocity = 0.0;
+	real_t m_maxAngMotorForce = 0.0;
+	real_t m_accumulatedAngMotorImpulse = 0.0;
+
+public:
+	// constructors
+	GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB);
+	//SliderJointSW();
+	// overrides
+
+	// access
+	const GodotBody3D *getRigidBodyA() const { return A; }
+	const GodotBody3D *getRigidBodyB() const { return B; }
+	const Transform3D &getCalculatedTransformA() const { return m_calculatedTransformA; }
+	const Transform3D &getCalculatedTransformB() const { return m_calculatedTransformB; }
+	const Transform3D &getFrameOffsetA() const { return m_frameInA; }
+	const Transform3D &getFrameOffsetB() const { return m_frameInB; }
+	Transform3D &getFrameOffsetA() { return m_frameInA; }
+	Transform3D &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 testLinLimits();
+	void testAngLimits();
+	// access for PE Solver
+	Vector3 getAncorInA();
+	Vector3 getAncorInB();
+
+	void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer3D::SliderJointParam p_param) const;
+
+	virtual bool setup(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; }
+};