1//************************************ bs::framework - Copyright 2018 Marko Pintera **************************************//
2//*********** Licensed under the MIT license. See LICENSE.md for full terms. This notice is not to be removed. ***********//
3#include "Components/BsCJoint.h"
4#include "Components/BsCRigidbody.h"
5#include "Scene/BsSceneObject.h"
6#include "Physics/BsPhysics.h"
7#include "Private/RTTI/BsCJointRTTI.h"
8
9using namespace std::placeholders;
10
11namespace bs
12{
13 CJoint::CJoint(JOINT_DESC& desc)
14 :mDesc(desc)
15 {
16 mPositions[0] = Vector3::ZERO;
17 mPositions[1] = Vector3::ZERO;
18
19 mRotations[0] = Quaternion::IDENTITY;
20 mRotations[1] = Quaternion::IDENTITY;
21 }
22
23 CJoint::CJoint(const HSceneObject& parent, JOINT_DESC& desc)
24 : Component(parent), mDesc(desc)
25 {
26 setName("Joint");
27
28 mPositions[0] = Vector3::ZERO;
29 mPositions[1] = Vector3::ZERO;
30
31 mRotations[0] = Quaternion::IDENTITY;
32 mRotations[1] = Quaternion::IDENTITY;
33
34 mNotifyFlags = (TransformChangedFlags)(TCF_Parent | TCF_Transform);
35 }
36
37 HRigidbody CJoint::getBody(JointBody body) const
38 {
39 return mBodies[(int)body];
40 }
41
42 void CJoint::setBody(JointBody body, const HRigidbody& value)
43 {
44 if (mBodies[(int)body] == value)
45 return;
46
47 if (mBodies[(int)body] != nullptr)
48 mBodies[(int)body]->_setJoint(HJoint());
49
50 mBodies[(int)body] = value;
51
52 if (value != nullptr)
53 mBodies[(int)body]->_setJoint(static_object_cast<CJoint>(mThisHandle));
54
55 // If joint already exists, destroy it if we removed all bodies, otherwise update its transform
56 if(mInternal != nullptr)
57 {
58 if (!isBodyValid(mBodies[0]) && !isBodyValid(mBodies[1]))
59 destroyInternal();
60 else
61 {
62 Rigidbody* rigidbody = nullptr;
63 if (value != nullptr)
64 rigidbody = value->_getInternal();
65
66 mInternal->setBody(body, rigidbody);
67 updateTransform(body);
68 }
69 }
70 else // If joint doesn't exist, check if we can create it
71 {
72 // Must be an active component and at least one of the bodies must be non-null
73 if (SO()->getActive() && (isBodyValid(mBodies[0]) || isBodyValid(mBodies[1])))
74 {
75 restoreInternal();
76 }
77 }
78 }
79
80 Vector3 CJoint::getPosition(JointBody body) const
81 {
82 return mPositions[(int)body];
83 }
84
85 Quaternion CJoint::getRotation(JointBody body) const
86 {
87 return mRotations[(int)body];
88 }
89
90 void CJoint::setTransform(JointBody body, const Vector3& position, const Quaternion& rotation)
91 {
92 if (mPositions[(int)body] == position && mRotations[(int)body] == rotation)
93 return;
94
95 mPositions[(int)body] = position;
96 mRotations[(int)body] = rotation;
97
98 if (mInternal != nullptr)
99 updateTransform(body);
100 }
101
102 float CJoint::getBreakForce() const
103 {
104 return mDesc.breakForce;
105 }
106
107 void CJoint::setBreakForce(float force)
108 {
109 if (mDesc.breakForce == force)
110 return;
111
112 mDesc.breakForce = force;
113
114 if (mInternal != nullptr)
115 mInternal->setBreakForce(force);
116 }
117
118 float CJoint::getBreakTorque() const
119 {
120 return mDesc.breakTorque;
121 }
122
123 void CJoint::setBreakTorque(float torque)
124 {
125 if (mDesc.breakTorque == torque)
126 return;
127
128 mDesc.breakTorque = torque;
129
130 if (mInternal != nullptr)
131 mInternal->setBreakTorque(torque);
132 }
133
134 bool CJoint::getEnableCollision() const
135 {
136 return mDesc.enableCollision;
137 }
138
139 void CJoint::setEnableCollision(bool value)
140 {
141 if (mDesc.enableCollision == value)
142 return;
143
144 mDesc.enableCollision = value;
145
146 if (mInternal != nullptr)
147 mInternal->setEnableCollision(value);
148 }
149
150 void CJoint::onInitialized()
151 {
152
153 }
154
155 void CJoint::onDestroyed()
156 {
157 if (mBodies[0] != nullptr)
158 mBodies[0]->_setJoint(HJoint());
159
160 if (mBodies[1] != nullptr)
161 mBodies[1]->_setJoint(HJoint());
162
163 if(mInternal != nullptr)
164 destroyInternal();
165 }
166
167 void CJoint::onDisabled()
168 {
169 if (mInternal != nullptr)
170 destroyInternal();
171 }
172
173 void CJoint::onEnabled()
174 {
175 if(isBodyValid(mBodies[0]) || isBodyValid(mBodies[1]))
176 restoreInternal();
177 }
178
179 void CJoint::onTransformChanged(TransformChangedFlags flags)
180 {
181 if (mInternal == nullptr)
182 return;
183
184 // We're ignoring this during physics update because it would cause problems if the joint itself was moved by physics
185 // Note: This isn't particularily correct because if the joint is being moved by physics but the rigidbodies
186 // themselves are not parented to the joint, the transform will need updating. However I'm leaving it up to the
187 // user to ensure rigidbodies are always parented to the joint in such a case (It's an unlikely situation that
188 // I can't think of an use for - joint transform will almost always be set as an initialization step and not a
189 // physics response).
190 if (gPhysics()._isUpdateInProgress())
191 return;
192
193 updateTransform(JointBody::Target);
194 updateTransform(JointBody::Anchor);
195 }
196
197 void CJoint::restoreInternal()
198 {
199 if (mBodies[0] != nullptr)
200 mDesc.bodies[0].body = mBodies[0]->_getInternal();
201 else
202 mDesc.bodies[0].body = nullptr;
203
204 if (mBodies[1] != nullptr)
205 mDesc.bodies[1].body = mBodies[1]->_getInternal();
206 else
207 mDesc.bodies[1].body = nullptr;
208
209 getLocalTransform(JointBody::Target, mDesc.bodies[0].position, mDesc.bodies[0].rotation);
210 getLocalTransform(JointBody::Anchor, mDesc.bodies[1].position, mDesc.bodies[1].rotation);
211
212 mInternal = createInternal();
213
214 mInternal->onJointBreak.connect(std::bind(&CJoint::triggerOnJointBroken, this));
215 }
216
217 void CJoint::destroyInternal()
218 {
219 // This should release the last reference and destroy the internal joint
220 if(mInternal)
221 {
222 mInternal->_setOwner(PhysicsOwnerType::None, nullptr);
223 mInternal = nullptr;
224 }
225 }
226
227 void CJoint::notifyRigidbodyMoved(const HRigidbody& body)
228 {
229 if (mInternal == nullptr)
230 return;
231
232 // If physics update is in progress do nothing, as its the joint itself that's probably moving the body
233 if (gPhysics()._isUpdateInProgress())
234 return;
235
236 if (mBodies[0] == body)
237 updateTransform(JointBody::Target);
238 else if (mBodies[1] == body)
239 updateTransform(JointBody::Anchor);
240 else
241 assert(false); // Not allowed to happen
242 }
243
244 bool CJoint::isBodyValid(const HRigidbody& body)
245 {
246 if (body == nullptr)
247 return false;
248
249 if (body->_getInternal() == nullptr)
250 return false;
251
252 return true;
253 }
254
255 void CJoint::updateTransform(JointBody body)
256 {
257 Vector3 localPos;
258 Quaternion localRot;
259 getLocalTransform(body, localPos, localRot);
260
261 mInternal->setTransform(body, localPos, localRot);
262 }
263
264 void CJoint::getLocalTransform(JointBody body, Vector3& position, Quaternion& rotation)
265 {
266 position = mPositions[(UINT32)body];
267 rotation = mRotations[(UINT32)body];
268
269 HRigidbody rigidbody = mBodies[(UINT32)body];
270 if (rigidbody == nullptr) // Get world space transform if no relative to any body
271 {
272 const Transform& tfrm = SO()->getTransform();
273 Quaternion worldRot = tfrm.getRotation();
274
275 rotation = worldRot*rotation;
276 position = worldRot.rotate(position) + tfrm.getPosition();
277 }
278 else
279 {
280 position = rotation.rotate(position);
281 }
282 }
283
284 void CJoint::triggerOnJointBroken()
285 {
286 onJointBreak();
287 }
288
289 RTTITypeBase* CJoint::getRTTIStatic()
290 {
291 return CJointRTTI::instance();
292 }
293
294 RTTITypeBase* CJoint::getRTTI() const
295 {
296 return CJoint::getRTTIStatic();
297 }
298}