1/**************************************************************************/
2/* godot_generic_6dof_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/*
512007-09-09
52GodotGeneric6DOFJoint3D Refactored by Francisco Le?n
53email: projectileman@yahoo.com
54http://gimpact.sf.net
55*/
56
57#include "godot_generic_6dof_joint_3d.h"
58
59#define GENERIC_D6_DISABLE_WARMSTARTING 1
60
61//////////////////////////// GodotG6DOFRotationalLimitMotor3D ////////////////////////////////////
62
63int GodotG6DOFRotationalLimitMotor3D::testLimitValue(real_t test_value) {
64 if (m_loLimit > m_hiLimit) {
65 m_currentLimit = 0; //Free from violation
66 return 0;
67 }
68
69 if (test_value < m_loLimit) {
70 m_currentLimit = 1; //low limit violation
71 m_currentLimitError = test_value - m_loLimit;
72 return 1;
73 } else if (test_value > m_hiLimit) {
74 m_currentLimit = 2; //High limit violation
75 m_currentLimitError = test_value - m_hiLimit;
76 return 2;
77 };
78
79 m_currentLimit = 0; //Free from violation
80 return 0;
81}
82
83real_t GodotG6DOFRotationalLimitMotor3D::solveAngularLimits(
84 real_t timeStep, Vector3 &axis, real_t jacDiagABInv,
85 GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic) {
86 if (!needApplyTorques()) {
87 return 0.0f;
88 }
89
90 real_t target_velocity = m_targetVelocity;
91 real_t maxMotorForce = m_maxMotorForce;
92
93 //current error correction
94 if (m_currentLimit != 0) {
95 target_velocity = -m_ERP * m_currentLimitError / (timeStep);
96 maxMotorForce = m_maxLimitForce;
97 }
98
99 maxMotorForce *= timeStep;
100
101 // current velocity difference
102 Vector3 vel_diff = body0->get_angular_velocity();
103 if (body1) {
104 vel_diff -= body1->get_angular_velocity();
105 }
106
107 real_t rel_vel = axis.dot(vel_diff);
108
109 // correction velocity
110 real_t motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);
111
112 if (Math::is_zero_approx(motor_relvel)) {
113 return 0.0f; //no need for applying force
114 }
115
116 // correction impulse
117 real_t unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;
118
119 // clip correction impulse
120 real_t clippedMotorImpulse;
121
122 ///@todo: should clip against accumulated impulse
123 if (unclippedMotorImpulse > 0.0f) {
124 clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
125 } else {
126 clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
127 }
128
129 // sort with accumulated impulses
130 real_t lo = real_t(-1e30);
131 real_t hi = real_t(1e30);
132
133 real_t oldaccumImpulse = m_accumulatedImpulse;
134 real_t sum = oldaccumImpulse + clippedMotorImpulse;
135 m_accumulatedImpulse = sum > hi ? real_t(0.) : (sum < lo ? real_t(0.) : sum);
136
137 clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
138
139 Vector3 motorImp = clippedMotorImpulse * axis;
140
141 if (p_body0_dynamic) {
142 body0->apply_torque_impulse(motorImp);
143 }
144 if (body1 && p_body1_dynamic) {
145 body1->apply_torque_impulse(-motorImp);
146 }
147
148 return clippedMotorImpulse;
149}
150
151//////////////////////////// GodotG6DOFTranslationalLimitMotor3D ////////////////////////////////////
152
153real_t GodotG6DOFTranslationalLimitMotor3D::solveLinearAxis(
154 real_t timeStep,
155 real_t jacDiagABInv,
156 GodotBody3D *body1, const Vector3 &pointInA,
157 GodotBody3D *body2, const Vector3 &pointInB,
158 bool p_body1_dynamic, bool p_body2_dynamic,
159 int limit_index,
160 const Vector3 &axis_normal_on_a,
161 const Vector3 &anchorPos) {
162 ///find relative velocity
163 // Vector3 rel_pos1 = pointInA - body1->get_transform().origin;
164 // Vector3 rel_pos2 = pointInB - body2->get_transform().origin;
165 Vector3 rel_pos1 = anchorPos - body1->get_transform().origin;
166 Vector3 rel_pos2 = anchorPos - body2->get_transform().origin;
167
168 Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1);
169 Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2);
170 Vector3 vel = vel1 - vel2;
171
172 real_t rel_vel = axis_normal_on_a.dot(vel);
173
174 /// apply displacement correction
175
176 //positional error (zeroth order error)
177 real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a);
178 real_t lo = real_t(-1e30);
179 real_t hi = real_t(1e30);
180
181 real_t minLimit = m_lowerLimit[limit_index];
182 real_t maxLimit = m_upperLimit[limit_index];
183
184 //handle the limits
185 if (minLimit < maxLimit) {
186 {
187 if (depth > maxLimit) {
188 depth -= maxLimit;
189 lo = real_t(0.);
190
191 } else {
192 if (depth < minLimit) {
193 depth -= minLimit;
194 hi = real_t(0.);
195 } else {
196 return 0.0f;
197 }
198 }
199 }
200 }
201
202 real_t normalImpulse = m_limitSoftness[limit_index] * (m_restitution[limit_index] * depth / timeStep - m_damping[limit_index] * rel_vel) * jacDiagABInv;
203
204 real_t oldNormalImpulse = m_accumulatedImpulse[limit_index];
205 real_t sum = oldNormalImpulse + normalImpulse;
206 m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : (sum < lo ? real_t(0.) : sum);
207 normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
208
209 Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
210 if (p_body1_dynamic) {
211 body1->apply_impulse(impulse_vector, rel_pos1);
212 }
213 if (p_body2_dynamic) {
214 body2->apply_impulse(-impulse_vector, rel_pos2);
215 }
216 return normalImpulse;
217}
218
219//////////////////////////// GodotGeneric6DOFJoint3D ////////////////////////////////////
220
221GodotGeneric6DOFJoint3D::GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA) :
222 GodotJoint3D(_arr, 2),
223 m_frameInA(frameInA),
224 m_frameInB(frameInB),
225 m_useLinearReferenceFrameA(useLinearReferenceFrameA) {
226 A = rbA;
227 B = rbB;
228 A->add_constraint(this, 0);
229 B->add_constraint(this, 1);
230}
231
232void GodotGeneric6DOFJoint3D::calculateAngleInfo() {
233 Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis;
234
235 m_calculatedAxisAngleDiff = relative_frame.get_euler(EulerOrder::XYZ);
236
237 // in euler angle mode we do not actually constrain the angular velocity
238 // along the axes axis[0] and axis[2] (although we do use axis[1]) :
239 //
240 // to get constrain w2-w1 along ...not
241 // ------ --------------------- ------
242 // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
243 // d(angle[1])/dt = 0 ax[1]
244 // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
245 //
246 // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
247 // to prove the result for angle[0], write the expression for angle[0] from
248 // GetInfo1 then take the derivative. to prove this for angle[2] it is
249 // easier to take the euler rate expression for d(angle[2])/dt with respect
250 // to the components of w and set that to 0.
251
252 Vector3 axis0 = m_calculatedTransformB.basis.get_column(0);
253 Vector3 axis2 = m_calculatedTransformA.basis.get_column(2);
254
255 m_calculatedAxis[1] = axis2.cross(axis0);
256 m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
257 m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
258
259 /*
260 if(m_debugDrawer)
261 {
262 char buff[300];
263 sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
264 m_calculatedAxisAngleDiff[0],
265 m_calculatedAxisAngleDiff[1],
266 m_calculatedAxisAngleDiff[2]);
267 m_debugDrawer->reportErrorWarning(buff);
268 }
269 */
270}
271
272void GodotGeneric6DOFJoint3D::calculateTransforms() {
273 m_calculatedTransformA = A->get_transform() * m_frameInA;
274 m_calculatedTransformB = B->get_transform() * m_frameInB;
275
276 calculateAngleInfo();
277}
278
279void GodotGeneric6DOFJoint3D::buildLinearJacobian(
280 GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld,
281 const Vector3 &pivotAInW, const Vector3 &pivotBInW) {
282 memnew_placement(
283 &jacLinear,
284 GodotJacobianEntry3D(
285 A->get_principal_inertia_axes().transposed(),
286 B->get_principal_inertia_axes().transposed(),
287 pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
288 pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
289 normalWorld,
290 A->get_inv_inertia(),
291 A->get_inv_mass(),
292 B->get_inv_inertia(),
293 B->get_inv_mass()));
294}
295
296void GodotGeneric6DOFJoint3D::buildAngularJacobian(
297 GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW) {
298 memnew_placement(
299 &jacAngular,
300 GodotJacobianEntry3D(
301 jointAxisW,
302 A->get_principal_inertia_axes().transposed(),
303 B->get_principal_inertia_axes().transposed(),
304 A->get_inv_inertia(),
305 B->get_inv_inertia()));
306}
307
308bool GodotGeneric6DOFJoint3D::testAngularLimitMotor(int axis_index) {
309 real_t angle = m_calculatedAxisAngleDiff[axis_index];
310
311 //test limits
312 m_angularLimits[axis_index].testLimitValue(angle);
313 return m_angularLimits[axis_index].needApplyTorques();
314}
315
316bool GodotGeneric6DOFJoint3D::setup(real_t p_timestep) {
317 dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
318 dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
319
320 if (!dynamic_A && !dynamic_B) {
321 return false;
322 }
323
324 // Clear accumulated impulses for the next simulation step
325 m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.));
326 int i;
327 for (i = 0; i < 3; i++) {
328 m_angularLimits[i].m_accumulatedImpulse = real_t(0.);
329 }
330 //calculates transform
331 calculateTransforms();
332
333 // const Vector3& pivotAInW = m_calculatedTransformA.origin;
334 // const Vector3& pivotBInW = m_calculatedTransformB.origin;
335 calcAnchorPos();
336 Vector3 pivotAInW = m_AnchorPos;
337 Vector3 pivotBInW = m_AnchorPos;
338
339 // not used here
340 // Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
341 // Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
342
343 Vector3 normalWorld;
344 //linear part
345 for (i = 0; i < 3; i++) {
346 if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) {
347 if (m_useLinearReferenceFrameA) {
348 normalWorld = m_calculatedTransformA.basis.get_column(i);
349 } else {
350 normalWorld = m_calculatedTransformB.basis.get_column(i);
351 }
352
353 buildLinearJacobian(
354 m_jacLinear[i], normalWorld,
355 pivotAInW, pivotBInW);
356 }
357 }
358
359 // angular part
360 for (i = 0; i < 3; i++) {
361 //calculates error angle
362 if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) {
363 normalWorld = this->getAxis(i);
364 // Create angular atom
365 buildAngularJacobian(m_jacAng[i], normalWorld);
366 }
367 }
368
369 return true;
370}
371
372void GodotGeneric6DOFJoint3D::solve(real_t p_timestep) {
373 m_timeStep = p_timestep;
374
375 //calculateTransforms();
376
377 int i;
378
379 // linear
380
381 Vector3 pointInA = m_calculatedTransformA.origin;
382 Vector3 pointInB = m_calculatedTransformB.origin;
383
384 real_t jacDiagABInv;
385 Vector3 linear_axis;
386 for (i = 0; i < 3; i++) {
387 if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) {
388 jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal();
389
390 if (m_useLinearReferenceFrameA) {
391 linear_axis = m_calculatedTransformA.basis.get_column(i);
392 } else {
393 linear_axis = m_calculatedTransformB.basis.get_column(i);
394 }
395
396 m_linearLimits.solveLinearAxis(
397 m_timeStep,
398 jacDiagABInv,
399 A, pointInA,
400 B, pointInB,
401 dynamic_A, dynamic_B,
402 i, linear_axis, m_AnchorPos);
403 }
404 }
405
406 // angular
407 Vector3 angular_axis;
408 real_t angularJacDiagABInv;
409 for (i = 0; i < 3; i++) {
410 if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) {
411 // get axis
412 angular_axis = getAxis(i);
413
414 angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal();
415
416 m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B, dynamic_A, dynamic_B);
417 }
418 }
419}
420
421void GodotGeneric6DOFJoint3D::updateRHS(real_t timeStep) {
422 (void)timeStep;
423}
424
425Vector3 GodotGeneric6DOFJoint3D::getAxis(int axis_index) const {
426 return m_calculatedAxis[axis_index];
427}
428
429real_t GodotGeneric6DOFJoint3D::getAngle(int axis_index) const {
430 return m_calculatedAxisAngleDiff[axis_index];
431}
432
433void GodotGeneric6DOFJoint3D::calcAnchorPos() {
434 real_t imA = A->get_inv_mass();
435 real_t imB = B->get_inv_mass();
436 real_t weight;
437 if (imB == real_t(0.0)) {
438 weight = real_t(1.0);
439 } else {
440 weight = imA / (imA + imB);
441 }
442 const Vector3 &pA = m_calculatedTransformA.origin;
443 const Vector3 &pB = m_calculatedTransformB.origin;
444 m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight);
445}
446
447void GodotGeneric6DOFJoint3D::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) {
448 ERR_FAIL_INDEX(p_axis, 3);
449 switch (p_param) {
450 case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
451 m_linearLimits.m_lowerLimit[p_axis] = p_value;
452 } break;
453 case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
454 m_linearLimits.m_upperLimit[p_axis] = p_value;
455
456 } break;
457 case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
458 m_linearLimits.m_limitSoftness[p_axis] = p_value;
459
460 } break;
461 case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: {
462 m_linearLimits.m_restitution[p_axis] = p_value;
463
464 } break;
465 case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: {
466 m_linearLimits.m_damping[p_axis] = p_value;
467
468 } break;
469 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
470 m_angularLimits[p_axis].m_loLimit = p_value;
471
472 } break;
473 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
474 m_angularLimits[p_axis].m_hiLimit = p_value;
475
476 } break;
477 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
478 m_angularLimits[p_axis].m_limitSoftness = p_value;
479
480 } break;
481 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: {
482 m_angularLimits[p_axis].m_damping = p_value;
483
484 } break;
485 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: {
486 m_angularLimits[p_axis].m_bounce = p_value;
487
488 } break;
489 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
490 m_angularLimits[p_axis].m_maxLimitForce = p_value;
491
492 } break;
493 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: {
494 m_angularLimits[p_axis].m_ERP = p_value;
495
496 } break;
497 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
498 m_angularLimits[p_axis].m_targetVelocity = p_value;
499
500 } break;
501 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
502 m_angularLimits[p_axis].m_maxLimitForce = p_value;
503
504 } break;
505 case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: {
506 // Not implemented in GodotPhysics3D backend
507 } break;
508 case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: {
509 // Not implemented in GodotPhysics3D backend
510 } break;
511 case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: {
512 // Not implemented in GodotPhysics3D backend
513 } break;
514 case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: {
515 // Not implemented in GodotPhysics3D backend
516 } break;
517 case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: {
518 // Not implemented in GodotPhysics3D backend
519 } break;
520 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: {
521 // Not implemented in GodotPhysics3D backend
522 } break;
523 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: {
524 // Not implemented in GodotPhysics3D backend
525 } break;
526 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: {
527 // Not implemented in GodotPhysics3D backend
528 } break;
529 case PhysicsServer3D::G6DOF_JOINT_MAX:
530 break; // Can't happen, but silences warning
531 }
532}
533
534real_t GodotGeneric6DOFJoint3D::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const {
535 ERR_FAIL_INDEX_V(p_axis, 3, 0);
536 switch (p_param) {
537 case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
538 return m_linearLimits.m_lowerLimit[p_axis];
539 } break;
540 case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
541 return m_linearLimits.m_upperLimit[p_axis];
542
543 } break;
544 case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
545 return m_linearLimits.m_limitSoftness[p_axis];
546
547 } break;
548 case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: {
549 return m_linearLimits.m_restitution[p_axis];
550
551 } break;
552 case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: {
553 return m_linearLimits.m_damping[p_axis];
554
555 } break;
556 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
557 return m_angularLimits[p_axis].m_loLimit;
558
559 } break;
560 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
561 return m_angularLimits[p_axis].m_hiLimit;
562
563 } break;
564 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
565 return m_angularLimits[p_axis].m_limitSoftness;
566
567 } break;
568 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: {
569 return m_angularLimits[p_axis].m_damping;
570
571 } break;
572 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: {
573 return m_angularLimits[p_axis].m_bounce;
574
575 } break;
576 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
577 return m_angularLimits[p_axis].m_maxLimitForce;
578
579 } break;
580 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: {
581 return m_angularLimits[p_axis].m_ERP;
582
583 } break;
584 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
585 return m_angularLimits[p_axis].m_targetVelocity;
586
587 } break;
588 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
589 return m_angularLimits[p_axis].m_maxMotorForce;
590
591 } break;
592 case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: {
593 // Not implemented in GodotPhysics3D backend
594 } break;
595 case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: {
596 // Not implemented in GodotPhysics3D backend
597 } break;
598 case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: {
599 // Not implemented in GodotPhysics3D backend
600 } break;
601 case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: {
602 // Not implemented in GodotPhysics3D backend
603 } break;
604 case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: {
605 // Not implemented in GodotPhysics3D backend
606 } break;
607 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: {
608 // Not implemented in GodotPhysics3D backend
609 } break;
610 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: {
611 // Not implemented in GodotPhysics3D backend
612 } break;
613 case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: {
614 // Not implemented in GodotPhysics3D backend
615 } break;
616 case PhysicsServer3D::G6DOF_JOINT_MAX:
617 break; // Can't happen, but silences warning
618 }
619 return 0;
620}
621
622void GodotGeneric6DOFJoint3D::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) {
623 ERR_FAIL_INDEX(p_axis, 3);
624
625 switch (p_flag) {
626 case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
627 m_linearLimits.enable_limit[p_axis] = p_value;
628 } break;
629 case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
630 m_angularLimits[p_axis].m_enableLimit = p_value;
631 } break;
632 case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
633 m_angularLimits[p_axis].m_enableMotor = p_value;
634 } break;
635 case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: {
636 // Not implemented in GodotPhysics3D backend
637 } break;
638 case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: {
639 // Not implemented in GodotPhysics3D backend
640 } break;
641 case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: {
642 // Not implemented in GodotPhysics3D backend
643 } break;
644 case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX:
645 break; // Can't happen, but silences warning
646 }
647}
648
649bool GodotGeneric6DOFJoint3D::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const {
650 ERR_FAIL_INDEX_V(p_axis, 3, 0);
651 switch (p_flag) {
652 case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
653 return m_linearLimits.enable_limit[p_axis];
654 } break;
655 case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
656 return m_angularLimits[p_axis].m_enableLimit;
657 } break;
658 case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
659 return m_angularLimits[p_axis].m_enableMotor;
660 } break;
661 case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: {
662 // Not implemented in GodotPhysics3D backend
663 } break;
664 case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: {
665 // Not implemented in GodotPhysics3D backend
666 } break;
667 case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: {
668 // Not implemented in GodotPhysics3D backend
669 } break;
670 case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX:
671 break; // Can't happen, but silences warning
672 }
673
674 return false;
675}
676