1/**************************************************************************/
2/* godot_cone_twist_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
37ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios
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
49Written by: Marcus Hennix
50*/
51
52#include "godot_cone_twist_joint_3d.h"
53
54GodotConeTwistJoint3D::GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) :
55 GodotJoint3D(_arr, 2) {
56 A = rbA;
57 B = rbB;
58
59 m_rbAFrame = rbAFrame;
60 m_rbBFrame = rbBFrame;
61
62 A->add_constraint(this, 0);
63 B->add_constraint(this, 1);
64}
65
66bool GodotConeTwistJoint3D::setup(real_t p_timestep) {
67 dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
68 dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
69
70 if (!dynamic_A && !dynamic_B) {
71 return false;
72 }
73
74 m_appliedImpulse = real_t(0.);
75
76 //set bias, sign, clear accumulator
77 m_swingCorrection = real_t(0.);
78 m_twistLimitSign = real_t(0.);
79 m_solveTwistLimit = false;
80 m_solveSwingLimit = false;
81 m_accTwistLimitImpulse = real_t(0.);
82 m_accSwingLimitImpulse = real_t(0.);
83
84 if (!m_angularOnly) {
85 Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
86 Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
87 Vector3 relPos = pivotBInW - pivotAInW;
88
89 Vector3 normal[3];
90 if (Math::is_zero_approx(relPos.length_squared())) {
91 normal[0] = Vector3(real_t(1.0), 0, 0);
92 } else {
93 normal[0] = relPos.normalized();
94 }
95
96 plane_space(normal[0], normal[1], normal[2]);
97
98 for (int i = 0; i < 3; i++) {
99 memnew_placement(
100 &m_jac[i],
101 GodotJacobianEntry3D(
102 A->get_principal_inertia_axes().transposed(),
103 B->get_principal_inertia_axes().transposed(),
104 pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
105 pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
106 normal[i],
107 A->get_inv_inertia(),
108 A->get_inv_mass(),
109 B->get_inv_inertia(),
110 B->get_inv_mass()));
111 }
112 }
113
114 Vector3 b1Axis1, b1Axis2, b1Axis3;
115 Vector3 b2Axis1, b2Axis2;
116
117 b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_column(0));
118 b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_column(0));
119
120 real_t swing1 = real_t(0.), swing2 = real_t(0.);
121
122 real_t swx = real_t(0.), swy = real_t(0.);
123 real_t thresh = real_t(10.);
124 real_t fact;
125
126 // Get Frame into world space
127 if (m_swingSpan1 >= real_t(0.05f)) {
128 b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_column(1));
129 //swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
130 swx = b2Axis1.dot(b1Axis1);
131 swy = b2Axis1.dot(b1Axis2);
132 swing1 = atan2fast(swy, swx);
133 fact = (swy * swy + swx * swx) * thresh * thresh;
134 fact = fact / (fact + real_t(1.0));
135 swing1 *= fact;
136 }
137
138 if (m_swingSpan2 >= real_t(0.05f)) {
139 b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_column(2));
140 //swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
141 swx = b2Axis1.dot(b1Axis1);
142 swy = b2Axis1.dot(b1Axis3);
143 swing2 = atan2fast(swy, swx);
144 fact = (swy * swy + swx * swx) * thresh * thresh;
145 fact = fact / (fact + real_t(1.0));
146 swing2 *= fact;
147 }
148
149 real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1);
150 real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2);
151 real_t EllipseAngle = Math::abs(swing1 * swing1) * RMaxAngle1Sq + Math::abs(swing2 * swing2) * RMaxAngle2Sq;
152
153 if (EllipseAngle > 1.0f) {
154 m_swingCorrection = EllipseAngle - 1.0f;
155 m_solveSwingLimit = true;
156
157 // Calculate necessary axis & factors
158 m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3));
159 m_swingAxis.normalize();
160
161 real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
162 m_swingAxis *= swingAxisSign;
163
164 m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) + B->compute_angular_impulse_denominator(m_swingAxis));
165 }
166
167 // Twist limits
168 if (m_twistSpan >= real_t(0.)) {
169 Vector3 b2Axis22 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_column(1));
170 Quaternion rotationArc = Quaternion(b2Axis1, b1Axis1);
171 Vector3 TwistRef = rotationArc.xform(b2Axis22);
172 real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
173
174 real_t lockedFreeFactor = (m_twistSpan > real_t(0.05f)) ? m_limitSoftness : real_t(0.);
175 if (twist <= -m_twistSpan * lockedFreeFactor) {
176 m_twistCorrection = -(twist + m_twistSpan);
177 m_solveTwistLimit = true;
178
179 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
180 m_twistAxis.normalize();
181 m_twistAxis *= -1.0f;
182
183 m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + B->compute_angular_impulse_denominator(m_twistAxis));
184
185 } else if (twist > m_twistSpan * lockedFreeFactor) {
186 m_twistCorrection = (twist - m_twistSpan);
187 m_solveTwistLimit = true;
188
189 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
190 m_twistAxis.normalize();
191
192 m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + B->compute_angular_impulse_denominator(m_twistAxis));
193 }
194 }
195
196 return true;
197}
198
199void GodotConeTwistJoint3D::solve(real_t p_timestep) {
200 Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
201 Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
202
203 real_t tau = real_t(0.3);
204
205 //linear part
206 if (!m_angularOnly) {
207 Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
208 Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
209
210 Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1);
211 Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
212 Vector3 vel = vel1 - vel2;
213
214 for (int i = 0; i < 3; i++) {
215 const Vector3 &normal = m_jac[i].m_linearJointAxis;
216 real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
217
218 real_t rel_vel;
219 rel_vel = normal.dot(vel);
220 //positional error (zeroth order error)
221 real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
222 real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv;
223 m_appliedImpulse += impulse;
224 Vector3 impulse_vector = normal * impulse;
225 if (dynamic_A) {
226 A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
227 }
228 if (dynamic_B) {
229 B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
230 }
231 }
232 }
233
234 {
235 ///solve angular part
236 const Vector3 &angVelA = A->get_angular_velocity();
237 const Vector3 &angVelB = B->get_angular_velocity();
238
239 // solve swing limit
240 if (m_solveSwingLimit) {
241 real_t amplitude = ((angVelB - angVelA).dot(m_swingAxis) * m_relaxationFactor * m_relaxationFactor + m_swingCorrection * (real_t(1.) / p_timestep) * m_biasFactor);
242 real_t impulseMag = amplitude * m_kSwing;
243
244 // Clamp the accumulated impulse
245 real_t temp = m_accSwingLimitImpulse;
246 m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0));
247 impulseMag = m_accSwingLimitImpulse - temp;
248
249 Vector3 impulse = m_swingAxis * impulseMag;
250
251 if (dynamic_A) {
252 A->apply_torque_impulse(impulse);
253 }
254 if (dynamic_B) {
255 B->apply_torque_impulse(-impulse);
256 }
257 }
258
259 // solve twist limit
260 if (m_solveTwistLimit) {
261 real_t amplitude = ((angVelB - angVelA).dot(m_twistAxis) * m_relaxationFactor * m_relaxationFactor + m_twistCorrection * (real_t(1.) / p_timestep) * m_biasFactor);
262 real_t impulseMag = amplitude * m_kTwist;
263
264 // Clamp the accumulated impulse
265 real_t temp = m_accTwistLimitImpulse;
266 m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0));
267 impulseMag = m_accTwistLimitImpulse - temp;
268
269 Vector3 impulse = m_twistAxis * impulseMag;
270
271 if (dynamic_A) {
272 A->apply_torque_impulse(impulse);
273 }
274 if (dynamic_B) {
275 B->apply_torque_impulse(-impulse);
276 }
277 }
278 }
279}
280
281void GodotConeTwistJoint3D::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) {
282 switch (p_param) {
283 case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: {
284 m_swingSpan1 = p_value;
285 m_swingSpan2 = p_value;
286 } break;
287 case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: {
288 m_twistSpan = p_value;
289 } break;
290 case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: {
291 m_biasFactor = p_value;
292 } break;
293 case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: {
294 m_limitSoftness = p_value;
295 } break;
296 case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: {
297 m_relaxationFactor = p_value;
298 } break;
299 case PhysicsServer3D::CONE_TWIST_MAX:
300 break; // Can't happen, but silences warning
301 }
302}
303
304real_t GodotConeTwistJoint3D::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const {
305 switch (p_param) {
306 case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: {
307 return m_swingSpan1;
308 } break;
309 case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: {
310 return m_twistSpan;
311 } break;
312 case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: {
313 return m_biasFactor;
314 } break;
315 case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: {
316 return m_limitSoftness;
317 } break;
318 case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: {
319 return m_relaxationFactor;
320 } break;
321 case PhysicsServer3D::CONE_TWIST_MAX:
322 break; // Can't happen, but silences warning
323 }
324
325 return 0;
326}
327