1 /*************************************************************************/
2 /* cone_twist_joint_sw.cpp */
3 /*************************************************************************/
4 /* This file is part of: */
5 /* GODOT ENGINE */
6 /* https://godotengine.org */
7 /*************************************************************************/
8 /* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
9 /* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */
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 /*
32 Adapted to Godot from the Bullet library.
33 */
34
35 /*
36 Bullet Continuous Collision Detection and Physics Library
37 ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios
38
39 This software is provided 'as-is', without any express or implied warranty.
40 In no event will the authors be held liable for any damages arising from the use of this software.
41 Permission is granted to anyone to use this software for any purpose,
42 including commercial applications, and to alter it and redistribute it freely,
43 subject to the following restrictions:
44
45 1. 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.
46 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
47 3. This notice may not be removed or altered from any source distribution.
48
49 Written by: Marcus Hennix
50 */
51
52 #include "cone_twist_joint_sw.h"
53
plane_space(const Vector3 & n,Vector3 & p,Vector3 & q)54 static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
55
56 if (Math::abs(n.z) > Math_SQRT12) {
57 // choose p in y-z plane
58 real_t a = n[1] * n[1] + n[2] * n[2];
59 real_t k = 1.0 / Math::sqrt(a);
60 p = Vector3(0, -n[2] * k, n[1] * k);
61 // set q = n x p
62 q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]);
63 } else {
64 // choose p in x-y plane
65 real_t a = n.x * n.x + n.y * n.y;
66 real_t k = 1.0 / Math::sqrt(a);
67 p = Vector3(-n.y * k, n.x * k, 0);
68 // set q = n x p
69 q = Vector3(-n.z * p.y, n.z * p.x, a * k);
70 }
71 }
72
atan2fast(real_t y,real_t x)73 static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
74 real_t coeff_1 = Math_PI / 4.0f;
75 real_t coeff_2 = 3.0f * coeff_1;
76 real_t abs_y = Math::abs(y);
77 real_t angle;
78 if (x >= 0.0f) {
79 real_t r = (x - abs_y) / (x + abs_y);
80 angle = coeff_1 - coeff_1 * r;
81 } else {
82 real_t r = (x + abs_y) / (abs_y - x);
83 angle = coeff_2 - coeff_1 * r;
84 }
85 return (y < 0.0f) ? -angle : angle;
86 }
87
ConeTwistJointSW(BodySW * rbA,BodySW * rbB,const Transform & rbAFrame,const Transform & rbBFrame)88 ConeTwistJointSW::ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &rbBFrame) :
89 JointSW(_arr, 2) {
90
91 A = rbA;
92 B = rbB;
93
94 m_rbAFrame = rbAFrame;
95 m_rbBFrame = rbBFrame;
96
97 m_swingSpan1 = Math_PI / 4.0;
98 m_swingSpan2 = Math_PI / 4.0;
99 m_twistSpan = Math_PI * 2;
100 m_biasFactor = 0.3f;
101 m_relaxationFactor = 1.0f;
102
103 m_angularOnly = false;
104 m_solveTwistLimit = false;
105 m_solveSwingLimit = false;
106
107 A->add_constraint(this, 0);
108 B->add_constraint(this, 1);
109
110 m_appliedImpulse = 0;
111 }
112
setup(real_t p_timestep)113 bool ConeTwistJointSW::setup(real_t p_timestep) {
114 m_appliedImpulse = real_t(0.);
115
116 //set bias, sign, clear accumulator
117 m_swingCorrection = real_t(0.);
118 m_twistLimitSign = real_t(0.);
119 m_solveTwistLimit = false;
120 m_solveSwingLimit = false;
121 m_accTwistLimitImpulse = real_t(0.);
122 m_accSwingLimitImpulse = real_t(0.);
123
124 if (!m_angularOnly) {
125 Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
126 Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
127 Vector3 relPos = pivotBInW - pivotAInW;
128
129 Vector3 normal[3];
130 if (Math::is_zero_approx(relPos.length_squared())) {
131 normal[0] = Vector3(real_t(1.0), 0, 0);
132 } else {
133 normal[0] = relPos.normalized();
134 }
135
136 plane_space(normal[0], normal[1], normal[2]);
137
138 for (int i = 0; i < 3; i++) {
139 memnew_placement(&m_jac[i], JacobianEntrySW(
140 A->get_principal_inertia_axes().transposed(),
141 B->get_principal_inertia_axes().transposed(),
142 pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
143 pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
144 normal[i],
145 A->get_inv_inertia(),
146 A->get_inv_mass(),
147 B->get_inv_inertia(),
148 B->get_inv_mass()));
149 }
150 }
151
152 Vector3 b1Axis1, b1Axis2, b1Axis3;
153 Vector3 b2Axis1, b2Axis2;
154
155 b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(0));
156 b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(0));
157
158 real_t swing1 = real_t(0.), swing2 = real_t(0.);
159
160 real_t swx = real_t(0.), swy = real_t(0.);
161 real_t thresh = real_t(10.);
162 real_t fact;
163
164 // Get Frame into world space
165 if (m_swingSpan1 >= real_t(0.05f)) {
166 b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(1));
167 //swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
168 swx = b2Axis1.dot(b1Axis1);
169 swy = b2Axis1.dot(b1Axis2);
170 swing1 = atan2fast(swy, swx);
171 fact = (swy * swy + swx * swx) * thresh * thresh;
172 fact = fact / (fact + real_t(1.0));
173 swing1 *= fact;
174 }
175
176 if (m_swingSpan2 >= real_t(0.05f)) {
177 b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(2));
178 //swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
179 swx = b2Axis1.dot(b1Axis1);
180 swy = b2Axis1.dot(b1Axis3);
181 swing2 = atan2fast(swy, swx);
182 fact = (swy * swy + swx * swx) * thresh * thresh;
183 fact = fact / (fact + real_t(1.0));
184 swing2 *= fact;
185 }
186
187 real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1);
188 real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2);
189 real_t EllipseAngle = Math::abs(swing1 * swing1) * RMaxAngle1Sq + Math::abs(swing2 * swing2) * RMaxAngle2Sq;
190
191 if (EllipseAngle > 1.0f) {
192 m_swingCorrection = EllipseAngle - 1.0f;
193 m_solveSwingLimit = true;
194
195 // Calculate necessary axis & factors
196 m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3));
197 m_swingAxis.normalize();
198
199 real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
200 m_swingAxis *= swingAxisSign;
201
202 m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) +
203 B->compute_angular_impulse_denominator(m_swingAxis));
204 }
205
206 // Twist limits
207 if (m_twistSpan >= real_t(0.)) {
208 Vector3 b2Axis22 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1));
209 Quat rotationArc = Quat(b2Axis1, b1Axis1);
210 Vector3 TwistRef = rotationArc.xform(b2Axis22);
211 real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
212
213 real_t lockedFreeFactor = (m_twistSpan > real_t(0.05f)) ? m_limitSoftness : real_t(0.);
214 if (twist <= -m_twistSpan * lockedFreeFactor) {
215 m_twistCorrection = -(twist + m_twistSpan);
216 m_solveTwistLimit = true;
217
218 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
219 m_twistAxis.normalize();
220 m_twistAxis *= -1.0f;
221
222 m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) +
223 B->compute_angular_impulse_denominator(m_twistAxis));
224
225 } else if (twist > m_twistSpan * lockedFreeFactor) {
226 m_twistCorrection = (twist - m_twistSpan);
227 m_solveTwistLimit = true;
228
229 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
230 m_twistAxis.normalize();
231
232 m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) +
233 B->compute_angular_impulse_denominator(m_twistAxis));
234 }
235 }
236
237 return true;
238 }
239
solve(real_t p_timestep)240 void ConeTwistJointSW::solve(real_t p_timestep) {
241
242 Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
243 Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
244
245 real_t tau = real_t(0.3);
246
247 //linear part
248 if (!m_angularOnly) {
249 Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
250 Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
251
252 Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1);
253 Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
254 Vector3 vel = vel1 - vel2;
255
256 for (int i = 0; i < 3; i++) {
257 const Vector3 &normal = m_jac[i].m_linearJointAxis;
258 real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
259
260 real_t rel_vel;
261 rel_vel = normal.dot(vel);
262 //positional error (zeroth order error)
263 real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
264 real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv;
265 m_appliedImpulse += impulse;
266 Vector3 impulse_vector = normal * impulse;
267 A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector);
268 B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector);
269 }
270 }
271
272 {
273 ///solve angular part
274 const Vector3 &angVelA = A->get_angular_velocity();
275 const Vector3 &angVelB = B->get_angular_velocity();
276
277 // solve swing limit
278 if (m_solveSwingLimit) {
279 real_t amplitude = ((angVelB - angVelA).dot(m_swingAxis) * m_relaxationFactor * m_relaxationFactor + m_swingCorrection * (real_t(1.) / p_timestep) * m_biasFactor);
280 real_t impulseMag = amplitude * m_kSwing;
281
282 // Clamp the accumulated impulse
283 real_t temp = m_accSwingLimitImpulse;
284 m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0));
285 impulseMag = m_accSwingLimitImpulse - temp;
286
287 Vector3 impulse = m_swingAxis * impulseMag;
288
289 A->apply_torque_impulse(impulse);
290 B->apply_torque_impulse(-impulse);
291 }
292
293 // solve twist limit
294 if (m_solveTwistLimit) {
295 real_t amplitude = ((angVelB - angVelA).dot(m_twistAxis) * m_relaxationFactor * m_relaxationFactor + m_twistCorrection * (real_t(1.) / p_timestep) * m_biasFactor);
296 real_t impulseMag = amplitude * m_kTwist;
297
298 // Clamp the accumulated impulse
299 real_t temp = m_accTwistLimitImpulse;
300 m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0));
301 impulseMag = m_accTwistLimitImpulse - temp;
302
303 Vector3 impulse = m_twistAxis * impulseMag;
304
305 A->apply_torque_impulse(impulse);
306 B->apply_torque_impulse(-impulse);
307 }
308 }
309 }
310
set_param(PhysicsServer::ConeTwistJointParam p_param,real_t p_value)311 void ConeTwistJointSW::set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value) {
312
313 switch (p_param) {
314 case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: {
315
316 m_swingSpan1 = p_value;
317 m_swingSpan2 = p_value;
318 } break;
319 case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: {
320
321 m_twistSpan = p_value;
322 } break;
323 case PhysicsServer::CONE_TWIST_JOINT_BIAS: {
324
325 m_biasFactor = p_value;
326 } break;
327 case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: {
328
329 m_limitSoftness = p_value;
330 } break;
331 case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: {
332
333 m_relaxationFactor = p_value;
334 } break;
335 case PhysicsServer::CONE_TWIST_MAX: break; // Can't happen, but silences warning
336 }
337 }
338
get_param(PhysicsServer::ConeTwistJointParam p_param) const339 real_t ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const {
340
341 switch (p_param) {
342 case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: {
343
344 return m_swingSpan1;
345 } break;
346 case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: {
347
348 return m_twistSpan;
349 } break;
350 case PhysicsServer::CONE_TWIST_JOINT_BIAS: {
351
352 return m_biasFactor;
353 } break;
354 case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: {
355
356 return m_limitSoftness;
357 } break;
358 case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: {
359
360 return m_relaxationFactor;
361 } break;
362 case PhysicsServer::CONE_TWIST_MAX: break; // Can't happen, but silences warning
363 }
364
365 return 0;
366 }
367