1/**************************************************************************/
2/* godot_joints_2d.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#include "godot_joints_2d.h"
32
33#include "godot_space_2d.h"
34
35//based on chipmunk joint constraints
36
37/* Copyright (c) 2007 Scott Lembcke
38 *
39 * Permission is hereby granted, free of charge, to any person obtaining a copy
40 * of this software and associated documentation files (the "Software"), to deal
41 * in the Software without restriction, including without limitation the rights
42 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
43 * copies of the Software, and to permit persons to whom the Software is
44 * furnished to do so, subject to the following conditions:
45 *
46 * The above copyright notice and this permission notice shall be included in
47 * all copies or substantial portions of the Software.
48 *
49 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
50 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
51 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
52 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
53 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
54 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
55 * SOFTWARE.
56 */
57
58void GodotJoint2D::copy_settings_from(GodotJoint2D *p_joint) {
59 set_self(p_joint->get_self());
60 set_max_force(p_joint->get_max_force());
61 set_bias(p_joint->get_bias());
62 set_max_bias(p_joint->get_max_bias());
63 disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies());
64}
65
66static inline real_t k_scalar(GodotBody2D *a, GodotBody2D *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) {
67 real_t value = 0.0;
68
69 {
70 value += a->get_inv_mass();
71 real_t rcn = (rA - a->get_center_of_mass()).cross(n);
72 value += a->get_inv_inertia() * rcn * rcn;
73 }
74
75 if (b) {
76 value += b->get_inv_mass();
77 real_t rcn = (rB - b->get_center_of_mass()).cross(n);
78 value += b->get_inv_inertia() * rcn * rcn;
79 }
80
81 return value;
82}
83
84static inline Vector2
85relative_velocity(GodotBody2D *a, GodotBody2D *b, Vector2 rA, Vector2 rB) {
86 Vector2 sum = a->get_linear_velocity() - (rA - a->get_center_of_mass()).orthogonal() * a->get_angular_velocity();
87 if (b) {
88 return (b->get_linear_velocity() - (rB - b->get_center_of_mass()).orthogonal() * b->get_angular_velocity()) - sum;
89 } else {
90 return -sum;
91 }
92}
93
94static inline real_t
95normal_relative_velocity(GodotBody2D *a, GodotBody2D *b, Vector2 rA, Vector2 rB, Vector2 n) {
96 return relative_velocity(a, b, rA, rB).dot(n);
97}
98
99bool GodotPinJoint2D::setup(real_t p_step) {
100 dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
101 dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
102
103 if (!dynamic_A && !dynamic_B) {
104 return false;
105 }
106
107 GodotSpace2D *space = A->get_space();
108 ERR_FAIL_COND_V(!space, false);
109
110 rA = A->get_transform().basis_xform(anchor_A);
111 rB = B ? B->get_transform().basis_xform(anchor_B) : anchor_B;
112
113 real_t B_inv_mass = B ? B->get_inv_mass() : 0.0;
114
115 Transform2D K1;
116 K1[0].x = A->get_inv_mass() + B_inv_mass;
117 K1[1].x = 0.0f;
118 K1[0].y = 0.0f;
119 K1[1].y = A->get_inv_mass() + B_inv_mass;
120
121 Vector2 r1 = rA - A->get_center_of_mass();
122
123 Transform2D K2;
124 K2[0].x = A->get_inv_inertia() * r1.y * r1.y;
125 K2[1].x = -A->get_inv_inertia() * r1.x * r1.y;
126 K2[0].y = -A->get_inv_inertia() * r1.x * r1.y;
127 K2[1].y = A->get_inv_inertia() * r1.x * r1.x;
128
129 Transform2D K;
130 K[0] = K1[0] + K2[0];
131 K[1] = K1[1] + K2[1];
132
133 if (B) {
134 Vector2 r2 = rB - B->get_center_of_mass();
135
136 Transform2D K3;
137 K3[0].x = B->get_inv_inertia() * r2.y * r2.y;
138 K3[1].x = -B->get_inv_inertia() * r2.x * r2.y;
139 K3[0].y = -B->get_inv_inertia() * r2.x * r2.y;
140 K3[1].y = B->get_inv_inertia() * r2.x * r2.x;
141
142 K[0] += K3[0];
143 K[1] += K3[1];
144 }
145
146 K[0].x += softness;
147 K[1].y += softness;
148
149 M = K.affine_inverse();
150
151 Vector2 gA = rA + A->get_transform().get_origin();
152 Vector2 gB = B ? rB + B->get_transform().get_origin() : rB;
153
154 Vector2 delta = gB - gA;
155
156 bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step);
157
158 return true;
159}
160
161inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) {
162 return Vector2(p_other * p_vec.y, -p_other * p_vec.x);
163}
164
165bool GodotPinJoint2D::pre_solve(real_t p_step) {
166 // Apply accumulated impulse.
167 if (dynamic_A) {
168 A->apply_impulse(-P, rA);
169 }
170 if (B && dynamic_B) {
171 B->apply_impulse(P, rB);
172 }
173
174 return true;
175}
176
177void GodotPinJoint2D::solve(real_t p_step) {
178 // compute relative velocity
179 Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity());
180
181 Vector2 rel_vel;
182 if (B) {
183 rel_vel = B->get_linear_velocity() - custom_cross(rB - B->get_center_of_mass(), B->get_angular_velocity()) - vA;
184 } else {
185 rel_vel = -vA;
186 }
187
188 Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P);
189
190 if (dynamic_A) {
191 A->apply_impulse(-impulse, rA);
192 }
193 if (B && dynamic_B) {
194 B->apply_impulse(impulse, rB);
195 }
196
197 P += impulse;
198}
199
200void GodotPinJoint2D::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) {
201 if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) {
202 softness = p_value;
203 }
204}
205
206real_t GodotPinJoint2D::get_param(PhysicsServer2D::PinJointParam p_param) const {
207 if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) {
208 return softness;
209 }
210 ERR_FAIL_V(0);
211}
212
213GodotPinJoint2D::GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, GodotBody2D *p_body_b) :
214 GodotJoint2D(_arr, p_body_b ? 2 : 1) {
215 A = p_body_a;
216 B = p_body_b;
217 anchor_A = p_body_a->get_inv_transform().xform(p_pos);
218 anchor_B = p_body_b ? p_body_b->get_inv_transform().xform(p_pos) : p_pos;
219
220 p_body_a->add_constraint(this, 0);
221 if (p_body_b) {
222 p_body_b->add_constraint(this, 1);
223 }
224}
225
226//////////////////////////////////////////////
227//////////////////////////////////////////////
228//////////////////////////////////////////////
229
230static inline void
231k_tensor(GodotBody2D *a, GodotBody2D *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2) {
232 // calculate mass matrix
233 // If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
234 real_t k11, k12, k21, k22;
235 real_t m_sum = a->get_inv_mass() + b->get_inv_mass();
236
237 // start with I*m_sum
238 k11 = m_sum;
239 k12 = 0.0f;
240 k21 = 0.0f;
241 k22 = m_sum;
242
243 r1 -= a->get_center_of_mass();
244 r2 -= b->get_center_of_mass();
245
246 // add the influence from r1
247 real_t a_i_inv = a->get_inv_inertia();
248 real_t r1xsq = r1.x * r1.x * a_i_inv;
249 real_t r1ysq = r1.y * r1.y * a_i_inv;
250 real_t r1nxy = -r1.x * r1.y * a_i_inv;
251 k11 += r1ysq;
252 k12 += r1nxy;
253 k21 += r1nxy;
254 k22 += r1xsq;
255
256 // add the influnce from r2
257 real_t b_i_inv = b->get_inv_inertia();
258 real_t r2xsq = r2.x * r2.x * b_i_inv;
259 real_t r2ysq = r2.y * r2.y * b_i_inv;
260 real_t r2nxy = -r2.x * r2.y * b_i_inv;
261 k11 += r2ysq;
262 k12 += r2nxy;
263 k21 += r2nxy;
264 k22 += r2xsq;
265
266 // invert
267 real_t determinant = k11 * k22 - k12 * k21;
268 ERR_FAIL_COND(determinant == 0.0);
269
270 real_t det_inv = 1.0f / determinant;
271 *k1 = Vector2(k22 * det_inv, -k12 * det_inv);
272 *k2 = Vector2(-k21 * det_inv, k11 * det_inv);
273}
274
275static _FORCE_INLINE_ Vector2
276mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) {
277 return Vector2(vr.dot(k1), vr.dot(k2));
278}
279
280bool GodotGrooveJoint2D::setup(real_t p_step) {
281 dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
282 dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
283
284 if (!dynamic_A && !dynamic_B) {
285 return false;
286 }
287
288 GodotSpace2D *space = A->get_space();
289 ERR_FAIL_COND_V(!space, false);
290
291 // calculate endpoints in worldspace
292 Vector2 ta = A->get_transform().xform(A_groove_1);
293 Vector2 tb = A->get_transform().xform(A_groove_2);
294
295 // calculate axis
296 Vector2 n = -(tb - ta).orthogonal().normalized();
297 real_t d = ta.dot(n);
298
299 xf_normal = n;
300 rB = B->get_transform().basis_xform(B_anchor);
301
302 // calculate tangential distance along the axis of rB
303 real_t td = (B->get_transform().get_origin() + rB).cross(n);
304 // calculate clamping factor and rB
305 if (td <= ta.cross(n)) {
306 clamp = 1.0f;
307 rA = ta - A->get_transform().get_origin();
308 } else if (td >= tb.cross(n)) {
309 clamp = -1.0f;
310 rA = tb - A->get_transform().get_origin();
311 } else {
312 clamp = 0.0f;
313 //joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
314 rA = ((-n.orthogonal() * -td) + n * d) - A->get_transform().get_origin();
315 }
316
317 // Calculate mass tensor
318 k_tensor(A, B, rA, rB, &k1, &k2);
319
320 // compute max impulse
321 jn_max = get_max_force() * p_step;
322
323 // calculate bias velocity
324 //cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
325 //joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
326
327 Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA);
328
329 real_t _b = get_bias();
330 gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).limit_length(get_max_bias());
331
332 correct = true;
333 return true;
334}
335
336bool GodotGrooveJoint2D::pre_solve(real_t p_step) {
337 // Apply accumulated impulse.
338 if (dynamic_A) {
339 A->apply_impulse(-jn_acc, rA);
340 }
341 if (dynamic_B) {
342 B->apply_impulse(jn_acc, rB);
343 }
344
345 return true;
346}
347
348void GodotGrooveJoint2D::solve(real_t p_step) {
349 // compute impulse
350 Vector2 vr = relative_velocity(A, B, rA, rB);
351
352 Vector2 j = mult_k(gbias - vr, k1, k2);
353 Vector2 jOld = jn_acc;
354 j += jOld;
355
356 jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : j.project(xf_normal)).limit_length(jn_max);
357
358 j = jn_acc - jOld;
359
360 if (dynamic_A) {
361 A->apply_impulse(-j, rA);
362 }
363 if (dynamic_B) {
364 B->apply_impulse(j, rB);
365 }
366}
367
368GodotGrooveJoint2D::GodotGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, GodotBody2D *p_body_a, GodotBody2D *p_body_b) :
369 GodotJoint2D(_arr, 2) {
370 A = p_body_a;
371 B = p_body_b;
372
373 A_groove_1 = A->get_inv_transform().xform(p_a_groove1);
374 A_groove_2 = A->get_inv_transform().xform(p_a_groove2);
375 B_anchor = B->get_inv_transform().xform(p_b_anchor);
376 A_groove_normal = -(A_groove_2 - A_groove_1).normalized().orthogonal();
377
378 A->add_constraint(this, 0);
379 B->add_constraint(this, 1);
380}
381
382//////////////////////////////////////////////
383//////////////////////////////////////////////
384//////////////////////////////////////////////
385
386bool GodotDampedSpringJoint2D::setup(real_t p_step) {
387 dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
388 dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
389
390 if (!dynamic_A && !dynamic_B) {
391 return false;
392 }
393
394 rA = A->get_transform().basis_xform(anchor_A);
395 rB = B->get_transform().basis_xform(anchor_B);
396
397 Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA);
398 real_t dist = delta.length();
399
400 if (dist) {
401 n = delta / dist;
402 } else {
403 n = Vector2();
404 }
405
406 real_t k = k_scalar(A, B, rA, rB, n);
407 n_mass = 1.0f / k;
408
409 target_vrn = 0.0f;
410 v_coef = 1.0f - Math::exp(-damping * (p_step)*k);
411
412 // Calculate spring force.
413 real_t f_spring = (rest_length - dist) * stiffness;
414 j = n * f_spring * (p_step);
415
416 return true;
417}
418
419bool GodotDampedSpringJoint2D::pre_solve(real_t p_step) {
420 // Apply spring force.
421 if (dynamic_A) {
422 A->apply_impulse(-j, rA);
423 }
424 if (dynamic_B) {
425 B->apply_impulse(j, rB);
426 }
427
428 return true;
429}
430
431void GodotDampedSpringJoint2D::solve(real_t p_step) {
432 // compute relative velocity
433 real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn;
434
435 // compute velocity loss from drag
436 // not 100% certain this is derived correctly, though it makes sense
437 real_t v_damp = -vrn * v_coef;
438 target_vrn = vrn + v_damp;
439 Vector2 j_new = n * v_damp * n_mass;
440
441 if (dynamic_A) {
442 A->apply_impulse(-j_new, rA);
443 }
444 if (dynamic_B) {
445 B->apply_impulse(j_new, rB);
446 }
447}
448
449void GodotDampedSpringJoint2D::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) {
450 switch (p_param) {
451 case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: {
452 rest_length = p_value;
453 } break;
454 case PhysicsServer2D::DAMPED_SPRING_DAMPING: {
455 damping = p_value;
456 } break;
457 case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: {
458 stiffness = p_value;
459 } break;
460 }
461}
462
463real_t GodotDampedSpringJoint2D::get_param(PhysicsServer2D::DampedSpringParam p_param) const {
464 switch (p_param) {
465 case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: {
466 return rest_length;
467 } break;
468 case PhysicsServer2D::DAMPED_SPRING_DAMPING: {
469 return damping;
470 } break;
471 case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: {
472 return stiffness;
473 } break;
474 }
475
476 ERR_FAIL_V(0);
477}
478
479GodotDampedSpringJoint2D::GodotDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, GodotBody2D *p_body_a, GodotBody2D *p_body_b) :
480 GodotJoint2D(_arr, 2) {
481 A = p_body_a;
482 B = p_body_b;
483 anchor_A = A->get_inv_transform().xform(p_anchor_a);
484 anchor_B = B->get_inv_transform().xform(p_anchor_b);
485
486 rest_length = p_anchor_a.distance_to(p_anchor_b);
487
488 A->add_constraint(this, 0);
489 B->add_constraint(this, 1);
490}
491