1/**************************************************************************/
2/* godot_pin_joint_3d.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31/*
32Adapted to Godot from the Bullet library.
33*/
34
35/*
36Bullet Continuous Collision Detection and Physics Library
37Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
38
39This software is provided 'as-is', without any express or implied warranty.
40In no event will the authors be held liable for any damages arising from the use of this software.
41Permission is granted to anyone to use this software for any purpose,
42including commercial applications, and to alter it and redistribute it freely,
43subject to the following restrictions:
44
451. 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.
462. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
473. This notice may not be removed or altered from any source distribution.
48*/
49
50#include "godot_pin_joint_3d.h"
51
52bool GodotPinJoint3D::setup(real_t p_step) {
53 dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
54 dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
55
56 if (!dynamic_A && !dynamic_B) {
57 return false;
58 }
59
60 m_appliedImpulse = real_t(0.);
61
62 Vector3 normal(0, 0, 0);
63
64 for (int i = 0; i < 3; i++) {
65 normal[i] = 1;
66 memnew_placement(
67 &m_jac[i],
68 GodotJacobianEntry3D(
69 A->get_principal_inertia_axes().transposed(),
70 B->get_principal_inertia_axes().transposed(),
71 A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(),
72 B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(),
73 normal,
74 A->get_inv_inertia(),
75 A->get_inv_mass(),
76 B->get_inv_inertia(),
77 B->get_inv_mass()));
78 normal[i] = 0;
79 }
80
81 return true;
82}
83
84void GodotPinJoint3D::solve(real_t p_step) {
85 Vector3 pivotAInW = A->get_transform().xform(m_pivotInA);
86 Vector3 pivotBInW = B->get_transform().xform(m_pivotInB);
87
88 Vector3 normal(0, 0, 0);
89
90 //Vector3 angvelA = A->get_transform().origin.getBasis().transpose() * A->getAngularVelocity();
91 //Vector3 angvelB = B->get_transform().origin.getBasis().transpose() * B->getAngularVelocity();
92
93 for (int i = 0; i < 3; i++) {
94 normal[i] = 1;
95 real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
96
97 Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
98 Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
99 //this jacobian entry could be re-used for all iterations
100
101 Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1);
102 Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
103 Vector3 vel = vel1 - vel2;
104
105 real_t rel_vel;
106 rel_vel = normal.dot(vel);
107
108 /*
109 //velocity error (first order error)
110 real_t rel_vel = m_jac[i].getRelativeVelocity(A->getLinearVelocity(),angvelA,
111 B->getLinearVelocity(),angvelB);
112 */
113
114 //positional error (zeroth order error)
115 real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
116
117 real_t impulse = depth * m_tau / p_step * jacDiagABInv - m_damping * rel_vel * jacDiagABInv;
118
119 real_t impulseClamp = m_impulseClamp;
120 if (impulseClamp > 0) {
121 if (impulse < -impulseClamp) {
122 impulse = -impulseClamp;
123 }
124 if (impulse > impulseClamp) {
125 impulse = impulseClamp;
126 }
127 }
128
129 m_appliedImpulse += impulse;
130 Vector3 impulse_vector = normal * impulse;
131 if (dynamic_A) {
132 A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
133 }
134 if (dynamic_B) {
135 B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
136 }
137
138 normal[i] = 0;
139 }
140}
141
142void GodotPinJoint3D::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) {
143 switch (p_param) {
144 case PhysicsServer3D::PIN_JOINT_BIAS:
145 m_tau = p_value;
146 break;
147 case PhysicsServer3D::PIN_JOINT_DAMPING:
148 m_damping = p_value;
149 break;
150 case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP:
151 m_impulseClamp = p_value;
152 break;
153 }
154}
155
156real_t GodotPinJoint3D::get_param(PhysicsServer3D::PinJointParam p_param) const {
157 switch (p_param) {
158 case PhysicsServer3D::PIN_JOINT_BIAS:
159 return m_tau;
160 case PhysicsServer3D::PIN_JOINT_DAMPING:
161 return m_damping;
162 case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP:
163 return m_impulseClamp;
164 }
165
166 return 0;
167}
168
169GodotPinJoint3D::GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b) :
170 GodotJoint3D(_arr, 2) {
171 A = p_body_a;
172 B = p_body_b;
173 m_pivotInA = p_pos_a;
174 m_pivotInB = p_pos_b;
175
176 A->add_constraint(this, 0);
177 B->add_constraint(this, 1);
178}
179
180GodotPinJoint3D::~GodotPinJoint3D() {
181}
182