1/**************************************************************************/
2/* godot_hinge_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_hinge_joint_3d.h"
51
52GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB) :
53 GodotJoint3D(_arr, 2) {
54 A = rbA;
55 B = rbB;
56
57 m_rbAFrame = frameA;
58 m_rbBFrame = frameB;
59 // flip axis
60 m_rbBFrame.basis[0][2] *= real_t(-1.);
61 m_rbBFrame.basis[1][2] *= real_t(-1.);
62 m_rbBFrame.basis[2][2] *= real_t(-1.);
63
64 A->add_constraint(this, 0);
65 B->add_constraint(this, 1);
66}
67
68GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB,
69 const Vector3 &axisInA, const Vector3 &axisInB) :
70 GodotJoint3D(_arr, 2) {
71 A = rbA;
72 B = rbB;
73
74 m_rbAFrame.origin = pivotInA;
75
76 // since no frame is given, assume this to be zero angle and just pick rb transform axis
77 Vector3 rbAxisA1 = rbA->get_transform().basis.get_column(0);
78
79 Vector3 rbAxisA2;
80 real_t projection = axisInA.dot(rbAxisA1);
81 if (projection >= 1.0f - CMP_EPSILON) {
82 rbAxisA1 = -rbA->get_transform().basis.get_column(2);
83 rbAxisA2 = rbA->get_transform().basis.get_column(1);
84 } else if (projection <= -1.0f + CMP_EPSILON) {
85 rbAxisA1 = rbA->get_transform().basis.get_column(2);
86 rbAxisA2 = rbA->get_transform().basis.get_column(1);
87 } else {
88 rbAxisA2 = axisInA.cross(rbAxisA1);
89 rbAxisA1 = rbAxisA2.cross(axisInA);
90 }
91
92 m_rbAFrame.basis = Basis(rbAxisA1.x, rbAxisA2.x, axisInA.x,
93 rbAxisA1.y, rbAxisA2.y, axisInA.y,
94 rbAxisA1.z, rbAxisA2.z, axisInA.z);
95
96 Quaternion rotationArc = Quaternion(axisInA, axisInB);
97 Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1);
98 Vector3 rbAxisB2 = axisInB.cross(rbAxisB1);
99
100 m_rbBFrame.origin = pivotInB;
101 m_rbBFrame.basis = Basis(rbAxisB1.x, rbAxisB2.x, -axisInB.x,
102 rbAxisB1.y, rbAxisB2.y, -axisInB.y,
103 rbAxisB1.z, rbAxisB2.z, -axisInB.z);
104
105 A->add_constraint(this, 0);
106 B->add_constraint(this, 1);
107}
108
109bool GodotHingeJoint3D::setup(real_t p_step) {
110 dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
111 dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
112
113 if (!dynamic_A && !dynamic_B) {
114 return false;
115 }
116
117 m_appliedImpulse = real_t(0.);
118
119 if (!m_angularOnly) {
120 Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
121 Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
122 Vector3 relPos = pivotBInW - pivotAInW;
123
124 Vector3 normal[3];
125 if (Math::is_zero_approx(relPos.length_squared())) {
126 normal[0] = Vector3(real_t(1.0), 0, 0);
127 } else {
128 normal[0] = relPos.normalized();
129 }
130
131 plane_space(normal[0], normal[1], normal[2]);
132
133 for (int i = 0; i < 3; i++) {
134 memnew_placement(
135 &m_jac[i],
136 GodotJacobianEntry3D(
137 A->get_principal_inertia_axes().transposed(),
138 B->get_principal_inertia_axes().transposed(),
139 pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
140 pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
141 normal[i],
142 A->get_inv_inertia(),
143 A->get_inv_mass(),
144 B->get_inv_inertia(),
145 B->get_inv_mass()));
146 }
147 }
148
149 //calculate two perpendicular jointAxis, orthogonal to hingeAxis
150 //these two jointAxis require equal angular velocities for both bodies
151
152 //this is unused for now, it's a todo
153 Vector3 jointAxis0local;
154 Vector3 jointAxis1local;
155
156 plane_space(m_rbAFrame.basis.get_column(2), jointAxis0local, jointAxis1local);
157
158 Vector3 jointAxis0 = A->get_transform().basis.xform(jointAxis0local);
159 Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local);
160 Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2));
161
162 memnew_placement(
163 &m_jacAng[0],
164 GodotJacobianEntry3D(
165 jointAxis0,
166 A->get_principal_inertia_axes().transposed(),
167 B->get_principal_inertia_axes().transposed(),
168 A->get_inv_inertia(),
169 B->get_inv_inertia()));
170
171 memnew_placement(
172 &m_jacAng[1],
173 GodotJacobianEntry3D(
174 jointAxis1,
175 A->get_principal_inertia_axes().transposed(),
176 B->get_principal_inertia_axes().transposed(),
177 A->get_inv_inertia(),
178 B->get_inv_inertia()));
179
180 memnew_placement(
181 &m_jacAng[2],
182 GodotJacobianEntry3D(
183 hingeAxisWorld,
184 A->get_principal_inertia_axes().transposed(),
185 B->get_principal_inertia_axes().transposed(),
186 A->get_inv_inertia(),
187 B->get_inv_inertia()));
188
189 // Compute limit information
190 real_t hingeAngle = get_hinge_angle();
191
192 //set bias, sign, clear accumulator
193 m_correction = real_t(0.);
194 m_limitSign = real_t(0.);
195 m_solveLimit = false;
196 m_accLimitImpulse = real_t(0.);
197
198 if (m_useLimit && m_lowerLimit <= m_upperLimit) {
199 if (hingeAngle <= m_lowerLimit) {
200 m_correction = (m_lowerLimit - hingeAngle);
201 m_limitSign = 1.0f;
202 m_solveLimit = true;
203 } else if (hingeAngle >= m_upperLimit) {
204 m_correction = m_upperLimit - hingeAngle;
205 m_limitSign = -1.0f;
206 m_solveLimit = true;
207 }
208 }
209
210 //Compute K = J*W*J' for hinge axis
211 Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2));
212 m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA));
213
214 return true;
215}
216
217void GodotHingeJoint3D::solve(real_t p_step) {
218 Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
219 Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
220
221 //real_t tau = real_t(0.3);
222
223 //linear part
224 if (!m_angularOnly) {
225 Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
226 Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
227
228 Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1);
229 Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
230 Vector3 vel = vel1 - vel2;
231
232 for (int i = 0; i < 3; i++) {
233 const Vector3 &normal = m_jac[i].m_linearJointAxis;
234 real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
235
236 real_t rel_vel;
237 rel_vel = normal.dot(vel);
238 //positional error (zeroth order error)
239 real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
240 real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv;
241 m_appliedImpulse += impulse;
242 Vector3 impulse_vector = normal * impulse;
243 if (dynamic_A) {
244 A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
245 }
246 if (dynamic_B) {
247 B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
248 }
249 }
250 }
251
252 {
253 ///solve angular part
254
255 // get axes in world space
256 Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2));
257 Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(2));
258
259 const Vector3 &angVelA = A->get_angular_velocity();
260 const Vector3 &angVelB = B->get_angular_velocity();
261
262 Vector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
263 Vector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
264
265 Vector3 angAorthog = angVelA - angVelAroundHingeAxisA;
266 Vector3 angBorthog = angVelB - angVelAroundHingeAxisB;
267 Vector3 velrelOrthog = angAorthog - angBorthog;
268 {
269 //solve orthogonal angular velocity correction
270 real_t relaxation = real_t(1.);
271 real_t len = velrelOrthog.length();
272 if (len > real_t(0.00001)) {
273 Vector3 normal = velrelOrthog.normalized();
274 real_t denom = A->compute_angular_impulse_denominator(normal) +
275 B->compute_angular_impulse_denominator(normal);
276 // scale for mass and relaxation
277 velrelOrthog *= (real_t(1.) / denom) * m_relaxationFactor;
278 }
279
280 //solve angular positional correction
281 Vector3 angularError = -axisA.cross(axisB) * (real_t(1.) / p_step);
282 real_t len2 = angularError.length();
283 if (len2 > real_t(0.00001)) {
284 Vector3 normal2 = angularError.normalized();
285 real_t denom2 = A->compute_angular_impulse_denominator(normal2) +
286 B->compute_angular_impulse_denominator(normal2);
287 angularError *= (real_t(1.) / denom2) * relaxation;
288 }
289
290 if (dynamic_A) {
291 A->apply_torque_impulse(-velrelOrthog + angularError);
292 }
293 if (dynamic_B) {
294 B->apply_torque_impulse(velrelOrthog - angularError);
295 }
296
297 // solve limit
298 if (m_solveLimit) {
299 real_t amplitude = ((angVelB - angVelA).dot(axisA) * m_relaxationFactor + m_correction * (real_t(1.) / p_step) * m_biasFactor) * m_limitSign;
300
301 real_t impulseMag = amplitude * m_kHinge;
302
303 // Clamp the accumulated impulse
304 real_t temp = m_accLimitImpulse;
305 m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0));
306 impulseMag = m_accLimitImpulse - temp;
307
308 Vector3 impulse = axisA * impulseMag * m_limitSign;
309 if (dynamic_A) {
310 A->apply_torque_impulse(impulse);
311 }
312 if (dynamic_B) {
313 B->apply_torque_impulse(-impulse);
314 }
315 }
316 }
317
318 //apply motor
319 if (m_enableAngularMotor) {
320 //todo: add limits too
321 Vector3 angularLimit(0, 0, 0);
322
323 Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
324 real_t projRelVel = velrel.dot(axisA);
325
326 real_t desiredMotorVel = m_motorTargetVelocity;
327 real_t motor_relvel = desiredMotorVel - projRelVel;
328
329 real_t unclippedMotorImpulse = m_kHinge * motor_relvel;
330 //todo: should clip against accumulated impulse
331 real_t clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
332 clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
333 Vector3 motorImp = clippedMotorImpulse * axisA;
334
335 if (dynamic_A) {
336 A->apply_torque_impulse(motorImp + angularLimit);
337 }
338 if (dynamic_B) {
339 B->apply_torque_impulse(-motorImp - angularLimit);
340 }
341 }
342 }
343}
344
345/*
346void HingeJointSW::updateRHS(real_t timeStep)
347{
348 (void)timeStep;
349}
350
351*/
352
353real_t GodotHingeJoint3D::get_hinge_angle() {
354 const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(0));
355 const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(1));
356 const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(1));
357
358 return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
359}
360
361void GodotHingeJoint3D::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) {
362 switch (p_param) {
363 case PhysicsServer3D::HINGE_JOINT_BIAS:
364 tau = p_value;
365 break;
366 case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER:
367 m_upperLimit = p_value;
368 break;
369 case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER:
370 m_lowerLimit = p_value;
371 break;
372 case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS:
373 m_biasFactor = p_value;
374 break;
375 case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS:
376 m_limitSoftness = p_value;
377 break;
378 case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION:
379 m_relaxationFactor = p_value;
380 break;
381 case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY:
382 m_motorTargetVelocity = p_value;
383 break;
384 case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE:
385 m_maxMotorImpulse = p_value;
386 break;
387 case PhysicsServer3D::HINGE_JOINT_MAX:
388 break; // Can't happen, but silences warning
389 }
390}
391
392real_t GodotHingeJoint3D::get_param(PhysicsServer3D::HingeJointParam p_param) const {
393 switch (p_param) {
394 case PhysicsServer3D::HINGE_JOINT_BIAS:
395 return tau;
396 case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER:
397 return m_upperLimit;
398 case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER:
399 return m_lowerLimit;
400 case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS:
401 return m_biasFactor;
402 case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS:
403 return m_limitSoftness;
404 case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION:
405 return m_relaxationFactor;
406 case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY:
407 return m_motorTargetVelocity;
408 case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE:
409 return m_maxMotorImpulse;
410 case PhysicsServer3D::HINGE_JOINT_MAX:
411 break; // Can't happen, but silences warning
412 }
413
414 return 0;
415}
416
417void GodotHingeJoint3D::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) {
418 switch (p_flag) {
419 case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT:
420 m_useLimit = p_value;
421 break;
422 case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR:
423 m_enableAngularMotor = p_value;
424 break;
425 case PhysicsServer3D::HINGE_JOINT_FLAG_MAX:
426 break; // Can't happen, but silences warning
427 }
428}
429
430bool GodotHingeJoint3D::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const {
431 switch (p_flag) {
432 case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT:
433 return m_useLimit;
434 case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR:
435 return m_enableAngularMotor;
436 case PhysicsServer3D::HINGE_JOINT_FLAG_MAX:
437 break; // Can't happen, but silences warning
438 }
439
440 return false;
441}
442