1/**************************************************************************/
2/* godot_slider_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/*
51Added by Roman Ponomarev (rponom@gmail.com)
52April 04, 2008
53
54*/
55
56#include "godot_slider_joint_3d.h"
57
58//-----------------------------------------------------------------------------
59
60GodotSliderJoint3D::GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB) :
61 GodotJoint3D(_arr, 2),
62 m_frameInA(frameInA),
63 m_frameInB(frameInB) {
64 A = rbA;
65 B = rbB;
66
67 A->add_constraint(this, 0);
68 B->add_constraint(this, 1);
69}
70
71//-----------------------------------------------------------------------------
72
73bool GodotSliderJoint3D::setup(real_t p_step) {
74 dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
75 dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
76
77 if (!dynamic_A && !dynamic_B) {
78 return false;
79 }
80
81 //calculate transforms
82 m_calculatedTransformA = A->get_transform() * m_frameInA;
83 m_calculatedTransformB = B->get_transform() * m_frameInB;
84 m_realPivotAInW = m_calculatedTransformA.origin;
85 m_realPivotBInW = m_calculatedTransformB.origin;
86 m_sliderAxis = m_calculatedTransformA.basis.get_column(0); // along X
87 m_delta = m_realPivotBInW - m_realPivotAInW;
88 m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
89 m_relPosA = m_projPivotInW - A->get_transform().origin;
90 m_relPosB = m_realPivotBInW - B->get_transform().origin;
91 Vector3 normalWorld;
92 int i;
93 //linear part
94 for (i = 0; i < 3; i++) {
95 normalWorld = m_calculatedTransformA.basis.get_column(i);
96 memnew_placement(
97 &m_jacLin[i],
98 GodotJacobianEntry3D(
99 A->get_principal_inertia_axes().transposed(),
100 B->get_principal_inertia_axes().transposed(),
101 m_relPosA - A->get_center_of_mass(),
102 m_relPosB - B->get_center_of_mass(),
103 normalWorld,
104 A->get_inv_inertia(),
105 A->get_inv_mass(),
106 B->get_inv_inertia(),
107 B->get_inv_mass()));
108 m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal();
109 m_depth[i] = m_delta.dot(normalWorld);
110 }
111 testLinLimits();
112 // angular part
113 for (i = 0; i < 3; i++) {
114 normalWorld = m_calculatedTransformA.basis.get_column(i);
115 memnew_placement(
116 &m_jacAng[i],
117 GodotJacobianEntry3D(
118 normalWorld,
119 A->get_principal_inertia_axes().transposed(),
120 B->get_principal_inertia_axes().transposed(),
121 A->get_inv_inertia(),
122 B->get_inv_inertia()));
123 }
124 testAngLimits();
125 Vector3 axisA = m_calculatedTransformA.basis.get_column(0);
126 m_kAngle = real_t(1.0) / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA));
127 // clear accumulator for motors
128 m_accumulatedLinMotorImpulse = real_t(0.0);
129 m_accumulatedAngMotorImpulse = real_t(0.0);
130
131 return true;
132}
133
134//-----------------------------------------------------------------------------
135
136void GodotSliderJoint3D::solve(real_t p_step) {
137 int i;
138 // linear
139 Vector3 velA = A->get_velocity_in_local_point(m_relPosA);
140 Vector3 velB = B->get_velocity_in_local_point(m_relPosB);
141 Vector3 vel = velA - velB;
142 for (i = 0; i < 3; i++) {
143 const Vector3 &normal = m_jacLin[i].m_linearJointAxis;
144 real_t rel_vel = normal.dot(vel);
145 // calculate positional error
146 real_t depth = m_depth[i];
147 // get parameters
148 real_t softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
149 real_t restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
150 real_t damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
151 // Calculate and apply impulse.
152 real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i];
153 Vector3 impulse_vector = normal * normalImpulse;
154 if (dynamic_A) {
155 A->apply_impulse(impulse_vector, m_relPosA);
156 }
157 if (dynamic_B) {
158 B->apply_impulse(-impulse_vector, m_relPosB);
159 }
160 if (m_poweredLinMotor && (!i)) { // apply linear motor
161 if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) {
162 real_t desiredMotorVel = m_targetLinMotorVelocity;
163 real_t motor_relvel = desiredMotorVel + rel_vel;
164 normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
165 // clamp accumulated impulse
166 real_t new_acc = m_accumulatedLinMotorImpulse + Math::abs(normalImpulse);
167 if (new_acc > m_maxLinMotorForce) {
168 new_acc = m_maxLinMotorForce;
169 }
170 real_t del = new_acc - m_accumulatedLinMotorImpulse;
171 if (normalImpulse < real_t(0.0)) {
172 normalImpulse = -del;
173 } else {
174 normalImpulse = del;
175 }
176 m_accumulatedLinMotorImpulse = new_acc;
177 // apply clamped impulse
178 impulse_vector = normal * normalImpulse;
179 if (dynamic_A) {
180 A->apply_impulse(impulse_vector, m_relPosA);
181 }
182 if (dynamic_B) {
183 B->apply_impulse(-impulse_vector, m_relPosB);
184 }
185 }
186 }
187 }
188 // angular
189 // get axes in world space
190 Vector3 axisA = m_calculatedTransformA.basis.get_column(0);
191 Vector3 axisB = m_calculatedTransformB.basis.get_column(0);
192
193 const Vector3 &angVelA = A->get_angular_velocity();
194 const Vector3 &angVelB = B->get_angular_velocity();
195
196 Vector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
197 Vector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
198
199 Vector3 angAorthog = angVelA - angVelAroundAxisA;
200 Vector3 angBorthog = angVelB - angVelAroundAxisB;
201 Vector3 velrelOrthog = angAorthog - angBorthog;
202 //solve orthogonal angular velocity correction
203 real_t len = velrelOrthog.length();
204 if (len > real_t(0.00001)) {
205 Vector3 normal = velrelOrthog.normalized();
206 real_t denom = A->compute_angular_impulse_denominator(normal) + B->compute_angular_impulse_denominator(normal);
207 velrelOrthog *= (real_t(1.) / denom) * m_dampingOrthoAng * m_softnessOrthoAng;
208 }
209 //solve angular positional correction
210 Vector3 angularError = axisA.cross(axisB) * (real_t(1.) / p_step);
211 real_t len2 = angularError.length();
212 if (len2 > real_t(0.00001)) {
213 Vector3 normal2 = angularError.normalized();
214 real_t denom2 = A->compute_angular_impulse_denominator(normal2) + B->compute_angular_impulse_denominator(normal2);
215 angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
216 }
217 // apply impulse
218 if (dynamic_A) {
219 A->apply_torque_impulse(-velrelOrthog + angularError);
220 }
221 if (dynamic_B) {
222 B->apply_torque_impulse(velrelOrthog - angularError);
223 }
224 real_t impulseMag;
225 //solve angular limits
226 if (m_solveAngLim) {
227 impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / p_step;
228 impulseMag *= m_kAngle * m_softnessLimAng;
229 } else {
230 impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / p_step;
231 impulseMag *= m_kAngle * m_softnessDirAng;
232 }
233 Vector3 impulse = axisA * impulseMag;
234 if (dynamic_A) {
235 A->apply_torque_impulse(impulse);
236 }
237 if (dynamic_B) {
238 B->apply_torque_impulse(-impulse);
239 }
240 //apply angular motor
241 if (m_poweredAngMotor) {
242 if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) {
243 Vector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
244 real_t projRelVel = velrel.dot(axisA);
245
246 real_t desiredMotorVel = m_targetAngMotorVelocity;
247 real_t motor_relvel = desiredMotorVel - projRelVel;
248
249 real_t angImpulse = m_kAngle * motor_relvel;
250 // clamp accumulated impulse
251 real_t new_acc = m_accumulatedAngMotorImpulse + Math::abs(angImpulse);
252 if (new_acc > m_maxAngMotorForce) {
253 new_acc = m_maxAngMotorForce;
254 }
255 real_t del = new_acc - m_accumulatedAngMotorImpulse;
256 if (angImpulse < real_t(0.0)) {
257 angImpulse = -del;
258 } else {
259 angImpulse = del;
260 }
261 m_accumulatedAngMotorImpulse = new_acc;
262 // apply clamped impulse
263 Vector3 motorImp = angImpulse * axisA;
264 if (dynamic_A) {
265 A->apply_torque_impulse(motorImp);
266 }
267 if (dynamic_B) {
268 B->apply_torque_impulse(-motorImp);
269 }
270 }
271 }
272}
273
274//-----------------------------------------------------------------------------
275
276void GodotSliderJoint3D::calculateTransforms() {
277 m_calculatedTransformA = A->get_transform() * m_frameInA;
278 m_calculatedTransformB = B->get_transform() * m_frameInB;
279 m_realPivotAInW = m_calculatedTransformA.origin;
280 m_realPivotBInW = m_calculatedTransformB.origin;
281 m_sliderAxis = m_calculatedTransformA.basis.get_column(0); // along X
282 m_delta = m_realPivotBInW - m_realPivotAInW;
283 m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
284 Vector3 normalWorld;
285 int i;
286 //linear part
287 for (i = 0; i < 3; i++) {
288 normalWorld = m_calculatedTransformA.basis.get_column(i);
289 m_depth[i] = m_delta.dot(normalWorld);
290 }
291}
292
293//-----------------------------------------------------------------------------
294
295void GodotSliderJoint3D::testLinLimits() {
296 m_solveLinLim = false;
297 m_linPos = m_depth[0];
298 if (m_lowerLinLimit <= m_upperLinLimit) {
299 if (m_depth[0] > m_upperLinLimit) {
300 m_depth[0] -= m_upperLinLimit;
301 m_solveLinLim = true;
302 } else if (m_depth[0] < m_lowerLinLimit) {
303 m_depth[0] -= m_lowerLinLimit;
304 m_solveLinLim = true;
305 } else {
306 m_depth[0] = real_t(0.);
307 }
308 } else {
309 m_depth[0] = real_t(0.);
310 }
311}
312
313//-----------------------------------------------------------------------------
314
315void GodotSliderJoint3D::testAngLimits() {
316 m_angDepth = real_t(0.);
317 m_solveAngLim = false;
318 if (m_lowerAngLimit <= m_upperAngLimit) {
319 const Vector3 axisA0 = m_calculatedTransformA.basis.get_column(1);
320 const Vector3 axisA1 = m_calculatedTransformA.basis.get_column(2);
321 const Vector3 axisB0 = m_calculatedTransformB.basis.get_column(1);
322 real_t rot = atan2fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
323 if (rot < m_lowerAngLimit) {
324 m_angDepth = rot - m_lowerAngLimit;
325 m_solveAngLim = true;
326 } else if (rot > m_upperAngLimit) {
327 m_angDepth = rot - m_upperAngLimit;
328 m_solveAngLim = true;
329 }
330 }
331}
332
333//-----------------------------------------------------------------------------
334
335Vector3 GodotSliderJoint3D::getAncorInA() {
336 Vector3 ancorInA;
337 ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis;
338 ancorInA = A->get_transform().inverse().xform(ancorInA);
339 return ancorInA;
340}
341
342//-----------------------------------------------------------------------------
343
344Vector3 GodotSliderJoint3D::getAncorInB() {
345 Vector3 ancorInB;
346 ancorInB = m_frameInB.origin;
347 return ancorInB;
348}
349
350void GodotSliderJoint3D::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) {
351 switch (p_param) {
352 case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
353 m_upperLinLimit = p_value;
354 break;
355 case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER:
356 m_lowerLinLimit = p_value;
357 break;
358 case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS:
359 m_softnessLimLin = p_value;
360 break;
361 case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION:
362 m_restitutionLimLin = p_value;
363 break;
364 case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING:
365 m_dampingLimLin = p_value;
366 break;
367 case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS:
368 m_softnessDirLin = p_value;
369 break;
370 case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION:
371 m_restitutionDirLin = p_value;
372 break;
373 case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING:
374 m_dampingDirLin = p_value;
375 break;
376 case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS:
377 m_softnessOrthoLin = p_value;
378 break;
379 case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION:
380 m_restitutionOrthoLin = p_value;
381 break;
382 case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING:
383 m_dampingOrthoLin = p_value;
384 break;
385
386 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER:
387 m_upperAngLimit = p_value;
388 break;
389 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER:
390 m_lowerAngLimit = p_value;
391 break;
392 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS:
393 m_softnessLimAng = p_value;
394 break;
395 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION:
396 m_restitutionLimAng = p_value;
397 break;
398 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING:
399 m_dampingLimAng = p_value;
400 break;
401 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS:
402 m_softnessDirAng = p_value;
403 break;
404 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION:
405 m_restitutionDirAng = p_value;
406 break;
407 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING:
408 m_dampingDirAng = p_value;
409 break;
410 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS:
411 m_softnessOrthoAng = p_value;
412 break;
413 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION:
414 m_restitutionOrthoAng = p_value;
415 break;
416 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING:
417 m_dampingOrthoAng = p_value;
418 break;
419
420 case PhysicsServer3D::SLIDER_JOINT_MAX:
421 break; // Can't happen, but silences warning
422 }
423}
424
425real_t GodotSliderJoint3D::get_param(PhysicsServer3D::SliderJointParam p_param) const {
426 switch (p_param) {
427 case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
428 return m_upperLinLimit;
429 case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER:
430 return m_lowerLinLimit;
431 case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS:
432 return m_softnessLimLin;
433 case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION:
434 return m_restitutionLimLin;
435 case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING:
436 return m_dampingLimLin;
437 case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS:
438 return m_softnessDirLin;
439 case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION:
440 return m_restitutionDirLin;
441 case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING:
442 return m_dampingDirLin;
443 case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS:
444 return m_softnessOrthoLin;
445 case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION:
446 return m_restitutionOrthoLin;
447 case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING:
448 return m_dampingOrthoLin;
449
450 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER:
451 return m_upperAngLimit;
452 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER:
453 return m_lowerAngLimit;
454 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS:
455 return m_softnessLimAng;
456 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION:
457 return m_restitutionLimAng;
458 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING:
459 return m_dampingLimAng;
460 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS:
461 return m_softnessDirAng;
462 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION:
463 return m_restitutionDirAng;
464 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING:
465 return m_dampingDirAng;
466 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS:
467 return m_softnessOrthoAng;
468 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION:
469 return m_restitutionOrthoAng;
470 case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING:
471 return m_dampingOrthoAng;
472
473 case PhysicsServer3D::SLIDER_JOINT_MAX:
474 break; // Can't happen, but silences warning
475 }
476
477 return 0;
478}
479