1/**************************************************************************/
2/* godot_generic_6dof_joint_3d.h */
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#ifndef GODOT_GENERIC_6DOF_JOINT_3D_H
32#define GODOT_GENERIC_6DOF_JOINT_3D_H
33
34/*
35Adapted to Godot from the Bullet library.
36*/
37
38#include "servers/physics_3d/godot_joint_3d.h"
39#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
40
41/*
42Bullet Continuous Collision Detection and Physics Library
43Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
44
45This software is provided 'as-is', without any express or implied warranty.
46In no event will the authors be held liable for any damages arising from the use of this software.
47Permission is granted to anyone to use this software for any purpose,
48including commercial applications, and to alter it and redistribute it freely,
49subject to the following restrictions:
50
511. 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.
522. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
533. This notice may not be removed or altered from any source distribution.
54*/
55
56/*
572007-09-09
58GodotGeneric6DOFJoint3D Refactored by Francisco Le?n
59email: projectileman@yahoo.com
60http://gimpact.sf.net
61*/
62
63//! Rotation Limit structure for generic joints
64class GodotG6DOFRotationalLimitMotor3D {
65public:
66 //! limit_parameters
67 //!@{
68 real_t m_loLimit = -1e30; //!< joint limit
69 real_t m_hiLimit = 1e30; //!< joint limit
70 real_t m_targetVelocity = 0.0; //!< target motor velocity
71 real_t m_maxMotorForce = 0.1; //!< max force on motor
72 real_t m_maxLimitForce = 300.0; //!< max force on limit
73 real_t m_damping = 1.0; //!< Damping.
74 real_t m_limitSoftness = 0.5; //! Relaxation factor
75 real_t m_ERP = 0.5; //!< Error tolerance factor when joint is at limit
76 real_t m_bounce = 0.0; //!< restitution factor
77 bool m_enableMotor = false;
78 bool m_enableLimit = false;
79
80 //!@}
81
82 //! temp_variables
83 //!@{
84 real_t m_currentLimitError = 0.0; //!< How much is violated this limit
85 int m_currentLimit = 0; //!< 0=free, 1=at lo limit, 2=at hi limit
86 real_t m_accumulatedImpulse = 0.0;
87 //!@}
88
89 GodotG6DOFRotationalLimitMotor3D() {}
90
91 bool isLimited() {
92 return (m_loLimit < m_hiLimit);
93 }
94
95 // Need apply correction.
96 bool needApplyTorques() {
97 return (m_enableMotor || m_currentLimit != 0);
98 }
99
100 // Calculates m_currentLimit and m_currentLimitError.
101 int testLimitValue(real_t test_value);
102
103 // Apply the correction impulses for two bodies.
104 real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic);
105};
106
107class GodotG6DOFTranslationalLimitMotor3D {
108public:
109 Vector3 m_lowerLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint lower limits
110 Vector3 m_upperLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint upper limits
111 Vector3 m_accumulatedImpulse = Vector3(0.0, 0.0, 0.0);
112 //! Linear_Limit_parameters
113 //!@{
114 Vector3 m_limitSoftness = Vector3(0.7, 0.7, 0.7); //!< Softness for linear limit
115 Vector3 m_damping = Vector3(1.0, 1.0, 1.0); //!< Damping for linear limit
116 Vector3 m_restitution = Vector3(0.5, 0.5, 0.5); //! Bounce parameter for linear limit
117 //!@}
118 bool enable_limit[3] = { true, true, true };
119
120 //! Test limit
121 /*!
122 * - free means upper < lower,
123 * - locked means upper == lower
124 * - limited means upper > lower
125 * - limitIndex: first 3 are linear, next 3 are angular
126 */
127 inline bool isLimited(int limitIndex) {
128 return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
129 }
130
131 real_t solveLinearAxis(
132 real_t timeStep,
133 real_t jacDiagABInv,
134 GodotBody3D *body1, const Vector3 &pointInA,
135 GodotBody3D *body2, const Vector3 &pointInB,
136 bool p_body1_dynamic, bool p_body2_dynamic,
137 int limit_index,
138 const Vector3 &axis_normal_on_a,
139 const Vector3 &anchorPos);
140};
141
142class GodotGeneric6DOFJoint3D : public GodotJoint3D {
143protected:
144 union {
145 struct {
146 GodotBody3D *A;
147 GodotBody3D *B;
148 };
149
150 GodotBody3D *_arr[2] = { nullptr, nullptr };
151 };
152
153 //! relative_frames
154 //!@{
155 Transform3D m_frameInA; //!< the constraint space w.r.t body A
156 Transform3D m_frameInB; //!< the constraint space w.r.t body B
157 //!@}
158
159 //! Jacobians
160 //!@{
161 GodotJacobianEntry3D m_jacLinear[3]; //!< 3 orthogonal linear constraints
162 GodotJacobianEntry3D m_jacAng[3]; //!< 3 orthogonal angular constraints
163 //!@}
164
165 //! Linear_Limit_parameters
166 //!@{
167 GodotG6DOFTranslationalLimitMotor3D m_linearLimits;
168 //!@}
169
170 //! hinge_parameters
171 //!@{
172 GodotG6DOFRotationalLimitMotor3D m_angularLimits[3];
173 //!@}
174
175protected:
176 //! temporal variables
177 //!@{
178 real_t m_timeStep = 0.0;
179 Transform3D m_calculatedTransformA;
180 Transform3D m_calculatedTransformB;
181 Vector3 m_calculatedAxisAngleDiff;
182 Vector3 m_calculatedAxis[3];
183
184 Vector3 m_AnchorPos; // point between pivots of bodies A and B to solve linear axes
185
186 bool m_useLinearReferenceFrameA = false;
187
188 //!@}
189
190 GodotGeneric6DOFJoint3D(GodotGeneric6DOFJoint3D const &) = delete;
191 void operator=(GodotGeneric6DOFJoint3D const &) = delete;
192
193 void buildLinearJacobian(
194 GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld,
195 const Vector3 &pivotAInW, const Vector3 &pivotBInW);
196
197 void buildAngularJacobian(GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW);
198
199 //! calcs the euler angles between the two bodies.
200 void calculateAngleInfo();
201
202public:
203 GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA);
204
205 virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; }
206
207 virtual bool setup(real_t p_step) override;
208 virtual void solve(real_t p_step) override;
209
210 // Calcs the global transform for the joint offset for body A an B, and also calcs the angle differences between the bodies.
211 void calculateTransforms();
212
213 // Gets the global transform of the offset for body A.
214 const Transform3D &getCalculatedTransformA() const {
215 return m_calculatedTransformA;
216 }
217
218 // Gets the global transform of the offset for body B.
219 const Transform3D &getCalculatedTransformB() const {
220 return m_calculatedTransformB;
221 }
222
223 const Transform3D &getFrameOffsetA() const {
224 return m_frameInA;
225 }
226
227 const Transform3D &getFrameOffsetB() const {
228 return m_frameInB;
229 }
230
231 Transform3D &getFrameOffsetA() {
232 return m_frameInA;
233 }
234
235 Transform3D &getFrameOffsetB() {
236 return m_frameInB;
237 }
238
239 // Performs Jacobian calculation, and also calculates angle differences and axis.
240 void updateRHS(real_t timeStep);
241
242 // Get the rotation axis in global coordinates.
243 Vector3 getAxis(int axis_index) const;
244
245 // Get the relative Euler angle.
246 real_t getAngle(int axis_index) const;
247
248 // Calculates angular correction and returns true if limit needs to be corrected.
249 bool testAngularLimitMotor(int axis_index);
250
251 void setLinearLowerLimit(const Vector3 &linearLower) {
252 m_linearLimits.m_lowerLimit = linearLower;
253 }
254
255 void setLinearUpperLimit(const Vector3 &linearUpper) {
256 m_linearLimits.m_upperLimit = linearUpper;
257 }
258
259 void setAngularLowerLimit(const Vector3 &angularLower) {
260 m_angularLimits[0].m_loLimit = angularLower.x;
261 m_angularLimits[1].m_loLimit = angularLower.y;
262 m_angularLimits[2].m_loLimit = angularLower.z;
263 }
264
265 void setAngularUpperLimit(const Vector3 &angularUpper) {
266 m_angularLimits[0].m_hiLimit = angularUpper.x;
267 m_angularLimits[1].m_hiLimit = angularUpper.y;
268 m_angularLimits[2].m_hiLimit = angularUpper.z;
269 }
270
271 // Retrieves the angular limit information.
272 GodotG6DOFRotationalLimitMotor3D *getRotationalLimitMotor(int index) {
273 return &m_angularLimits[index];
274 }
275
276 // Retrieves the limit information.
277 GodotG6DOFTranslationalLimitMotor3D *getTranslationalLimitMotor() {
278 return &m_linearLimits;
279 }
280
281 // First 3 are linear, next 3 are angular.
282 void setLimit(int axis, real_t lo, real_t hi) {
283 if (axis < 3) {
284 m_linearLimits.m_lowerLimit[axis] = lo;
285 m_linearLimits.m_upperLimit[axis] = hi;
286 } else {
287 m_angularLimits[axis - 3].m_loLimit = lo;
288 m_angularLimits[axis - 3].m_hiLimit = hi;
289 }
290 }
291
292 //! Test limit
293 /*!
294 * - free means upper < lower,
295 * - locked means upper == lower
296 * - limited means upper > lower
297 * - limitIndex: first 3 are linear, next 3 are angular
298 */
299 bool isLimited(int limitIndex) {
300 if (limitIndex < 3) {
301 return m_linearLimits.isLimited(limitIndex);
302 }
303 return m_angularLimits[limitIndex - 3].isLimited();
304 }
305
306 const GodotBody3D *getRigidBodyA() const {
307 return A;
308 }
309 const GodotBody3D *getRigidBodyB() const {
310 return B;
311 }
312
313 virtual void calcAnchorPos(); // overridable
314
315 void set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value);
316 real_t get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const;
317
318 void set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value);
319 bool get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const;
320};
321
322#endif // GODOT_GENERIC_6DOF_JOINT_3D_H
323