1/**************************************************************************/
2/* joint_3d_gizmo_plugin.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 "joint_3d_gizmo_plugin.h"
32
33#include "editor/editor_node.h"
34#include "editor/editor_settings.h"
35#include "editor/plugins/node_3d_editor_plugin.h"
36#include "scene/3d/joint_3d.h"
37
38#define BODY_A_RADIUS 0.25
39#define BODY_B_RADIUS 0.27
40
41Basis JointGizmosDrawer::look_body(const Transform3D &p_joint_transform, const Transform3D &p_body_transform) {
42 const Vector3 &p_eye(p_joint_transform.origin);
43 const Vector3 &p_target(p_body_transform.origin);
44
45 Vector3 v_x, v_y, v_z;
46
47 // Look the body with X
48 v_x = p_target - p_eye;
49 v_x.normalize();
50
51 v_z = v_x.cross(Vector3(0, 1, 0));
52 v_z.normalize();
53
54 v_y = v_z.cross(v_x);
55 v_y.normalize();
56
57 Basis base;
58 base.set_columns(v_x, v_y, v_z);
59
60 // Absorb current joint transform
61 base = p_joint_transform.basis.inverse() * base;
62
63 return base;
64}
65
66Basis JointGizmosDrawer::look_body_toward(Vector3::Axis p_axis, const Transform3D &joint_transform, const Transform3D &body_transform) {
67 switch (p_axis) {
68 case Vector3::AXIS_X:
69 return look_body_toward_x(joint_transform, body_transform);
70 case Vector3::AXIS_Y:
71 return look_body_toward_y(joint_transform, body_transform);
72 case Vector3::AXIS_Z:
73 return look_body_toward_z(joint_transform, body_transform);
74 default:
75 return Basis();
76 }
77}
78
79Basis JointGizmosDrawer::look_body_toward_x(const Transform3D &p_joint_transform, const Transform3D &p_body_transform) {
80 const Vector3 &p_eye(p_joint_transform.origin);
81 const Vector3 &p_target(p_body_transform.origin);
82
83 const Vector3 p_front(p_joint_transform.basis.get_column(0));
84
85 Vector3 v_x, v_y, v_z;
86
87 // Look the body with X
88 v_x = p_target - p_eye;
89 v_x.normalize();
90
91 v_y = p_front.cross(v_x);
92 v_y.normalize();
93
94 v_z = v_y.cross(p_front);
95 v_z.normalize();
96
97 // Clamp X to FRONT axis
98 v_x = p_front;
99 v_x.normalize();
100
101 Basis base;
102 base.set_columns(v_x, v_y, v_z);
103
104 // Absorb current joint transform
105 base = p_joint_transform.basis.inverse() * base;
106
107 return base;
108}
109
110Basis JointGizmosDrawer::look_body_toward_y(const Transform3D &p_joint_transform, const Transform3D &p_body_transform) {
111 const Vector3 &p_eye(p_joint_transform.origin);
112 const Vector3 &p_target(p_body_transform.origin);
113
114 const Vector3 p_up(p_joint_transform.basis.get_column(1));
115
116 Vector3 v_x, v_y, v_z;
117
118 // Look the body with X
119 v_x = p_target - p_eye;
120 v_x.normalize();
121
122 v_z = v_x.cross(p_up);
123 v_z.normalize();
124
125 v_x = p_up.cross(v_z);
126 v_x.normalize();
127
128 // Clamp Y to UP axis
129 v_y = p_up;
130 v_y.normalize();
131
132 Basis base;
133 base.set_columns(v_x, v_y, v_z);
134
135 // Absorb current joint transform
136 base = p_joint_transform.basis.inverse() * base;
137
138 return base;
139}
140
141Basis JointGizmosDrawer::look_body_toward_z(const Transform3D &p_joint_transform, const Transform3D &p_body_transform) {
142 const Vector3 &p_eye(p_joint_transform.origin);
143 const Vector3 &p_target(p_body_transform.origin);
144
145 const Vector3 p_lateral(p_joint_transform.basis.get_column(2));
146
147 Vector3 v_x, v_y, v_z;
148
149 // Look the body with X
150 v_x = p_target - p_eye;
151 v_x.normalize();
152
153 v_z = p_lateral;
154 v_z.normalize();
155
156 v_y = v_z.cross(v_x);
157 v_y.normalize();
158
159 // Clamp X to Z axis
160 v_x = v_y.cross(v_z);
161 v_x.normalize();
162
163 Basis base;
164 base.set_columns(v_x, v_y, v_z);
165
166 // Absorb current joint transform
167 base = p_joint_transform.basis.inverse() * base;
168
169 return base;
170}
171
172void JointGizmosDrawer::draw_circle(Vector3::Axis p_axis, real_t p_radius, const Transform3D &p_offset, const Basis &p_base, real_t p_limit_lower, real_t p_limit_upper, Vector<Vector3> &r_points, bool p_inverse) {
173 if (p_limit_lower == p_limit_upper) {
174 r_points.push_back(p_offset.translated_local(Vector3()).origin);
175 r_points.push_back(p_offset.translated_local(p_base.xform(Vector3(0.5, 0, 0))).origin);
176
177 } else {
178 if (p_limit_lower > p_limit_upper) {
179 p_limit_lower = -Math_PI;
180 p_limit_upper = Math_PI;
181 }
182
183 const int points = 32;
184
185 for (int i = 0; i < points; i++) {
186 real_t s = p_limit_lower + i * (p_limit_upper - p_limit_lower) / points;
187 real_t n = p_limit_lower + (i + 1) * (p_limit_upper - p_limit_lower) / points;
188
189 Vector3 from;
190 Vector3 to;
191 switch (p_axis) {
192 case Vector3::AXIS_X:
193 if (p_inverse) {
194 from = p_base.xform(Vector3(0, Math::sin(s), Math::cos(s))) * p_radius;
195 to = p_base.xform(Vector3(0, Math::sin(n), Math::cos(n))) * p_radius;
196 } else {
197 from = p_base.xform(Vector3(0, -Math::sin(s), Math::cos(s))) * p_radius;
198 to = p_base.xform(Vector3(0, -Math::sin(n), Math::cos(n))) * p_radius;
199 }
200 break;
201 case Vector3::AXIS_Y:
202 if (p_inverse) {
203 from = p_base.xform(Vector3(Math::cos(s), 0, -Math::sin(s))) * p_radius;
204 to = p_base.xform(Vector3(Math::cos(n), 0, -Math::sin(n))) * p_radius;
205 } else {
206 from = p_base.xform(Vector3(Math::cos(s), 0, Math::sin(s))) * p_radius;
207 to = p_base.xform(Vector3(Math::cos(n), 0, Math::sin(n))) * p_radius;
208 }
209 break;
210 case Vector3::AXIS_Z:
211 from = p_base.xform(Vector3(Math::cos(s), Math::sin(s), 0)) * p_radius;
212 to = p_base.xform(Vector3(Math::cos(n), Math::sin(n), 0)) * p_radius;
213 break;
214 }
215
216 if (i == points - 1) {
217 r_points.push_back(p_offset.translated_local(to).origin);
218 r_points.push_back(p_offset.translated_local(Vector3()).origin);
219 }
220 if (i == 0) {
221 r_points.push_back(p_offset.translated_local(from).origin);
222 r_points.push_back(p_offset.translated_local(Vector3()).origin);
223 }
224
225 r_points.push_back(p_offset.translated_local(from).origin);
226 r_points.push_back(p_offset.translated_local(to).origin);
227 }
228
229 r_points.push_back(p_offset.translated_local(Vector3(0, p_radius * 1.5, 0)).origin);
230 r_points.push_back(p_offset.translated_local(Vector3()).origin);
231 }
232}
233
234void JointGizmosDrawer::draw_cone(const Transform3D &p_offset, const Basis &p_base, real_t p_swing, real_t p_twist, Vector<Vector3> &r_points) {
235 float r = 1.0;
236 float w = r * Math::sin(p_swing);
237 float d = r * Math::cos(p_swing);
238
239 //swing
240 for (int i = 0; i < 360; i += 10) {
241 float ra = Math::deg_to_rad((float)i);
242 float rb = Math::deg_to_rad((float)i + 10);
243 Point2 a = Vector2(Math::sin(ra), Math::cos(ra)) * w;
244 Point2 b = Vector2(Math::sin(rb), Math::cos(rb)) * w;
245
246 r_points.push_back(p_offset.translated_local(p_base.xform(Vector3(d, a.x, a.y))).origin);
247 r_points.push_back(p_offset.translated_local(p_base.xform(Vector3(d, b.x, b.y))).origin);
248
249 if (i % 90 == 0) {
250 r_points.push_back(p_offset.translated_local(p_base.xform(Vector3(d, a.x, a.y))).origin);
251 r_points.push_back(p_offset.translated_local(p_base.xform(Vector3())).origin);
252 }
253 }
254
255 r_points.push_back(p_offset.translated_local(p_base.xform(Vector3())).origin);
256 r_points.push_back(p_offset.translated_local(p_base.xform(Vector3(1, 0, 0))).origin);
257
258 /// Twist
259 float ts = Math::rad_to_deg(p_twist);
260 ts = MIN(ts, 720);
261
262 for (int i = 0; i < int(ts); i += 5) {
263 float ra = Math::deg_to_rad((float)i);
264 float rb = Math::deg_to_rad((float)i + 5);
265 float c = i / 720.0;
266 float cn = (i + 5) / 720.0;
267 Point2 a = Vector2(Math::sin(ra), Math::cos(ra)) * w * c;
268 Point2 b = Vector2(Math::sin(rb), Math::cos(rb)) * w * cn;
269
270 r_points.push_back(p_offset.translated_local(p_base.xform(Vector3(c, a.x, a.y))).origin);
271 r_points.push_back(p_offset.translated_local(p_base.xform(Vector3(cn, b.x, b.y))).origin);
272 }
273}
274
275////
276
277Joint3DGizmoPlugin::Joint3DGizmoPlugin() {
278 create_material("joint_material", EDITOR_GET("editors/3d_gizmos/gizmo_colors/joint"));
279 create_material("joint_body_a_material", EDITOR_DEF("editors/3d_gizmos/gizmo_colors/joint_body_a", Color(0.6, 0.8, 1)));
280 create_material("joint_body_b_material", EDITOR_DEF("editors/3d_gizmos/gizmo_colors/joint_body_b", Color(0.6, 0.9, 1)));
281
282 update_timer = memnew(Timer);
283 update_timer->set_name("JointGizmoUpdateTimer");
284 update_timer->set_wait_time(1.0 / 120.0);
285 update_timer->connect("timeout", callable_mp(this, &Joint3DGizmoPlugin::incremental_update_gizmos));
286 update_timer->set_autostart(true);
287 EditorNode::get_singleton()->call_deferred(SNAME("add_child"), update_timer);
288}
289
290void Joint3DGizmoPlugin::incremental_update_gizmos() {
291 if (!current_gizmos.is_empty()) {
292 update_idx++;
293 update_idx = update_idx % current_gizmos.size();
294 redraw(current_gizmos[update_idx]);
295 }
296}
297
298bool Joint3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
299 return Object::cast_to<Joint3D>(p_spatial) != nullptr;
300}
301
302String Joint3DGizmoPlugin::get_gizmo_name() const {
303 return "Joint3D";
304}
305
306int Joint3DGizmoPlugin::get_priority() const {
307 return -1;
308}
309
310void Joint3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
311 Joint3D *joint = Object::cast_to<Joint3D>(p_gizmo->get_node_3d());
312
313 p_gizmo->clear();
314
315 Node3D *node_body_a = nullptr;
316 if (!joint->get_node_a().is_empty()) {
317 node_body_a = Object::cast_to<Node3D>(joint->get_node(joint->get_node_a()));
318 }
319
320 Node3D *node_body_b = nullptr;
321 if (!joint->get_node_b().is_empty()) {
322 node_body_b = Object::cast_to<Node3D>(joint->get_node(joint->get_node_b()));
323 }
324
325 if (!node_body_a && !node_body_b) {
326 return;
327 }
328
329 Ref<Material> common_material = get_material("joint_material", p_gizmo);
330 Ref<Material> body_a_material = get_material("joint_body_a_material", p_gizmo);
331 Ref<Material> body_b_material = get_material("joint_body_b_material", p_gizmo);
332
333 Vector<Vector3> points;
334 Vector<Vector3> body_a_points;
335 Vector<Vector3> body_b_points;
336
337 if (Object::cast_to<PinJoint3D>(joint)) {
338 CreatePinJointGizmo(Transform3D(), points);
339 p_gizmo->add_collision_segments(points);
340 p_gizmo->add_lines(points, common_material);
341 }
342
343 HingeJoint3D *hinge = Object::cast_to<HingeJoint3D>(joint);
344 if (hinge) {
345 CreateHingeJointGizmo(
346 Transform3D(),
347 hinge->get_global_transform(),
348 node_body_a ? node_body_a->get_global_transform() : Transform3D(),
349 node_body_b ? node_body_b->get_global_transform() : Transform3D(),
350 hinge->get_param(HingeJoint3D::PARAM_LIMIT_LOWER),
351 hinge->get_param(HingeJoint3D::PARAM_LIMIT_UPPER),
352 hinge->get_flag(HingeJoint3D::FLAG_USE_LIMIT),
353 points,
354 node_body_a ? &body_a_points : nullptr,
355 node_body_b ? &body_b_points : nullptr);
356
357 p_gizmo->add_collision_segments(points);
358 p_gizmo->add_collision_segments(body_a_points);
359 p_gizmo->add_collision_segments(body_b_points);
360
361 p_gizmo->add_lines(points, common_material);
362 p_gizmo->add_lines(body_a_points, body_a_material);
363 p_gizmo->add_lines(body_b_points, body_b_material);
364 }
365
366 SliderJoint3D *slider = Object::cast_to<SliderJoint3D>(joint);
367 if (slider) {
368 CreateSliderJointGizmo(
369 Transform3D(),
370 slider->get_global_transform(),
371 node_body_a ? node_body_a->get_global_transform() : Transform3D(),
372 node_body_b ? node_body_b->get_global_transform() : Transform3D(),
373 slider->get_param(SliderJoint3D::PARAM_ANGULAR_LIMIT_LOWER),
374 slider->get_param(SliderJoint3D::PARAM_ANGULAR_LIMIT_UPPER),
375 slider->get_param(SliderJoint3D::PARAM_LINEAR_LIMIT_LOWER),
376 slider->get_param(SliderJoint3D::PARAM_LINEAR_LIMIT_UPPER),
377 points,
378 node_body_a ? &body_a_points : nullptr,
379 node_body_b ? &body_b_points : nullptr);
380
381 p_gizmo->add_collision_segments(points);
382 p_gizmo->add_collision_segments(body_a_points);
383 p_gizmo->add_collision_segments(body_b_points);
384
385 p_gizmo->add_lines(points, common_material);
386 p_gizmo->add_lines(body_a_points, body_a_material);
387 p_gizmo->add_lines(body_b_points, body_b_material);
388 }
389
390 ConeTwistJoint3D *cone = Object::cast_to<ConeTwistJoint3D>(joint);
391 if (cone) {
392 CreateConeTwistJointGizmo(
393 Transform3D(),
394 cone->get_global_transform(),
395 node_body_a ? node_body_a->get_global_transform() : Transform3D(),
396 node_body_b ? node_body_b->get_global_transform() : Transform3D(),
397 cone->get_param(ConeTwistJoint3D::PARAM_SWING_SPAN),
398 cone->get_param(ConeTwistJoint3D::PARAM_TWIST_SPAN),
399 node_body_a ? &body_a_points : nullptr,
400 node_body_b ? &body_b_points : nullptr);
401
402 p_gizmo->add_collision_segments(body_a_points);
403 p_gizmo->add_collision_segments(body_b_points);
404
405 p_gizmo->add_lines(body_a_points, body_a_material);
406 p_gizmo->add_lines(body_b_points, body_b_material);
407 }
408
409 Generic6DOFJoint3D *gen = Object::cast_to<Generic6DOFJoint3D>(joint);
410 if (gen) {
411 CreateGeneric6DOFJointGizmo(
412 Transform3D(),
413 gen->get_global_transform(),
414 node_body_a ? node_body_a->get_global_transform() : Transform3D(),
415 node_body_b ? node_body_b->get_global_transform() : Transform3D(),
416
417 gen->get_param_x(Generic6DOFJoint3D::PARAM_ANGULAR_LOWER_LIMIT),
418 gen->get_param_x(Generic6DOFJoint3D::PARAM_ANGULAR_UPPER_LIMIT),
419 gen->get_param_x(Generic6DOFJoint3D::PARAM_LINEAR_LOWER_LIMIT),
420 gen->get_param_x(Generic6DOFJoint3D::PARAM_LINEAR_UPPER_LIMIT),
421 gen->get_flag_x(Generic6DOFJoint3D::FLAG_ENABLE_ANGULAR_LIMIT),
422 gen->get_flag_x(Generic6DOFJoint3D::FLAG_ENABLE_LINEAR_LIMIT),
423
424 gen->get_param_y(Generic6DOFJoint3D::PARAM_ANGULAR_LOWER_LIMIT),
425 gen->get_param_y(Generic6DOFJoint3D::PARAM_ANGULAR_UPPER_LIMIT),
426 gen->get_param_y(Generic6DOFJoint3D::PARAM_LINEAR_LOWER_LIMIT),
427 gen->get_param_y(Generic6DOFJoint3D::PARAM_LINEAR_UPPER_LIMIT),
428 gen->get_flag_y(Generic6DOFJoint3D::FLAG_ENABLE_ANGULAR_LIMIT),
429 gen->get_flag_y(Generic6DOFJoint3D::FLAG_ENABLE_LINEAR_LIMIT),
430
431 gen->get_param_z(Generic6DOFJoint3D::PARAM_ANGULAR_LOWER_LIMIT),
432 gen->get_param_z(Generic6DOFJoint3D::PARAM_ANGULAR_UPPER_LIMIT),
433 gen->get_param_z(Generic6DOFJoint3D::PARAM_LINEAR_LOWER_LIMIT),
434 gen->get_param_z(Generic6DOFJoint3D::PARAM_LINEAR_UPPER_LIMIT),
435 gen->get_flag_z(Generic6DOFJoint3D::FLAG_ENABLE_ANGULAR_LIMIT),
436 gen->get_flag_z(Generic6DOFJoint3D::FLAG_ENABLE_LINEAR_LIMIT),
437
438 points,
439 node_body_a ? &body_a_points : nullptr,
440 node_body_a ? &body_b_points : nullptr);
441
442 p_gizmo->add_collision_segments(points);
443 p_gizmo->add_collision_segments(body_a_points);
444 p_gizmo->add_collision_segments(body_b_points);
445
446 p_gizmo->add_lines(points, common_material);
447 p_gizmo->add_lines(body_a_points, body_a_material);
448 p_gizmo->add_lines(body_b_points, body_b_material);
449 }
450}
451
452void Joint3DGizmoPlugin::CreatePinJointGizmo(const Transform3D &p_offset, Vector<Vector3> &r_cursor_points) {
453 float cs = 0.25;
454
455 r_cursor_points.push_back(p_offset.translated_local(Vector3(+cs, 0, 0)).origin);
456 r_cursor_points.push_back(p_offset.translated_local(Vector3(-cs, 0, 0)).origin);
457 r_cursor_points.push_back(p_offset.translated_local(Vector3(0, +cs, 0)).origin);
458 r_cursor_points.push_back(p_offset.translated_local(Vector3(0, -cs, 0)).origin);
459 r_cursor_points.push_back(p_offset.translated_local(Vector3(0, 0, +cs)).origin);
460 r_cursor_points.push_back(p_offset.translated_local(Vector3(0, 0, -cs)).origin);
461}
462
463void Joint3DGizmoPlugin::CreateHingeJointGizmo(const Transform3D &p_offset, const Transform3D &p_trs_joint, const Transform3D &p_trs_body_a, const Transform3D &p_trs_body_b, real_t p_limit_lower, real_t p_limit_upper, bool p_use_limit, Vector<Vector3> &r_common_points, Vector<Vector3> *r_body_a_points, Vector<Vector3> *r_body_b_points) {
464 r_common_points.push_back(p_offset.translated_local(Vector3(0, 0, 0.5)).origin);
465 r_common_points.push_back(p_offset.translated_local(Vector3(0, 0, -0.5)).origin);
466
467 if (!p_use_limit) {
468 p_limit_upper = -1;
469 p_limit_lower = 0;
470 }
471
472 if (r_body_a_points) {
473 JointGizmosDrawer::draw_circle(Vector3::AXIS_Z,
474 BODY_A_RADIUS,
475 p_offset,
476 JointGizmosDrawer::look_body_toward_z(p_trs_joint, p_trs_body_a),
477 p_limit_lower,
478 p_limit_upper,
479 *r_body_a_points);
480 }
481
482 if (r_body_b_points) {
483 JointGizmosDrawer::draw_circle(Vector3::AXIS_Z,
484 BODY_B_RADIUS,
485 p_offset,
486 JointGizmosDrawer::look_body_toward_z(p_trs_joint, p_trs_body_b),
487 p_limit_lower,
488 p_limit_upper,
489 *r_body_b_points);
490 }
491}
492
493void Joint3DGizmoPlugin::CreateSliderJointGizmo(const Transform3D &p_offset, const Transform3D &p_trs_joint, const Transform3D &p_trs_body_a, const Transform3D &p_trs_body_b, real_t p_angular_limit_lower, real_t p_angular_limit_upper, real_t p_linear_limit_lower, real_t p_linear_limit_upper, Vector<Vector3> &r_points, Vector<Vector3> *r_body_a_points, Vector<Vector3> *r_body_b_points) {
494 p_linear_limit_lower = -p_linear_limit_lower;
495 p_linear_limit_upper = -p_linear_limit_upper;
496
497 float cs = 0.25;
498 r_points.push_back(p_offset.translated_local(Vector3(0, 0, 0.5)).origin);
499 r_points.push_back(p_offset.translated_local(Vector3(0, 0, -0.5)).origin);
500
501 if (p_linear_limit_lower >= p_linear_limit_upper) {
502 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, 0, 0)).origin);
503 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, 0, 0)).origin);
504
505 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, -cs, -cs)).origin);
506 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, -cs, cs)).origin);
507 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, -cs, cs)).origin);
508 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, cs, cs)).origin);
509 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, cs, cs)).origin);
510 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, cs, -cs)).origin);
511 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, cs, -cs)).origin);
512 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, -cs, -cs)).origin);
513
514 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, -cs, -cs)).origin);
515 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, -cs, cs)).origin);
516 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, -cs, cs)).origin);
517 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, cs, cs)).origin);
518 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, cs, cs)).origin);
519 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, cs, -cs)).origin);
520 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, cs, -cs)).origin);
521 r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, -cs, -cs)).origin);
522
523 } else {
524 r_points.push_back(p_offset.translated_local(Vector3(+cs * 2, 0, 0)).origin);
525 r_points.push_back(p_offset.translated_local(Vector3(-cs * 2, 0, 0)).origin);
526 }
527
528 if (r_body_a_points) {
529 JointGizmosDrawer::draw_circle(
530 Vector3::AXIS_X,
531 BODY_A_RADIUS,
532 p_offset,
533 JointGizmosDrawer::look_body_toward(Vector3::AXIS_X, p_trs_joint, p_trs_body_a),
534 p_angular_limit_lower,
535 p_angular_limit_upper,
536 *r_body_a_points);
537 }
538
539 if (r_body_b_points) {
540 JointGizmosDrawer::draw_circle(
541 Vector3::AXIS_X,
542 BODY_B_RADIUS,
543 p_offset,
544 JointGizmosDrawer::look_body_toward(Vector3::AXIS_X, p_trs_joint, p_trs_body_b),
545 p_angular_limit_lower,
546 p_angular_limit_upper,
547 *r_body_b_points,
548 true);
549 }
550}
551
552void Joint3DGizmoPlugin::CreateConeTwistJointGizmo(const Transform3D &p_offset, const Transform3D &p_trs_joint, const Transform3D &p_trs_body_a, const Transform3D &p_trs_body_b, real_t p_swing, real_t p_twist, Vector<Vector3> *r_body_a_points, Vector<Vector3> *r_body_b_points) {
553 if (r_body_a_points) {
554 JointGizmosDrawer::draw_cone(
555 p_offset,
556 JointGizmosDrawer::look_body(p_trs_joint, p_trs_body_a),
557 p_swing,
558 p_twist,
559 *r_body_a_points);
560 }
561
562 if (r_body_b_points) {
563 JointGizmosDrawer::draw_cone(
564 p_offset,
565 JointGizmosDrawer::look_body(p_trs_joint, p_trs_body_b),
566 p_swing,
567 p_twist,
568 *r_body_b_points);
569 }
570}
571
572void Joint3DGizmoPlugin::CreateGeneric6DOFJointGizmo(
573 const Transform3D &p_offset,
574 const Transform3D &p_trs_joint,
575 const Transform3D &p_trs_body_a,
576 const Transform3D &p_trs_body_b,
577 real_t p_angular_limit_lower_x,
578 real_t p_angular_limit_upper_x,
579 real_t p_linear_limit_lower_x,
580 real_t p_linear_limit_upper_x,
581 bool p_enable_angular_limit_x,
582 bool p_enable_linear_limit_x,
583 real_t p_angular_limit_lower_y,
584 real_t p_angular_limit_upper_y,
585 real_t p_linear_limit_lower_y,
586 real_t p_linear_limit_upper_y,
587 bool p_enable_angular_limit_y,
588 bool p_enable_linear_limit_y,
589 real_t p_angular_limit_lower_z,
590 real_t p_angular_limit_upper_z,
591 real_t p_linear_limit_lower_z,
592 real_t p_linear_limit_upper_z,
593 bool p_enable_angular_limit_z,
594 bool p_enable_linear_limit_z,
595 Vector<Vector3> &r_points,
596 Vector<Vector3> *r_body_a_points,
597 Vector<Vector3> *r_body_b_points) {
598 float cs = 0.25;
599
600 for (int ax = 0; ax < 3; ax++) {
601 float ll = 0;
602 float ul = 0;
603 float lll = 0;
604 float lul = 0;
605
606 int a1 = 0;
607 int a2 = 0;
608 int a3 = 0;
609 bool enable_ang = false;
610 bool enable_lin = false;
611
612 switch (ax) {
613 case 0:
614 ll = p_angular_limit_lower_x;
615 ul = p_angular_limit_upper_x;
616 lll = -p_linear_limit_lower_x;
617 lul = -p_linear_limit_upper_x;
618 enable_ang = p_enable_angular_limit_x;
619 enable_lin = p_enable_linear_limit_x;
620 a1 = 0;
621 a2 = 1;
622 a3 = 2;
623 break;
624 case 1:
625 ll = p_angular_limit_lower_y;
626 ul = p_angular_limit_upper_y;
627 lll = -p_linear_limit_lower_y;
628 lul = -p_linear_limit_upper_y;
629 enable_ang = p_enable_angular_limit_y;
630 enable_lin = p_enable_linear_limit_y;
631 a1 = 1;
632 a2 = 2;
633 a3 = 0;
634 break;
635 case 2:
636 ll = p_angular_limit_lower_z;
637 ul = p_angular_limit_upper_z;
638 lll = -p_linear_limit_lower_z;
639 lul = -p_linear_limit_upper_z;
640 enable_ang = p_enable_angular_limit_z;
641 enable_lin = p_enable_linear_limit_z;
642 a1 = 2;
643 a2 = 0;
644 a3 = 1;
645 break;
646 }
647
648#define ADD_VTX(x, y, z) \
649 { \
650 Vector3 v; \
651 v[a1] = (x); \
652 v[a2] = (y); \
653 v[a3] = (z); \
654 r_points.push_back(p_offset.translated_local(v).origin); \
655 }
656
657 if (enable_lin && lll >= lul) {
658 ADD_VTX(lul, 0, 0);
659 ADD_VTX(lll, 0, 0);
660
661 ADD_VTX(lul, -cs, -cs);
662 ADD_VTX(lul, -cs, cs);
663 ADD_VTX(lul, -cs, cs);
664 ADD_VTX(lul, cs, cs);
665 ADD_VTX(lul, cs, cs);
666 ADD_VTX(lul, cs, -cs);
667 ADD_VTX(lul, cs, -cs);
668 ADD_VTX(lul, -cs, -cs);
669
670 ADD_VTX(lll, -cs, -cs);
671 ADD_VTX(lll, -cs, cs);
672 ADD_VTX(lll, -cs, cs);
673 ADD_VTX(lll, cs, cs);
674 ADD_VTX(lll, cs, cs);
675 ADD_VTX(lll, cs, -cs);
676 ADD_VTX(lll, cs, -cs);
677 ADD_VTX(lll, -cs, -cs);
678
679 } else {
680 ADD_VTX(+cs * 2, 0, 0);
681 ADD_VTX(-cs * 2, 0, 0);
682 }
683
684 if (!enable_ang) {
685 ll = 0;
686 ul = -1;
687 }
688
689 if (r_body_a_points) {
690 JointGizmosDrawer::draw_circle(
691 static_cast<Vector3::Axis>(ax),
692 BODY_A_RADIUS,
693 p_offset,
694 JointGizmosDrawer::look_body_toward(static_cast<Vector3::Axis>(ax), p_trs_joint, p_trs_body_a),
695 ll,
696 ul,
697 *r_body_a_points,
698 true);
699 }
700
701 if (r_body_b_points) {
702 JointGizmosDrawer::draw_circle(
703 static_cast<Vector3::Axis>(ax),
704 BODY_B_RADIUS,
705 p_offset,
706 JointGizmosDrawer::look_body_toward(static_cast<Vector3::Axis>(ax), p_trs_joint, p_trs_body_b),
707 ll,
708 ul,
709 *r_body_b_points);
710 }
711 }
712
713#undef ADD_VTX
714}
715