1 /********************************************************************************
2 * ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
3 * Copyright (c) 2010-2020 Daniel Chappuis                                       *
4 *********************************************************************************
5 *                                                                               *
6 * This software is provided 'as-is', without any express or implied warranty.   *
7 * In no event will the authors be held liable for any damages arising from the  *
8 * use of this software.                                                         *
9 *                                                                               *
10 * Permission is granted to anyone to use this software for any purpose,         *
11 * including commercial applications, and to alter it and redistribute it        *
12 * freely, subject to the following restrictions:                                *
13 *                                                                               *
14 * 1. The origin of this software must not be misrepresented; you must not claim *
15 *    that you wrote the original software. If you use this software in a        *
16 *    product, an acknowledgment in the product documentation would be           *
17 *    appreciated but is not required.                                           *
18 *                                                                               *
19 * 2. Altered source versions must be plainly marked as such, and must not be    *
20 *    misrepresented as being the original software.                             *
21 *                                                                               *
22 * 3. This notice may not be removed or altered from any source distribution.    *
23 *                                                                               *
24 ********************************************************************************/
25 
26 #ifndef REACTPHYSICS3D_HINGE_JOINT_COMPONENTS_H
27 #define REACTPHYSICS3D_HINGE_JOINT_COMPONENTS_H
28 
29 // Libraries
30 #include <reactphysics3d/mathematics/Transform.h>
31 #include <reactphysics3d/mathematics/Matrix3x3.h>
32 #include <reactphysics3d/mathematics/Matrix2x2.h>
33 #include <reactphysics3d/engine/Entity.h>
34 #include <reactphysics3d/components/Components.h>
35 #include <reactphysics3d/containers/Map.h>
36 
37 // ReactPhysics3D namespace
38 namespace reactphysics3d {
39 
40 // Class declarations
41 class MemoryAllocator;
42 class EntityManager;
43 class HingeJoint;
44 enum class JointType;
45 
46 // Class HingeJointComponents
47 /**
48  * This class represent the component of the ECS with data for the HingeJoint.
49  */
50 class HingeJointComponents : public Components {
51 
52     private:
53 
54         // -------------------- Attributes -------------------- //
55 
56         /// Array of joint entities
57         Entity* mJointEntities;
58 
59         /// Array of pointers to the joints
60         HingeJoint** mJoints;
61 
62         /// Anchor point of body 1 (in local-space coordinates of body 1)
63         Vector3* mLocalAnchorPointBody1;
64 
65         /// Anchor point of body 2 (in local-space coordinates of body 2)
66         Vector3* mLocalAnchorPointBody2;
67 
68         /// Vector from center of body 2 to anchor point in world-space
69         Vector3* mR1World;
70 
71         /// Vector from center of body 2 to anchor point in world-space
72         Vector3* mR2World;
73 
74         /// Inertia tensor of body 1 (in world-space coordinates)
75         Matrix3x3* mI1;
76 
77         /// Inertia tensor of body 2 (in world-space coordinates)
78         Matrix3x3* mI2;
79 
80         /// Accumulated impulse for the 3 translation constraints
81         Vector3* mImpulseTranslation;
82 
83         /// Accumulate impulse for the 3 rotation constraints
84         Vector2* mImpulseRotation;
85 
86         /// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix)
87         Matrix3x3* mInverseMassMatrixTranslation;
88 
89         /// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix)
90         Matrix2x2* mInverseMassMatrixRotation;
91 
92         /// Bias vector for the 3 translation constraints
93         Vector3* mBiasTranslation;
94 
95         /// Bias vector for the 3 rotation constraints
96         Vector2* mBiasRotation;
97 
98         /// Inverse of the initial orientation difference between the two bodies
99         Quaternion* mInitOrientationDifferenceInv;
100 
101         /// Hinge rotation axis (in local-space coordinates of body 1)
102         Vector3* mHingeLocalAxisBody1;
103 
104         /// Hinge rotation axis (in local-space coordiantes of body 2)
105         Vector3* mHingeLocalAxisBody2;
106 
107         /// Hinge rotation axis (in world-space coordinates) computed from body 1
108         Vector3* mA1;
109 
110         /// Cross product of vector b2 and a1
111         Vector3* mB2CrossA1;
112 
113         /// Cross product of vector c2 and a1;
114         Vector3* mC2CrossA1;
115 
116         /// Accumulated impulse for the lower limit constraint
117         decimal* mImpulseLowerLimit;
118 
119         /// Accumulated impulse for the upper limit constraint
120         decimal* mImpulseUpperLimit;
121 
122         /// Accumulated impulse for the motor constraint;
123         decimal* mImpulseMotor;
124 
125         /// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix)
126         decimal* mInverseMassMatrixLimitMotor;
127 
128         /// Inverse of mass matrix K=JM^-1J^t for the motor
129         decimal* mInverseMassMatrixMotor;
130 
131         /// Bias of the lower limit constraint
132         decimal* mBLowerLimit;
133 
134         /// Bias of the upper limit constraint
135         decimal* mBUpperLimit;
136 
137         /// True if the joint limits are enabled
138         bool* mIsLimitEnabled;
139 
140         /// True if the motor of the joint in enabled
141         bool* mIsMotorEnabled;
142 
143         /// Lower limit (minimum allowed rotation angle in radian)
144         decimal* mLowerLimit;
145 
146         /// Upper limit (maximum translation distance)
147         decimal* mUpperLimit;
148 
149         /// True if the lower limit is violated
150         bool* mIsLowerLimitViolated;
151 
152         /// True if the upper limit is violated
153         bool* mIsUpperLimitViolated;
154 
155         /// Motor speed (in rad/s)
156         decimal* mMotorSpeed;
157 
158         /// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
159         decimal* mMaxMotorTorque;
160 
161         // -------------------- Methods -------------------- //
162 
163         /// Allocate memory for a given number of components
164         virtual void allocate(uint32 nbComponentsToAllocate) override;
165 
166         /// Destroy a component at a given index
167         virtual void destroyComponent(uint32 index) override;
168 
169         /// Move a component from a source to a destination index in the components array
170         virtual void moveComponentToIndex(uint32 srcIndex, uint32 destIndex) override;
171 
172         /// Swap two components in the array
173         virtual void swapComponents(uint32 index1, uint32 index2) override;
174 
175     public:
176 
177         /// Structure for the data of a transform component
178         struct HingeJointComponent {
179 
180              bool isLimitEnabled;
181              bool isMotorEnabled;
182              decimal lowerLimit;
183              decimal upperLimit;
184              decimal motorSpeed;
185              decimal maxMotorTorque;
186 
187             /// Constructor
HingeJointComponentHingeJointComponent188             HingeJointComponent(bool isLimitEnabled, bool isMotorEnabled, decimal lowerLimit, decimal upperLimit,
189                                 decimal motorSpeed, decimal maxMotorTorque)
190                 : isLimitEnabled(isLimitEnabled), isMotorEnabled(isMotorEnabled), lowerLimit(lowerLimit), upperLimit(upperLimit),
191                   motorSpeed(motorSpeed), maxMotorTorque(maxMotorTorque) {
192 
193             }
194         };
195 
196         // -------------------- Methods -------------------- //
197 
198         /// Constructor
199         HingeJointComponents(MemoryAllocator& allocator);
200 
201         /// Destructor
202         virtual ~HingeJointComponents() override = default;
203 
204         /// Add a component
205         void addComponent(Entity jointEntity, bool isSleeping, const HingeJointComponent& component);
206 
207         /// Return a pointer to a given joint
208         HingeJoint* getJoint(Entity jointEntity) const;
209 
210         /// Set the joint pointer to a given joint
211         void setJoint(Entity jointEntity, HingeJoint* joint) const;
212 
213         /// Return the local anchor point of body 1 for a given joint
214         const Vector3& getLocalAnchorPointBody1(Entity jointEntity) const;
215 
216         /// Set the local anchor point of body 1 for a given joint
217         void setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1);
218 
219         /// Return the local anchor point of body 2 for a given joint
220         const Vector3& getLocalAnchorPointBody2(Entity jointEntity) const;
221 
222         /// Set the local anchor point of body 2 for a given joint
223         void setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2);
224 
225         /// Return the vector from center of body 1 to anchor point in world-space
226         const Vector3& getR1World(Entity jointEntity) const;
227 
228         /// Set the vector from center of body 1 to anchor point in world-space
229         void setR1World(Entity jointEntity, const Vector3& r1World);
230 
231         /// Return the vector from center of body 2 to anchor point in world-space
232         const Vector3& getR2World(Entity jointEntity) const;
233 
234         /// Set the vector from center of body 2 to anchor point in world-space
235         void setR2World(Entity jointEntity, const Vector3& r2World);
236 
237         /// Return the inertia tensor of body 1 (in world-space coordinates)
238         const Matrix3x3& getI1(Entity jointEntity) const;
239 
240         /// Set the inertia tensor of body 1 (in world-space coordinates)
241         void setI1(Entity jointEntity, const Matrix3x3& i1);
242 
243         /// Return the inertia tensor of body 2 (in world-space coordinates)
244         const Matrix3x3& getI2(Entity jointEntity) const;
245 
246         /// Set the inertia tensor of body 2 (in world-space coordinates)
247         void setI2(Entity jointEntity, const Matrix3x3& i2);
248 
249         /// Return the translation impulse
250         Vector3& getImpulseTranslation(Entity jointEntity);
251 
252         /// Set the translation impulse
253         void setImpulseTranslation(Entity jointEntity, const Vector3& impulseTranslation);
254 
255         /// Return the translation impulse
256         Vector2& getImpulseRotation(Entity jointEntity);
257 
258         /// Set the translation impulse
259         void setImpulseRotation(Entity jointEntity, const Vector2& impulseTranslation);
260 
261         /// Return the translation inverse mass matrix of the constraint
262         Matrix3x3& getInverseMassMatrixTranslation(Entity jointEntity);
263 
264         /// Set the translation inverse mass matrix of the constraint
265         void setInverseMassMatrixTranslation(Entity jointEntity, const Matrix3x3& inverseMassMatrix);
266 
267         /// Return the rotation inverse mass matrix of the constraint
268         Matrix2x2& getInverseMassMatrixRotation(Entity jointEntity);
269 
270         /// Set the rotation inverse mass matrix of the constraint
271         void setInverseMassMatrixRotation(Entity jointEntity, const Matrix2x2& inverseMassMatrix);
272 
273         /// Return the translation bias
274         Vector3& getBiasTranslation(Entity jointEntity);
275 
276         /// Set the translation impulse
277         void setBiasTranslation(Entity jointEntity, const Vector3& impulseTranslation);
278 
279         /// Return the rotation bias
280         Vector2& getBiasRotation(Entity jointEntity);
281 
282         /// Set the rotation impulse
283         void setBiasRotation(Entity jointEntity, const Vector2& impulseRotation);
284 
285         /// Return the initial orientation difference
286         Quaternion& getInitOrientationDifferenceInv(Entity jointEntity);
287 
288         /// Set the rotation impulse
289         void setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv);
290 
291         /// Return the hinge rotation axis (in local-space coordinates of body 1)
292         Vector3& getHingeLocalAxisBody1(Entity jointEntity);
293 
294         /// Set the hinge rotation axis (in local-space coordinates of body 1)
295         void setHingeLocalAxisBody1(Entity jointEntity, const Vector3& hingeLocalAxisBody1);
296 
297         /// Return the hinge rotation axis (in local-space coordiantes of body 2)
298         Vector3& getHingeLocalAxisBody2(Entity jointEntity);
299 
300         /// Set the hinge rotation axis (in local-space coordiantes of body 2)
301         void setHingeLocalAxisBody2(Entity jointEntity, const Vector3& hingeLocalAxisBody2);
302 
303         /// Return the hinge rotation axis (in world-space coordinates) computed from body 1
304         Vector3& getA1(Entity jointEntity);
305 
306         /// Set the hinge rotation axis (in world-space coordinates) computed from body 1
307         void setA1(Entity jointEntity, const Vector3& a1);
308 
309         /// Return the cross product of vector b2 and a1
310         Vector3& getB2CrossA1(Entity jointEntity);
311 
312         /// Set the cross product of vector b2 and a1
313         void setB2CrossA1(Entity jointEntity, const Vector3& b2CrossA1);
314 
315         /// Return the cross product of vector c2 and a1;
316         Vector3& getC2CrossA1(Entity jointEntity);
317 
318         /// Set the cross product of vector c2 and a1;
319         void setC2CrossA1(Entity jointEntity, const Vector3& c2CrossA1);
320 
321         /// Return the accumulated impulse for the lower limit constraint
322         decimal getImpulseLowerLimit(Entity jointEntity) const;
323 
324         /// Set the accumulated impulse for the lower limit constraint
325         void setImpulseLowerLimit(Entity jointEntity, decimal impulseLowerLimit);
326 
327         /// Return the accumulated impulse for the upper limit constraint
328         decimal getImpulseUpperLimit(Entity jointEntity) const;
329 
330         /// Set the accumulated impulse for the upper limit constraint
331         void setImpulseUpperLimit(Entity jointEntity, decimal impulseUpperLimit) const;
332 
333         /// Return the accumulated impulse for the motor constraint;
334         decimal getImpulseMotor(Entity jointEntity) const;
335 
336         /// Set the accumulated impulse for the motor constraint;
337         void setImpulseMotor(Entity jointEntity, decimal impulseMotor);
338 
339         /// Return the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix)
340         decimal getInverseMassMatrixLimitMotor(Entity jointEntity) const;
341 
342         /// Set the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix)
343         void setInverseMassMatrixLimitMotor(Entity jointEntity, decimal inverseMassMatrixLimitMotor);
344 
345         /// Return the inverse of mass matrix K=JM^-1J^t for the motor
346         decimal getInverseMassMatrixMotor(Entity jointEntity);
347 
348         /// Set the inverse of mass matrix K=JM^-1J^t for the motor
349         void setInverseMassMatrixMotor(Entity jointEntity, decimal inverseMassMatrixMotor);
350 
351         /// Return the bias of the lower limit constraint
352         decimal getBLowerLimit(Entity jointEntity) const;
353 
354         /// Set the bias of the lower limit constraint
355         void setBLowerLimit(Entity jointEntity, decimal bLowerLimit) const;
356 
357         /// Return the bias of the upper limit constraint
358         decimal getBUpperLimit(Entity jointEntity) const;
359 
360         /// Set the bias of the upper limit constraint
361         void setBUpperLimit(Entity jointEntity, decimal bUpperLimit);
362 
363         /// Return true if the joint limits are enabled
364         bool getIsLimitEnabled(Entity jointEntity) const;
365 
366         /// Set to true if the joint limits are enabled
367         void setIsLimitEnabled(Entity jointEntity, bool isLimitEnabled);
368 
369         /// Return true if the motor of the joint in enabled
370         bool getIsMotorEnabled(Entity jointEntity) const;
371 
372         /// Set to true if the motor of the joint in enabled
373         void setIsMotorEnabled(Entity jointEntity, bool isMotorEnabled) const;
374 
375         /// Return the Lower limit (minimum allowed rotation angle in radian)
376         decimal getLowerLimit(Entity jointEntity) const;
377 
378         /// Set the Lower limit (minimum allowed rotation angle in radian)
379         void setLowerLimit(Entity jointEntity, decimal lowerLimit) const;
380 
381         /// Return the upper limit (maximum translation distance)
382         decimal getUpperLimit(Entity jointEntity) const;
383 
384         /// Set the upper limit (maximum translation distance)
385         void setUpperLimit(Entity jointEntity, decimal upperLimit);
386 
387         /// Return true if the lower limit is violated
388         bool getIsLowerLimitViolated(Entity jointEntity) const;
389 
390         /// Set to true if the lower limit is violated
391         void setIsLowerLimitViolated(Entity jointEntity, bool isLowerLimitViolated);
392 
393         /// Return true if the upper limit is violated
394         bool getIsUpperLimitViolated(Entity jointEntity) const;
395 
396         /// Set to true if the upper limit is violated
397         void setIsUpperLimitViolated(Entity jointEntity, bool isUpperLimitViolated) const;
398 
399         /// Return the motor speed (in rad/s)
400         decimal getMotorSpeed(Entity jointEntity) const;
401 
402         /// Set the motor speed (in rad/s)
403         void setMotorSpeed(Entity jointEntity, decimal motorSpeed);
404 
405         /// Return the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
406         decimal getMaxMotorTorque(Entity jointEntity) const;
407 
408         /// Set the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
409         void setMaxMotorTorque(Entity jointEntity, decimal maxMotorTorque);
410 
411         // -------------------- Friendship -------------------- //
412 
413         friend class BroadPhaseSystem;
414         friend class SolveHingeJointSystem;
415 };
416 
417 // Return a pointer to a given joint
getJoint(Entity jointEntity)418 inline HingeJoint* HingeJointComponents::getJoint(Entity jointEntity) const {
419 
420     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
421     return mJoints[mMapEntityToComponentIndex[jointEntity]];
422 }
423 
424 // Set the joint pointer to a given joint
setJoint(Entity jointEntity,HingeJoint * joint)425 inline void HingeJointComponents::setJoint(Entity jointEntity, HingeJoint* joint) const {
426 
427     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
428     mJoints[mMapEntityToComponentIndex[jointEntity]] = joint;
429 }
430 
431 // Return the local anchor point of body 1 for a given joint
getLocalAnchorPointBody1(Entity jointEntity)432 inline const Vector3& HingeJointComponents::getLocalAnchorPointBody1(Entity jointEntity) const {
433 
434     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
435     return mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]];
436 }
437 
438 // Set the local anchor point of body 1 for a given joint
setLocalAnchorPointBody1(Entity jointEntity,const Vector3 & localAnchoirPointBody1)439 inline void HingeJointComponents::setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1) {
440 
441     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
442     mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody1;
443 }
444 
445 // Return the local anchor point of body 2 for a given joint
getLocalAnchorPointBody2(Entity jointEntity)446 inline const Vector3& HingeJointComponents::getLocalAnchorPointBody2(Entity jointEntity) const {
447 
448     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
449     return mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]];
450 }
451 
452 // Set the local anchor point of body 2 for a given joint
setLocalAnchorPointBody2(Entity jointEntity,const Vector3 & localAnchoirPointBody2)453 inline void HingeJointComponents::setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2) {
454 
455     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
456     mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody2;
457 }
458 
459 // Return the vector from center of body 1 to anchor point in world-space
getR1World(Entity jointEntity)460 inline const Vector3& HingeJointComponents::getR1World(Entity jointEntity) const {
461 
462     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
463     return mR1World[mMapEntityToComponentIndex[jointEntity]];
464 }
465 
466 // Set the vector from center of body 1 to anchor point in world-space
setR1World(Entity jointEntity,const Vector3 & r1World)467 inline void HingeJointComponents::setR1World(Entity jointEntity, const Vector3& r1World) {
468 
469     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
470     mR1World[mMapEntityToComponentIndex[jointEntity]] = r1World;
471 }
472 
473 // Return the vector from center of body 2 to anchor point in world-space
getR2World(Entity jointEntity)474 inline const Vector3& HingeJointComponents::getR2World(Entity jointEntity) const {
475 
476     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
477     return mR2World[mMapEntityToComponentIndex[jointEntity]];
478 }
479 
480 // Set the vector from center of body 2 to anchor point in world-space
setR2World(Entity jointEntity,const Vector3 & r2World)481 inline void HingeJointComponents::setR2World(Entity jointEntity, const Vector3& r2World) {
482 
483     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
484     mR2World[mMapEntityToComponentIndex[jointEntity]] = r2World;
485 }
486 
487 // Return the inertia tensor of body 1 (in world-space coordinates)
getI1(Entity jointEntity)488 inline const Matrix3x3& HingeJointComponents::getI1(Entity jointEntity) const {
489 
490     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
491     return mI1[mMapEntityToComponentIndex[jointEntity]];
492 }
493 
494 // Set the inertia tensor of body 1 (in world-space coordinates)
setI1(Entity jointEntity,const Matrix3x3 & i1)495 inline void HingeJointComponents::setI1(Entity jointEntity, const Matrix3x3& i1) {
496 
497     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
498     mI1[mMapEntityToComponentIndex[jointEntity]] = i1;
499 }
500 
501 // Return the inertia tensor of body 2 (in world-space coordinates)
getI2(Entity jointEntity)502 inline const Matrix3x3& HingeJointComponents::getI2(Entity jointEntity) const {
503 
504     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
505     return mI2[mMapEntityToComponentIndex[jointEntity]];
506 }
507 
508 // Set the inertia tensor of body 2 (in world-space coordinates)
setI2(Entity jointEntity,const Matrix3x3 & i2)509 inline void HingeJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) {
510 
511     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
512     mI2[mMapEntityToComponentIndex[jointEntity]] = i2;
513 }
514 
515 // Return the translation impulse
getImpulseTranslation(Entity jointEntity)516 inline Vector3& HingeJointComponents::getImpulseTranslation(Entity jointEntity) {
517 
518     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
519     return mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]];
520 }
521 
522 // Set the translation impulse
setImpulseTranslation(Entity jointEntity,const Vector3 & impulseTranslation)523 inline void HingeJointComponents::setImpulseTranslation(Entity jointEntity, const Vector3& impulseTranslation) {
524 
525     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
526     mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation;
527 }
528 
529 // Return the translation impulse
getImpulseRotation(Entity jointEntity)530 inline Vector2& HingeJointComponents::getImpulseRotation(Entity jointEntity) {
531 
532     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
533     return mImpulseRotation[mMapEntityToComponentIndex[jointEntity]];
534 }
535 
536 // Set the translation impulse
setImpulseRotation(Entity jointEntity,const Vector2 & impulseTranslation)537 inline void HingeJointComponents::setImpulseRotation(Entity jointEntity, const Vector2& impulseTranslation) {
538 
539     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
540     mImpulseRotation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation;
541 }
542 
543 // Return the translation inverse mass matrix of the constraint
getInverseMassMatrixTranslation(Entity jointEntity)544 inline Matrix3x3& HingeJointComponents::getInverseMassMatrixTranslation(Entity jointEntity) {
545 
546     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
547     return mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]];
548 }
549 
550 
551 // Set the translation inverse mass matrix of the constraint
setInverseMassMatrixTranslation(Entity jointEntity,const Matrix3x3 & inverseMassMatrix)552 inline void HingeJointComponents::setInverseMassMatrixTranslation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) {
553 
554     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
555     mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix;
556 }
557 
558 // Return the rotation inverse mass matrix of the constraint
getInverseMassMatrixRotation(Entity jointEntity)559 inline Matrix2x2& HingeJointComponents::getInverseMassMatrixRotation(Entity jointEntity) {
560 
561     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
562     return mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]];
563 }
564 
565 // Set the rotation inverse mass matrix of the constraint
setInverseMassMatrixRotation(Entity jointEntity,const Matrix2x2 & inverseMassMatrix)566 inline void HingeJointComponents::setInverseMassMatrixRotation(Entity jointEntity, const Matrix2x2& inverseMassMatrix) {
567 
568     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
569     mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix;
570 }
571 
572 // Return the translation bias
getBiasTranslation(Entity jointEntity)573 inline Vector3& HingeJointComponents::getBiasTranslation(Entity jointEntity) {
574 
575     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
576     return mBiasTranslation[mMapEntityToComponentIndex[jointEntity]];
577 }
578 
579 // Set the translation impulse
setBiasTranslation(Entity jointEntity,const Vector3 & impulseTranslation)580 inline void HingeJointComponents::setBiasTranslation(Entity jointEntity, const Vector3 &impulseTranslation) {
581 
582     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
583     mBiasTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation;
584 }
585 
586 // Return the rotation bias
getBiasRotation(Entity jointEntity)587 inline Vector2 &HingeJointComponents::getBiasRotation(Entity jointEntity) {
588 
589     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
590     return mBiasRotation[mMapEntityToComponentIndex[jointEntity]];
591 }
592 
593 // Set the rotation impulse
setBiasRotation(Entity jointEntity,const Vector2 & impulseRotation)594 inline void HingeJointComponents::setBiasRotation(Entity jointEntity, const Vector2& impulseRotation) {
595 
596     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
597     mBiasRotation[mMapEntityToComponentIndex[jointEntity]] = impulseRotation;
598 }
599 
600 // Return the initial orientation difference
getInitOrientationDifferenceInv(Entity jointEntity)601 inline Quaternion& HingeJointComponents::getInitOrientationDifferenceInv(Entity jointEntity) {
602 
603     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
604     return mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]];
605 }
606 
607 // Set the rotation impulse
setInitOrientationDifferenceInv(Entity jointEntity,const Quaternion & initOrientationDifferenceInv)608 inline void HingeJointComponents::setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv) {
609 
610     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
611     mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]] = initOrientationDifferenceInv;
612 }
613 
614 // Return the hinge rotation axis (in local-space coordinates of body 1)
getHingeLocalAxisBody1(Entity jointEntity)615 inline Vector3& HingeJointComponents::getHingeLocalAxisBody1(Entity jointEntity) {
616 
617     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
618     return mHingeLocalAxisBody1[mMapEntityToComponentIndex[jointEntity]];
619 }
620 
621 // Set the hinge rotation axis (in local-space coordinates of body 1)
setHingeLocalAxisBody1(Entity jointEntity,const Vector3 & hingeLocalAxisBody1)622 inline void HingeJointComponents::setHingeLocalAxisBody1(Entity jointEntity, const Vector3& hingeLocalAxisBody1) {
623 
624     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
625     mHingeLocalAxisBody1[mMapEntityToComponentIndex[jointEntity]] = hingeLocalAxisBody1;
626 }
627 
628 // Return the hinge rotation axis (in local-space coordiantes of body 2)
getHingeLocalAxisBody2(Entity jointEntity)629 inline Vector3& HingeJointComponents::getHingeLocalAxisBody2(Entity jointEntity) {
630 
631     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
632     return mHingeLocalAxisBody2[mMapEntityToComponentIndex[jointEntity]];
633 }
634 
635 // Set the hinge rotation axis (in local-space coordiantes of body 2)
setHingeLocalAxisBody2(Entity jointEntity,const Vector3 & hingeLocalAxisBody2)636 inline void HingeJointComponents::setHingeLocalAxisBody2(Entity jointEntity, const Vector3& hingeLocalAxisBody2) {
637 
638     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
639     mHingeLocalAxisBody2[mMapEntityToComponentIndex[jointEntity]] = hingeLocalAxisBody2;
640 }
641 
642 
643 // Return the hinge rotation axis (in world-space coordinates) computed from body 1
getA1(Entity jointEntity)644 inline Vector3& HingeJointComponents::getA1(Entity jointEntity) {
645 
646     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
647     return mA1[mMapEntityToComponentIndex[jointEntity]];
648 }
649 
650 // Set the hinge rotation axis (in world-space coordinates) computed from body 1
setA1(Entity jointEntity,const Vector3 & a1)651 inline void HingeJointComponents::setA1(Entity jointEntity, const Vector3& a1) {
652 
653     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
654     mA1[mMapEntityToComponentIndex[jointEntity]] = a1;
655 }
656 
657 // Return the cross product of vector b2 and a1
getB2CrossA1(Entity jointEntity)658 inline Vector3& HingeJointComponents::getB2CrossA1(Entity jointEntity) {
659 
660     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
661     return mB2CrossA1[mMapEntityToComponentIndex[jointEntity]];
662 }
663 
664 // Set the cross product of vector b2 and a1
setB2CrossA1(Entity jointEntity,const Vector3 & b2CrossA1)665 inline void HingeJointComponents::setB2CrossA1(Entity jointEntity, const Vector3& b2CrossA1) {
666 
667     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
668     mB2CrossA1[mMapEntityToComponentIndex[jointEntity]] = b2CrossA1;
669 }
670 
671 // Return the cross product of vector c2 and a1;
getC2CrossA1(Entity jointEntity)672 inline Vector3& HingeJointComponents::getC2CrossA1(Entity jointEntity) {
673 
674     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
675     return mC2CrossA1[mMapEntityToComponentIndex[jointEntity]];
676 }
677 
678 // Set the cross product of vector c2 and a1;
setC2CrossA1(Entity jointEntity,const Vector3 & c2CrossA1)679 inline void HingeJointComponents::setC2CrossA1(Entity jointEntity, const Vector3& c2CrossA1) {
680 
681     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
682     mC2CrossA1[mMapEntityToComponentIndex[jointEntity]] = c2CrossA1;
683 }
684 
685 // Return the accumulated impulse for the lower limit constraint
getImpulseLowerLimit(Entity jointEntity)686 inline decimal HingeJointComponents::getImpulseLowerLimit(Entity jointEntity) const {
687 
688     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
689     return mImpulseLowerLimit[mMapEntityToComponentIndex[jointEntity]];
690 }
691 
692 // Set the accumulated impulse for the lower limit constraint
setImpulseLowerLimit(Entity jointEntity,decimal impulseLowerLimit)693 inline void HingeJointComponents::setImpulseLowerLimit(Entity jointEntity, decimal impulseLowerLimit) {
694 
695     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
696     mImpulseLowerLimit[mMapEntityToComponentIndex[jointEntity]] = impulseLowerLimit;
697 }
698 
699 
700 // Return the accumulated impulse for the upper limit constraint
getImpulseUpperLimit(Entity jointEntity)701 inline decimal HingeJointComponents::getImpulseUpperLimit(Entity jointEntity) const {
702 
703     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
704     return mImpulseUpperLimit[mMapEntityToComponentIndex[jointEntity]];
705 }
706 
707 // Set the accumulated impulse for the upper limit constraint
setImpulseUpperLimit(Entity jointEntity,decimal impulseUpperLimit)708 inline void HingeJointComponents::setImpulseUpperLimit(Entity jointEntity, decimal impulseUpperLimit) const {
709 
710     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
711     mImpulseUpperLimit[mMapEntityToComponentIndex[jointEntity]] = impulseUpperLimit;
712 }
713 
714 
715 // Return the accumulated impulse for the motor constraint;
getImpulseMotor(Entity jointEntity)716 inline decimal HingeJointComponents::getImpulseMotor(Entity jointEntity) const {
717 
718     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
719     return mImpulseMotor[mMapEntityToComponentIndex[jointEntity]];
720 }
721 
722 // Set the accumulated impulse for the motor constraint;
setImpulseMotor(Entity jointEntity,decimal impulseMotor)723 inline void HingeJointComponents::setImpulseMotor(Entity jointEntity, decimal impulseMotor) {
724 
725     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
726     mImpulseMotor[mMapEntityToComponentIndex[jointEntity]] = impulseMotor;
727 }
728 
729 // Return the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix)
getInverseMassMatrixLimitMotor(Entity jointEntity)730 inline decimal HingeJointComponents::getInverseMassMatrixLimitMotor(Entity jointEntity) const {
731 
732     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
733     return mInverseMassMatrixLimitMotor[mMapEntityToComponentIndex[jointEntity]];
734 }
735 
736 // Set the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix)
setInverseMassMatrixLimitMotor(Entity jointEntity,decimal inverseMassMatrixLimitMotor)737 inline void HingeJointComponents::setInverseMassMatrixLimitMotor(Entity jointEntity, decimal inverseMassMatrixLimitMotor) {
738 
739     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
740     mInverseMassMatrixLimitMotor[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrixLimitMotor;
741 }
742 
743 // Return the inverse of mass matrix K=JM^-1J^t for the motor
getInverseMassMatrixMotor(Entity jointEntity)744 inline decimal HingeJointComponents::getInverseMassMatrixMotor(Entity jointEntity) {
745 
746     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
747     return mInverseMassMatrixMotor[mMapEntityToComponentIndex[jointEntity]];
748 }
749 
750 // Return the inverse of mass matrix K=JM^-1J^t for the motor
setInverseMassMatrixMotor(Entity jointEntity,decimal inverseMassMatrixMotor)751 inline void HingeJointComponents::setInverseMassMatrixMotor(Entity jointEntity, decimal inverseMassMatrixMotor) {
752 
753     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
754     mInverseMassMatrixMotor[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrixMotor;
755 }
756 
757 // Return the bias of the lower limit constraint
getBLowerLimit(Entity jointEntity)758 inline decimal HingeJointComponents::getBLowerLimit(Entity jointEntity) const {
759 
760     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
761     return mBLowerLimit[mMapEntityToComponentIndex[jointEntity]];
762 }
763 
764 // Set the bias of the lower limit constraint
setBLowerLimit(Entity jointEntity,decimal bLowerLimit)765 inline void HingeJointComponents::setBLowerLimit(Entity jointEntity, decimal bLowerLimit) const {
766 
767     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
768     mBLowerLimit[mMapEntityToComponentIndex[jointEntity]] = bLowerLimit;
769 }
770 
771 // Return the bias of the upper limit constraint
getBUpperLimit(Entity jointEntity)772 inline decimal HingeJointComponents::getBUpperLimit(Entity jointEntity) const {
773 
774     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
775     return mBUpperLimit[mMapEntityToComponentIndex[jointEntity]];
776 }
777 
778 // Set the bias of the upper limit constraint
setBUpperLimit(Entity jointEntity,decimal bUpperLimit)779 inline void HingeJointComponents::setBUpperLimit(Entity jointEntity, decimal bUpperLimit) {
780 
781     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
782     mBUpperLimit[mMapEntityToComponentIndex[jointEntity]] = bUpperLimit;
783 }
784 
785 // Return true if the joint limits are enabled
getIsLimitEnabled(Entity jointEntity)786 inline bool HingeJointComponents::getIsLimitEnabled(Entity jointEntity) const {
787 
788     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
789     return mIsLimitEnabled[mMapEntityToComponentIndex[jointEntity]];
790 }
791 
792 // Set to true if the joint limits are enabled
setIsLimitEnabled(Entity jointEntity,bool isLimitEnabled)793 inline void HingeJointComponents::setIsLimitEnabled(Entity jointEntity, bool isLimitEnabled) {
794 
795     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
796     mIsLimitEnabled[mMapEntityToComponentIndex[jointEntity]] = isLimitEnabled;
797 }
798 
799 // Return true if the motor of the joint in enabled
getIsMotorEnabled(Entity jointEntity)800 inline bool HingeJointComponents::getIsMotorEnabled(Entity jointEntity) const {
801 
802     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
803     return mIsMotorEnabled[mMapEntityToComponentIndex[jointEntity]];
804 }
805 
806 // Set to true if the motor of the joint in enabled
setIsMotorEnabled(Entity jointEntity,bool isMotorEnabled)807 inline void HingeJointComponents::setIsMotorEnabled(Entity jointEntity, bool isMotorEnabled) const {
808 
809     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
810     mIsMotorEnabled[mMapEntityToComponentIndex[jointEntity]] = isMotorEnabled;
811 }
812 
813 // Return the Lower limit (minimum allowed rotation angle in radian)
getLowerLimit(Entity jointEntity)814 inline decimal HingeJointComponents::getLowerLimit(Entity jointEntity) const {
815 
816     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
817     return mLowerLimit[mMapEntityToComponentIndex[jointEntity]];
818 }
819 
820 // Set the Lower limit (minimum allowed rotation angle in radian)
setLowerLimit(Entity jointEntity,decimal lowerLimit)821 inline void HingeJointComponents::setLowerLimit(Entity jointEntity, decimal lowerLimit) const {
822 
823     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
824     mLowerLimit[mMapEntityToComponentIndex[jointEntity]] = lowerLimit;
825 }
826 
827 // Return the upper limit (maximum translation distance)
getUpperLimit(Entity jointEntity)828 inline decimal HingeJointComponents::getUpperLimit(Entity jointEntity) const {
829 
830     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
831     return mUpperLimit[mMapEntityToComponentIndex[jointEntity]];
832 }
833 
834 // Set the upper limit (maximum translation distance)
setUpperLimit(Entity jointEntity,decimal upperLimit)835 inline void HingeJointComponents::setUpperLimit(Entity jointEntity, decimal upperLimit) {
836 
837     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
838     mUpperLimit[mMapEntityToComponentIndex[jointEntity]] = upperLimit;
839 }
840 
841 // Return true if the lower limit is violated
getIsLowerLimitViolated(Entity jointEntity)842 inline bool HingeJointComponents::getIsLowerLimitViolated(Entity jointEntity) const {
843 
844     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
845     return mIsLowerLimitViolated[mMapEntityToComponentIndex[jointEntity]];
846 }
847 
848 // Set to true if the lower limit is violated
setIsLowerLimitViolated(Entity jointEntity,bool isLowerLimitViolated)849 inline void HingeJointComponents::setIsLowerLimitViolated(Entity jointEntity, bool isLowerLimitViolated) {
850 
851     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
852     mIsLowerLimitViolated[mMapEntityToComponentIndex[jointEntity]] = isLowerLimitViolated;
853 }
854 
855 // Return true if the upper limit is violated
getIsUpperLimitViolated(Entity jointEntity)856 inline bool HingeJointComponents::getIsUpperLimitViolated(Entity jointEntity) const {
857 
858     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
859     return mIsUpperLimitViolated[mMapEntityToComponentIndex[jointEntity]];
860 }
861 
862 // Set to true if the upper limit is violated
setIsUpperLimitViolated(Entity jointEntity,bool isUpperLimitViolated)863 inline void HingeJointComponents::setIsUpperLimitViolated(Entity jointEntity, bool isUpperLimitViolated) const {
864 
865     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
866     mIsUpperLimitViolated[mMapEntityToComponentIndex[jointEntity]] = isUpperLimitViolated;
867 }
868 
869 // Return the motor speed (in rad/s)
getMotorSpeed(Entity jointEntity)870 inline decimal HingeJointComponents::getMotorSpeed(Entity jointEntity) const {
871 
872     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
873     return mMotorSpeed[mMapEntityToComponentIndex[jointEntity]];
874 }
875 
876 // Set the motor speed (in rad/s)
setMotorSpeed(Entity jointEntity,decimal motorSpeed)877 inline void HingeJointComponents::setMotorSpeed(Entity jointEntity, decimal motorSpeed) {
878 
879     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
880     mMotorSpeed[mMapEntityToComponentIndex[jointEntity]] = motorSpeed;
881 }
882 
883 // Return the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
getMaxMotorTorque(Entity jointEntity)884 inline decimal HingeJointComponents::getMaxMotorTorque(Entity jointEntity) const {
885 
886     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
887     return mMaxMotorTorque[mMapEntityToComponentIndex[jointEntity]];
888 }
889 
890 // Set the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
setMaxMotorTorque(Entity jointEntity,decimal maxMotorTorque)891 inline void HingeJointComponents::setMaxMotorTorque(Entity jointEntity, decimal maxMotorTorque) {
892 
893     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
894     mMaxMotorTorque[mMapEntityToComponentIndex[jointEntity]] = maxMotorTorque;
895 }
896 
897 }
898 
899 #endif
900