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 "BsPhysXD6Joint.h"
4#include "BsFPhysXJoint.h"
5#include "BsPhysX.h"
6#include "BsPhysXRigidbody.h"
7#include "PxRigidDynamic.h"
8
9using namespace physx;
10
11namespace bs
12{
13 PxD6Axis::Enum toPxAxis(D6JointAxis axis)
14 {
15 switch(axis)
16 {
17 default:
18 case D6JointAxis::X:
19 return PxD6Axis::eX;
20 case D6JointAxis::Y:
21 return PxD6Axis::eY;
22 case D6JointAxis::Z:
23 return PxD6Axis::eZ;
24 case D6JointAxis::Twist:
25 return PxD6Axis::eTWIST;
26 case D6JointAxis::SwingY:
27 return PxD6Axis::eSWING1;
28 case D6JointAxis::SwingZ:
29 return PxD6Axis::eSWING2;
30 }
31 }
32
33 PxD6Motion::Enum toPxMotion(D6JointMotion motion)
34 {
35 switch(motion)
36 {
37 default:
38 case D6JointMotion::Free:
39 return PxD6Motion::eFREE;
40 case D6JointMotion::Limited:
41 return PxD6Motion::eLIMITED;
42 case D6JointMotion::Locked:
43 return PxD6Motion::eLOCKED;
44 }
45 }
46
47 PxD6Drive::Enum toPxDrive(D6JointDriveType drive)
48 {
49 switch(drive)
50 {
51 default:
52 case D6JointDriveType::X:
53 return PxD6Drive::eX;
54 case D6JointDriveType::Y:
55 return PxD6Drive::eY;
56 case D6JointDriveType::Z:
57 return PxD6Drive::eZ;
58 case D6JointDriveType::Swing:
59 return PxD6Drive::eSWING;
60 case D6JointDriveType::Twist:
61 return PxD6Drive::eTWIST;
62 case D6JointDriveType::SLERP:
63 return PxD6Drive::eSLERP;
64 }
65 }
66
67 D6JointMotion fromPxMotion(PxD6Motion::Enum motion)
68 {
69 switch (motion)
70 {
71 default:
72 case PxD6Motion::eFREE:
73 return D6JointMotion::Free;
74 case PxD6Motion::eLIMITED:
75 return D6JointMotion::Limited;
76 case PxD6Motion::eLOCKED:
77 return D6JointMotion::Locked;
78 }
79 }
80
81
82 D6JointDriveType fromPxDrive(PxD6Drive::Enum drive)
83 {
84 switch (drive)
85 {
86 default:
87 case PxD6Drive::eX:
88 return D6JointDriveType::X;
89 case PxD6Drive::eY:
90 return D6JointDriveType::Y;
91 case PxD6Drive::eZ:
92 return D6JointDriveType::Z;
93 case PxD6Drive::eSWING:
94 return D6JointDriveType::Swing;
95 case PxD6Drive::eTWIST:
96 return D6JointDriveType::Twist;
97 case PxD6Drive::eSLERP:
98 return D6JointDriveType::SLERP;
99 }
100 }
101
102 PhysXD6Joint::PhysXD6Joint(PxPhysics* physx, const D6_JOINT_DESC& desc)
103 :D6Joint(desc)
104 {
105 PxRigidActor* actor0 = nullptr;
106 if (desc.bodies[0].body != nullptr)
107 actor0 = static_cast<PhysXRigidbody*>(desc.bodies[0].body)->_getInternal();
108
109 PxRigidActor* actor1 = nullptr;
110 if (desc.bodies[1].body != nullptr)
111 actor1 = static_cast<PhysXRigidbody*>(desc.bodies[1].body)->_getInternal();
112
113 PxTransform tfrm0 = toPxTransform(desc.bodies[0].position, desc.bodies[0].rotation);
114 PxTransform tfrm1 = toPxTransform(desc.bodies[1].position, desc.bodies[1].rotation);
115
116 PxD6Joint* joint = PxD6JointCreate(*physx, actor0, tfrm0, actor1, tfrm1);
117 joint->userData = this;
118
119 mInternal = bs_new<FPhysXJoint>(joint, desc);
120
121 // Calls to virtual methods are okay here
122 for (UINT32 i = 0; i < (UINT32)D6JointAxis::Count; i++)
123 setMotion((D6JointAxis)i, desc.motion[i]);
124
125 for (UINT32 i = 0; i < (UINT32)D6JointDriveType::Count; i++)
126 setDrive((D6JointDriveType)i, desc.drive[i]);
127
128 setLimitLinear(desc.limitLinear);
129 setLimitTwist(desc.limitTwist);
130 setLimitSwing(desc.limitSwing);
131
132 setDriveTransform(desc.drivePosition, desc.driveRotation);
133 setDriveVelocity(desc.driveLinearVelocity, desc.driveAngularVelocity);
134 }
135
136 PhysXD6Joint::~PhysXD6Joint()
137 {
138 bs_delete(mInternal);
139 }
140
141 D6JointMotion PhysXD6Joint::getMotion(D6JointAxis axis) const
142 {
143 return fromPxMotion(getInternal()->getMotion(toPxAxis(axis)));
144 }
145
146 void PhysXD6Joint::setMotion(D6JointAxis axis, D6JointMotion motion)
147 {
148 getInternal()->setMotion(toPxAxis(axis), toPxMotion(motion));
149 }
150
151 Radian PhysXD6Joint::getTwist() const
152 {
153 return Radian(getInternal()->getTwist());
154 }
155
156 Radian PhysXD6Joint::getSwingY() const
157 {
158 return Radian(getInternal()->getSwingYAngle());
159 }
160
161 Radian PhysXD6Joint::getSwingZ() const
162 {
163 return Radian(getInternal()->getSwingZAngle());
164 }
165
166 LimitLinear PhysXD6Joint::getLimitLinear() const
167 {
168 PxJointLinearLimit pxLimit = getInternal()->getLinearLimit();
169
170 LimitLinear limit;
171 limit.extent = pxLimit.value;
172 limit.contactDist = pxLimit.contactDistance;
173 limit.restitution = pxLimit.restitution;
174 limit.spring.stiffness = pxLimit.stiffness;
175 limit.spring.damping = pxLimit.damping;
176
177 return limit;
178 }
179
180 void PhysXD6Joint::setLimitLinear(const LimitLinear& limit)
181 {
182 PxJointLinearLimit pxLimit(gPhysX().getScale(), limit.extent, limit.contactDist);
183 pxLimit.stiffness = limit.spring.stiffness;
184 pxLimit.damping = limit.spring.damping;
185 pxLimit.restitution = limit.restitution;
186
187 getInternal()->setLinearLimit(pxLimit);
188 }
189
190 LimitAngularRange PhysXD6Joint::getLimitTwist() const
191 {
192 PxJointAngularLimitPair pxLimit = getInternal()->getTwistLimit();
193
194 LimitAngularRange limit;
195 limit.lower = pxLimit.lower;
196 limit.upper = pxLimit.upper;
197 limit.contactDist = pxLimit.contactDistance;
198 limit.restitution = pxLimit.restitution;
199 limit.spring.stiffness = pxLimit.stiffness;
200 limit.spring.damping = pxLimit.damping;
201
202 return limit;
203 }
204
205 void PhysXD6Joint::setLimitTwist(const LimitAngularRange& limit)
206 {
207 PxJointAngularLimitPair pxLimit(limit.lower.valueRadians(), limit.upper.valueRadians(), limit.contactDist);
208 pxLimit.stiffness = limit.spring.stiffness;
209 pxLimit.damping = limit.spring.damping;
210 pxLimit.restitution = limit.restitution;
211
212 getInternal()->setTwistLimit(pxLimit);
213 }
214
215 LimitConeRange PhysXD6Joint::getLimitSwing() const
216 {
217 PxJointLimitCone pxLimit = getInternal()->getSwingLimit();
218
219 LimitConeRange limit;
220 limit.yLimitAngle = pxLimit.yAngle;
221 limit.zLimitAngle = pxLimit.zAngle;
222 limit.contactDist = pxLimit.contactDistance;
223 limit.restitution = pxLimit.restitution;
224 limit.spring.stiffness = pxLimit.stiffness;
225 limit.spring.damping = pxLimit.damping;
226
227 return limit;
228 }
229
230 void PhysXD6Joint::setLimitSwing(const LimitConeRange& limit)
231 {
232 PxJointLimitCone pxLimit(limit.yLimitAngle.valueRadians(), limit.zLimitAngle.valueRadians(), limit.contactDist);
233 pxLimit.stiffness = limit.spring.stiffness;
234 pxLimit.damping = limit.spring.damping;
235 pxLimit.restitution = limit.restitution;
236
237 getInternal()->setSwingLimit(pxLimit);
238 }
239
240 D6JointDrive PhysXD6Joint::getDrive(D6JointDriveType type) const
241 {
242 PxD6JointDrive pxDrive = getInternal()->getDrive(toPxDrive(type));
243
244 D6JointDrive drive;
245 drive.acceleration = pxDrive.flags & PxD6JointDriveFlag::eACCELERATION;
246 drive.stiffness = pxDrive.stiffness;
247 drive.damping = pxDrive.damping;
248 drive.forceLimit = pxDrive.forceLimit;
249
250 return drive;
251 }
252
253 void PhysXD6Joint::setDrive(D6JointDriveType type, const D6JointDrive& drive)
254 {
255 PxD6JointDrive pxDrive;
256
257 if(drive.acceleration)
258 pxDrive.flags = PxD6JointDriveFlag::eACCELERATION;
259
260 pxDrive.stiffness = drive.stiffness;
261 pxDrive.damping = drive.damping;
262 pxDrive.forceLimit = drive.forceLimit;
263
264 getInternal()->setDrive(toPxDrive(type), pxDrive);
265 }
266
267 Vector3 PhysXD6Joint::getDrivePosition() const
268 {
269 return fromPxVector(getInternal()->getDrivePosition().p);
270 }
271
272 Quaternion PhysXD6Joint::getDriveRotation() const
273 {
274 return fromPxQuaternion(getInternal()->getDrivePosition().q);
275 }
276
277 void PhysXD6Joint::setDriveTransform(const Vector3& position, const Quaternion& rotation)
278 {
279 getInternal()->setDrivePosition(toPxTransform(position, rotation));
280 }
281
282 Vector3 PhysXD6Joint::getDriveLinearVelocity() const
283 {
284 PxVec3 linear;
285 PxVec3 angular;
286
287 getInternal()->getDriveVelocity(linear, angular);
288 return fromPxVector(linear);
289 }
290
291 Vector3 PhysXD6Joint::getDriveAngularVelocity() const
292 {
293 PxVec3 linear;
294 PxVec3 angular;
295
296 getInternal()->getDriveVelocity(linear, angular);
297 return fromPxVector(angular);
298 }
299
300 void PhysXD6Joint::setDriveVelocity(const Vector3& linear, const Vector3& angular)
301 {
302 getInternal()->setDriveVelocity(toPxVector(linear), toPxVector(angular));
303 }
304
305 PxD6Joint* PhysXD6Joint::getInternal() const
306 {
307 FPhysXJoint* internal = static_cast<FPhysXJoint*>(mInternal);
308
309 return static_cast<PxD6Joint*>(internal->_getInternal());
310 }
311}