1/**************************************************************************/
2/* godot_slider_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_SLIDER_JOINT_3D_H
32#define GODOT_SLIDER_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/*
57Added by Roman Ponomarev (rponom@gmail.com)
58April 04, 2008
59
60*/
61
62#define SLIDER_CONSTRAINT_DEF_SOFTNESS (real_t(1.0))
63#define SLIDER_CONSTRAINT_DEF_DAMPING (real_t(1.0))
64#define SLIDER_CONSTRAINT_DEF_RESTITUTION (real_t(0.7))
65
66//-----------------------------------------------------------------------------
67
68class GodotSliderJoint3D : public GodotJoint3D {
69protected:
70 union {
71 struct {
72 GodotBody3D *A;
73 GodotBody3D *B;
74 };
75
76 GodotBody3D *_arr[2] = { nullptr, nullptr };
77 };
78
79 Transform3D m_frameInA;
80 Transform3D m_frameInB;
81
82 // linear limits
83 real_t m_lowerLinLimit = 1.0;
84 real_t m_upperLinLimit = -1.0;
85 // angular limits
86 real_t m_lowerAngLimit = 0.0;
87 real_t m_upperAngLimit = 0.0;
88 // softness, restitution and damping for different cases
89 // DirLin - moving inside linear limits
90 // LimLin - hitting linear limit
91 // DirAng - moving inside angular limits
92 // LimAng - hitting angular limit
93 // OrthoLin, OrthoAng - against constraint axis
94 real_t m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
95 real_t m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
96 real_t m_dampingDirLin = 0.0;
97 real_t m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
98 real_t m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
99 real_t m_dampingDirAng = 0.0;
100 real_t m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
101 real_t m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
102 real_t m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
103 real_t m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
104 real_t m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
105 real_t m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
106 real_t m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
107 real_t m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
108 real_t m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
109 real_t m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
110 real_t m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
111 real_t m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
112
113 // for interlal use
114 bool m_solveLinLim = false;
115 bool m_solveAngLim = false;
116
117 GodotJacobianEntry3D m_jacLin[3] = {};
118 real_t m_jacLinDiagABInv[3] = {};
119
120 GodotJacobianEntry3D m_jacAng[3] = {};
121
122 real_t m_timeStep = 0.0;
123 Transform3D m_calculatedTransformA;
124 Transform3D m_calculatedTransformB;
125
126 Vector3 m_sliderAxis;
127 Vector3 m_realPivotAInW;
128 Vector3 m_realPivotBInW;
129 Vector3 m_projPivotInW;
130 Vector3 m_delta;
131 Vector3 m_depth;
132 Vector3 m_relPosA;
133 Vector3 m_relPosB;
134
135 real_t m_linPos = 0.0;
136
137 real_t m_angDepth = 0.0;
138 real_t m_kAngle = 0.0;
139
140 bool m_poweredLinMotor = false;
141 real_t m_targetLinMotorVelocity = 0.0;
142 real_t m_maxLinMotorForce = 0.0;
143 real_t m_accumulatedLinMotorImpulse = 0.0;
144
145 bool m_poweredAngMotor = false;
146 real_t m_targetAngMotorVelocity = 0.0;
147 real_t m_maxAngMotorForce = 0.0;
148 real_t m_accumulatedAngMotorImpulse = 0.0;
149
150public:
151 // constructors
152 GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB);
153 //SliderJointSW();
154 // overrides
155
156 // access
157 const GodotBody3D *getRigidBodyA() const { return A; }
158 const GodotBody3D *getRigidBodyB() const { return B; }
159 const Transform3D &getCalculatedTransformA() const { return m_calculatedTransformA; }
160 const Transform3D &getCalculatedTransformB() const { return m_calculatedTransformB; }
161 const Transform3D &getFrameOffsetA() const { return m_frameInA; }
162 const Transform3D &getFrameOffsetB() const { return m_frameInB; }
163 Transform3D &getFrameOffsetA() { return m_frameInA; }
164 Transform3D &getFrameOffsetB() { return m_frameInB; }
165 real_t getLowerLinLimit() { return m_lowerLinLimit; }
166 void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; }
167 real_t getUpperLinLimit() { return m_upperLinLimit; }
168 void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; }
169 real_t getLowerAngLimit() { return m_lowerAngLimit; }
170 void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; }
171 real_t getUpperAngLimit() { return m_upperAngLimit; }
172 void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; }
173
174 real_t getSoftnessDirLin() { return m_softnessDirLin; }
175 real_t getRestitutionDirLin() { return m_restitutionDirLin; }
176 real_t getDampingDirLin() { return m_dampingDirLin; }
177 real_t getSoftnessDirAng() { return m_softnessDirAng; }
178 real_t getRestitutionDirAng() { return m_restitutionDirAng; }
179 real_t getDampingDirAng() { return m_dampingDirAng; }
180 real_t getSoftnessLimLin() { return m_softnessLimLin; }
181 real_t getRestitutionLimLin() { return m_restitutionLimLin; }
182 real_t getDampingLimLin() { return m_dampingLimLin; }
183 real_t getSoftnessLimAng() { return m_softnessLimAng; }
184 real_t getRestitutionLimAng() { return m_restitutionLimAng; }
185 real_t getDampingLimAng() { return m_dampingLimAng; }
186 real_t getSoftnessOrthoLin() { return m_softnessOrthoLin; }
187 real_t getRestitutionOrthoLin() { return m_restitutionOrthoLin; }
188 real_t getDampingOrthoLin() { return m_dampingOrthoLin; }
189 real_t getSoftnessOrthoAng() { return m_softnessOrthoAng; }
190 real_t getRestitutionOrthoAng() { return m_restitutionOrthoAng; }
191 real_t getDampingOrthoAng() { return m_dampingOrthoAng; }
192 void setSoftnessDirLin(real_t softnessDirLin) { m_softnessDirLin = softnessDirLin; }
193 void setRestitutionDirLin(real_t restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; }
194 void setDampingDirLin(real_t dampingDirLin) { m_dampingDirLin = dampingDirLin; }
195 void setSoftnessDirAng(real_t softnessDirAng) { m_softnessDirAng = softnessDirAng; }
196 void setRestitutionDirAng(real_t restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; }
197 void setDampingDirAng(real_t dampingDirAng) { m_dampingDirAng = dampingDirAng; }
198 void setSoftnessLimLin(real_t softnessLimLin) { m_softnessLimLin = softnessLimLin; }
199 void setRestitutionLimLin(real_t restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; }
200 void setDampingLimLin(real_t dampingLimLin) { m_dampingLimLin = dampingLimLin; }
201 void setSoftnessLimAng(real_t softnessLimAng) { m_softnessLimAng = softnessLimAng; }
202 void setRestitutionLimAng(real_t restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; }
203 void setDampingLimAng(real_t dampingLimAng) { m_dampingLimAng = dampingLimAng; }
204 void setSoftnessOrthoLin(real_t softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; }
205 void setRestitutionOrthoLin(real_t restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; }
206 void setDampingOrthoLin(real_t dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; }
207 void setSoftnessOrthoAng(real_t softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; }
208 void setRestitutionOrthoAng(real_t restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; }
209 void setDampingOrthoAng(real_t dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; }
210 void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; }
211 bool getPoweredLinMotor() { return m_poweredLinMotor; }
212 void setTargetLinMotorVelocity(real_t targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; }
213 real_t getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; }
214 void setMaxLinMotorForce(real_t maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; }
215 real_t getMaxLinMotorForce() { return m_maxLinMotorForce; }
216 void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; }
217 bool getPoweredAngMotor() { return m_poweredAngMotor; }
218 void setTargetAngMotorVelocity(real_t targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; }
219 real_t getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
220 void setMaxAngMotorForce(real_t maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
221 real_t getMaxAngMotorForce() { return m_maxAngMotorForce; }
222 real_t getLinearPos() { return m_linPos; }
223
224 // access for ODE solver
225 bool getSolveLinLimit() { return m_solveLinLim; }
226 real_t getLinDepth() { return m_depth[0]; }
227 bool getSolveAngLimit() { return m_solveAngLim; }
228 real_t getAngDepth() { return m_angDepth; }
229 // shared code used by ODE solver
230 void calculateTransforms();
231 void testLinLimits();
232 void testAngLimits();
233 // access for PE Solver
234 Vector3 getAncorInA();
235 Vector3 getAncorInB();
236
237 void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value);
238 real_t get_param(PhysicsServer3D::SliderJointParam p_param) const;
239
240 virtual bool setup(real_t p_step) override;
241 virtual void solve(real_t p_step) override;
242
243 virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; }
244};
245
246#endif // GODOT_SLIDER_JOINT_3D_H
247