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 // Libraries
27 #include <reactphysics3d/components/HingeJointComponents.h>
28 #include <reactphysics3d/engine/EntityManager.h>
29 #include <reactphysics3d/mathematics/Matrix3x3.h>
30 #include <cassert>
31
32 // We want to use the ReactPhysics3D namespace
33 using namespace reactphysics3d;
34
35 // Constructor
HingeJointComponents(MemoryAllocator & allocator)36 HingeJointComponents::HingeJointComponents(MemoryAllocator& allocator)
37 :Components(allocator, sizeof(Entity) + sizeof(HingeJoint*) + sizeof(Vector3) +
38 sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
39 sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector3) +
40 sizeof(Vector2) + sizeof(Matrix3x3) + sizeof(Matrix2x2) +
41 sizeof(Vector3) + sizeof(Vector2) + sizeof(Quaternion) +
42 sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
43 sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) +
44 sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) +
45 sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(decimal) +
46 sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(decimal)) {
47
48 // Allocate memory for the components data
49 allocate(INIT_NB_ALLOCATED_COMPONENTS);
50 }
51
52 // Allocate memory for a given number of components
allocate(uint32 nbComponentsToAllocate)53 void HingeJointComponents::allocate(uint32 nbComponentsToAllocate) {
54
55 assert(nbComponentsToAllocate > mNbAllocatedComponents);
56
57 // Size for the data of a single component (in bytes)
58 const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize;
59
60 // Allocate memory
61 void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes);
62 assert(newBuffer != nullptr);
63
64 // New pointers to components data
65 Entity* newJointEntities = static_cast<Entity*>(newBuffer);
66 HingeJoint** newJoints = reinterpret_cast<HingeJoint**>(newJointEntities + nbComponentsToAllocate);
67 Vector3* newLocalAnchorPointBody1 = reinterpret_cast<Vector3*>(newJoints + nbComponentsToAllocate);
68 Vector3* newLocalAnchorPointBody2 = reinterpret_cast<Vector3*>(newLocalAnchorPointBody1 + nbComponentsToAllocate);
69 Vector3* newR1World = reinterpret_cast<Vector3*>(newLocalAnchorPointBody2 + nbComponentsToAllocate);
70 Vector3* newR2World = reinterpret_cast<Vector3*>(newR1World + nbComponentsToAllocate);
71 Matrix3x3* newI1 = reinterpret_cast<Matrix3x3*>(newR2World + nbComponentsToAllocate);
72 Matrix3x3* newI2 = reinterpret_cast<Matrix3x3*>(newI1 + nbComponentsToAllocate);
73 Vector3* newImpulseTranslation = reinterpret_cast<Vector3*>(newI2 + nbComponentsToAllocate);
74 Vector2* newImpulseRotation = reinterpret_cast<Vector2*>(newImpulseTranslation + nbComponentsToAllocate);
75 Matrix3x3* newInverseMassMatrixTranslation = reinterpret_cast<Matrix3x3*>(newImpulseRotation + nbComponentsToAllocate);
76 Matrix2x2* newInverseMassMatrixRotation = reinterpret_cast<Matrix2x2*>(newInverseMassMatrixTranslation + nbComponentsToAllocate);
77 Vector3* newBiasTranslation = reinterpret_cast<Vector3*>(newInverseMassMatrixRotation + nbComponentsToAllocate);
78 Vector2* newBiasRotation = reinterpret_cast<Vector2*>(newBiasTranslation + nbComponentsToAllocate);
79 Quaternion* newInitOrientationDifferenceInv = reinterpret_cast<Quaternion*>(newBiasRotation + nbComponentsToAllocate);
80 Vector3* newHingeLocalAxisBody1 = reinterpret_cast<Vector3*>(newInitOrientationDifferenceInv + nbComponentsToAllocate);
81 Vector3* newHingeLocalAxisBody2 = reinterpret_cast<Vector3*>(newHingeLocalAxisBody1 + nbComponentsToAllocate);
82 Vector3* newA1 = reinterpret_cast<Vector3*>(newHingeLocalAxisBody2 + nbComponentsToAllocate);
83 Vector3* newB2CrossA1 = reinterpret_cast<Vector3*>(newA1 + nbComponentsToAllocate);
84 Vector3* newC2CrossA1 = reinterpret_cast<Vector3*>(newB2CrossA1 + nbComponentsToAllocate);
85 decimal* newImpulseLowerLimit = reinterpret_cast<decimal*>(newC2CrossA1 + nbComponentsToAllocate);
86 decimal* newImpulseUpperLimit = reinterpret_cast<decimal*>(newImpulseLowerLimit + nbComponentsToAllocate);
87 decimal* newImpulseMotor = reinterpret_cast<decimal*>(newImpulseUpperLimit + nbComponentsToAllocate);
88 decimal* newInverseMassMatrixLimitMotor = reinterpret_cast<decimal*>(newImpulseMotor + nbComponentsToAllocate);
89 decimal* newInverseMassMatrixMotor = reinterpret_cast<decimal*>(newInverseMassMatrixLimitMotor + nbComponentsToAllocate);
90 decimal* newBLowerLimit = reinterpret_cast<decimal*>(newInverseMassMatrixMotor + nbComponentsToAllocate);
91 decimal* newBUpperLimit = reinterpret_cast<decimal*>(newBLowerLimit + nbComponentsToAllocate);
92 bool* newIsLimitEnabled = reinterpret_cast<bool*>(newBUpperLimit + nbComponentsToAllocate);
93 bool* newIsMotorEnabled = reinterpret_cast<bool*>(newIsLimitEnabled + nbComponentsToAllocate);
94 decimal* newLowerLimit = reinterpret_cast<decimal*>(newIsMotorEnabled + nbComponentsToAllocate);
95 decimal* newUpperLimit = reinterpret_cast<decimal*>(newLowerLimit + nbComponentsToAllocate);
96 bool* newIsLowerLimitViolated = reinterpret_cast<bool*>(newUpperLimit + nbComponentsToAllocate);
97 bool* newIsUpperLimitViolated = reinterpret_cast<bool*>(newIsLowerLimitViolated + nbComponentsToAllocate);
98 decimal* newMotorSpeed = reinterpret_cast<decimal*>(newIsUpperLimitViolated + nbComponentsToAllocate);
99 decimal* newMaxMotorTorque = reinterpret_cast<decimal*>(newMotorSpeed + nbComponentsToAllocate);
100
101 // If there was already components before
102 if (mNbComponents > 0) {
103
104 // Copy component data from the previous buffer to the new one
105 memcpy(newJointEntities, mJointEntities, mNbComponents * sizeof(Entity));
106 memcpy(newJoints, mJoints, mNbComponents * sizeof(HingeJoint*));
107 memcpy(newLocalAnchorPointBody1, mLocalAnchorPointBody1, mNbComponents * sizeof(Vector3));
108 memcpy(newLocalAnchorPointBody2, mLocalAnchorPointBody2, mNbComponents * sizeof(Vector3));
109 memcpy(newR1World, mR1World, mNbComponents * sizeof(Vector3));
110 memcpy(newR2World, mR2World, mNbComponents * sizeof(Vector3));
111 memcpy(newI1, mI1, mNbComponents * sizeof(Matrix3x3));
112 memcpy(newI2, mI2, mNbComponents * sizeof(Matrix3x3));
113 memcpy(newImpulseTranslation, mImpulseTranslation, mNbComponents * sizeof(Vector3));
114 memcpy(newImpulseRotation, mImpulseRotation, mNbComponents * sizeof(Vector2));
115 memcpy(newInverseMassMatrixTranslation, mInverseMassMatrixTranslation, mNbComponents * sizeof(Matrix3x3));
116 memcpy(newInverseMassMatrixRotation, mInverseMassMatrixRotation, mNbComponents * sizeof(Matrix2x2));
117 memcpy(newBiasTranslation, mBiasTranslation, mNbComponents * sizeof(Vector3));
118 memcpy(newBiasRotation, mBiasRotation, mNbComponents * sizeof(Vector2));
119 memcpy(newInitOrientationDifferenceInv, mInitOrientationDifferenceInv, mNbComponents * sizeof(Quaternion));
120 memcpy(newHingeLocalAxisBody1, mHingeLocalAxisBody1, mNbComponents * sizeof(Vector3));
121 memcpy(newHingeLocalAxisBody2, mHingeLocalAxisBody2, mNbComponents * sizeof(Vector3));
122 memcpy(newA1, mA1, mNbComponents * sizeof(Vector3));
123 memcpy(newB2CrossA1, mB2CrossA1, mNbComponents * sizeof(Vector3));
124 memcpy(newC2CrossA1, mC2CrossA1, mNbComponents * sizeof(Vector3));
125 memcpy(newImpulseLowerLimit, mImpulseLowerLimit, mNbComponents * sizeof(decimal));
126 memcpy(newImpulseUpperLimit, mImpulseUpperLimit, mNbComponents * sizeof(decimal));
127 memcpy(newImpulseMotor, mImpulseMotor, mNbComponents * sizeof(decimal));
128 memcpy(newInverseMassMatrixLimitMotor, mInverseMassMatrixLimitMotor, mNbComponents * sizeof(decimal));
129 memcpy(newInverseMassMatrixMotor, mInverseMassMatrixMotor, mNbComponents * sizeof(decimal));
130 memcpy(newBLowerLimit, mBLowerLimit, mNbComponents * sizeof(decimal));
131 memcpy(newBUpperLimit, mBUpperLimit, mNbComponents * sizeof(decimal));
132 memcpy(newIsLimitEnabled, mIsLimitEnabled, mNbComponents * sizeof(bool));
133 memcpy(newIsMotorEnabled, mIsMotorEnabled, mNbComponents * sizeof(bool));
134 memcpy(newLowerLimit, mLowerLimit, mNbComponents * sizeof(decimal));
135 memcpy(newUpperLimit, mUpperLimit, mNbComponents * sizeof(decimal));
136 memcpy(newIsLowerLimitViolated, mIsLowerLimitViolated, mNbComponents * sizeof(bool));
137 memcpy(newIsUpperLimitViolated, mIsUpperLimitViolated, mNbComponents * sizeof(bool));
138 memcpy(newMotorSpeed, mMotorSpeed, mNbComponents * sizeof(decimal));
139 memcpy(newMaxMotorTorque, mMaxMotorTorque, mNbComponents * sizeof(decimal));
140
141 // Deallocate previous memory
142 mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
143 }
144
145 mBuffer = newBuffer;
146 mJointEntities = newJointEntities;
147 mJoints = newJoints;
148 mNbAllocatedComponents = nbComponentsToAllocate;
149 mLocalAnchorPointBody1 = newLocalAnchorPointBody1;
150 mLocalAnchorPointBody2 = newLocalAnchorPointBody2;
151 mR1World = newR1World;
152 mR2World = newR2World;
153 mI1 = newI1;
154 mI2 = newI2;
155 mImpulseTranslation = newImpulseTranslation;
156 mImpulseRotation = newImpulseRotation;
157 mInverseMassMatrixTranslation = newInverseMassMatrixTranslation;
158 mInverseMassMatrixRotation = newInverseMassMatrixRotation;
159 mBiasTranslation = newBiasTranslation;
160 mBiasRotation = newBiasRotation;
161 mInitOrientationDifferenceInv = newInitOrientationDifferenceInv;
162 mHingeLocalAxisBody1 = newHingeLocalAxisBody1;
163 mHingeLocalAxisBody2 = newHingeLocalAxisBody2;
164 mA1 = newA1;
165 mB2CrossA1 = newB2CrossA1;
166 mC2CrossA1 = newC2CrossA1;
167 mImpulseLowerLimit = newImpulseLowerLimit;
168 mImpulseUpperLimit = newImpulseUpperLimit;
169 mImpulseMotor = newImpulseMotor;
170 mInverseMassMatrixLimitMotor = newInverseMassMatrixLimitMotor;
171 mInverseMassMatrixMotor = newInverseMassMatrixMotor;
172 mBLowerLimit = newBLowerLimit;
173 mBUpperLimit = newBUpperLimit;
174 mIsLimitEnabled = newIsLimitEnabled;
175 mIsMotorEnabled = newIsMotorEnabled;
176 mLowerLimit = newLowerLimit;
177 mUpperLimit = newUpperLimit;
178 mIsLowerLimitViolated = newIsLowerLimitViolated;
179 mIsUpperLimitViolated = newIsUpperLimitViolated;
180 mMotorSpeed = newMotorSpeed;
181 mMaxMotorTorque = newMaxMotorTorque;
182 }
183
184 // Add a component
addComponent(Entity jointEntity,bool isSleeping,const HingeJointComponent & component)185 void HingeJointComponents::addComponent(Entity jointEntity, bool isSleeping, const HingeJointComponent& component) {
186
187 // Prepare to add new component (allocate memory if necessary and compute insertion index)
188 uint32 index = prepareAddComponent(isSleeping);
189
190 // Insert the new component data
191 new (mJointEntities + index) Entity(jointEntity);
192 mJoints[index] = nullptr;
193 new (mLocalAnchorPointBody1 + index) Vector3(0, 0, 0);
194 new (mLocalAnchorPointBody2 + index) Vector3(0, 0, 0);
195 new (mR1World + index) Vector3(0, 0, 0);
196 new (mR2World + index) Vector3(0, 0, 0);
197 new (mI1 + index) Matrix3x3();
198 new (mI2 + index) Matrix3x3();
199 new (mImpulseTranslation + index) Vector3(0, 0, 0);
200 new (mImpulseRotation + index) Vector2(0, 0);
201 new (mInverseMassMatrixTranslation + index) Matrix3x3();
202 new (mInverseMassMatrixRotation + index) Matrix2x2();
203 new (mBiasTranslation + index) Vector3(0, 0, 0);
204 new (mBiasRotation + index) Vector2(0, 0);
205 new (mInitOrientationDifferenceInv + index) Quaternion(0, 0, 0, 0);
206 new (mHingeLocalAxisBody1 + index) Vector3(0, 0, 0);
207 new (mHingeLocalAxisBody2 + index) Vector3(0, 0, 0);
208 new (mA1 + index) Vector3(0, 0, 0);
209 new (mB2CrossA1 + index) Vector3(0, 0, 0);
210 new (mC2CrossA1 + index) Vector3(0, 0, 0);
211 mImpulseLowerLimit[index] = decimal(0.0);
212 mImpulseUpperLimit[index] = decimal(0.0);
213 mImpulseMotor[index] = decimal(0.0);
214 mInverseMassMatrixLimitMotor[index] = decimal(0.0);
215 mInverseMassMatrixMotor[index] = decimal(0.0);
216 mBLowerLimit[index] = decimal(0.0);
217 mBUpperLimit[index] = decimal(0.0);
218 mIsLimitEnabled[index] = component.isLimitEnabled;
219 mIsMotorEnabled[index] = component.isMotorEnabled;
220 mLowerLimit[index] = component.lowerLimit;
221 mUpperLimit[index] = component.upperLimit;
222 mIsLowerLimitViolated[index] = false;
223 mIsUpperLimitViolated[index] = false;
224 mMotorSpeed[index] = component.motorSpeed;
225 mMaxMotorTorque[index] = component.maxMotorTorque;
226
227 // Map the entity with the new component lookup index
228 mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity, index));
229
230 mNbComponents++;
231
232 assert(mDisabledStartIndex <= mNbComponents);
233 assert(mNbComponents == static_cast<uint32>(mMapEntityToComponentIndex.size()));
234 }
235
236 // Move a component from a source to a destination index in the components array
237 // The destination location must contain a constructed object
moveComponentToIndex(uint32 srcIndex,uint32 destIndex)238 void HingeJointComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) {
239
240 const Entity entity = mJointEntities[srcIndex];
241
242 // Copy the data of the source component to the destination location
243 new (mJointEntities + destIndex) Entity(mJointEntities[srcIndex]);
244 mJoints[destIndex] = mJoints[srcIndex];
245 new (mLocalAnchorPointBody1 + destIndex) Vector3(mLocalAnchorPointBody1[srcIndex]);
246 new (mLocalAnchorPointBody2 + destIndex) Vector3(mLocalAnchorPointBody2[srcIndex]);
247 new (mR1World + destIndex) Vector3(mR1World[srcIndex]);
248 new (mR2World + destIndex) Vector3(mR2World[srcIndex]);
249 new (mI1 + destIndex) Matrix3x3(mI1[srcIndex]);
250 new (mI2 + destIndex) Matrix3x3(mI2[srcIndex]);
251 new (mImpulseTranslation + destIndex) Vector3(mImpulseTranslation[srcIndex]);
252 new (mImpulseRotation + destIndex) Vector2(mImpulseRotation[srcIndex]);
253 new (mInverseMassMatrixTranslation + destIndex) Matrix3x3(mInverseMassMatrixTranslation[srcIndex]);
254 new (mInverseMassMatrixRotation + destIndex) Matrix2x2(mInverseMassMatrixRotation[srcIndex]);
255 new (mBiasTranslation + destIndex) Vector3(mBiasTranslation[srcIndex]);
256 new (mBiasRotation + destIndex) Vector2(mBiasRotation[srcIndex]);
257 new (mInitOrientationDifferenceInv + destIndex) Quaternion(mInitOrientationDifferenceInv[srcIndex]);
258 new (mHingeLocalAxisBody1 + destIndex) Vector3(mHingeLocalAxisBody1[srcIndex]);
259 new (mHingeLocalAxisBody2 + destIndex) Vector3(mHingeLocalAxisBody2[srcIndex]);
260 new (mA1 + destIndex) Vector3(mA1[srcIndex]);
261 new (mB2CrossA1 + destIndex) Vector3(mB2CrossA1[srcIndex]);
262 new (mC2CrossA1 + destIndex) Vector3(mC2CrossA1[srcIndex]);
263 mImpulseLowerLimit[destIndex] = mImpulseLowerLimit[srcIndex];
264 mImpulseUpperLimit[destIndex] = mImpulseUpperLimit[srcIndex];
265 mImpulseMotor[destIndex] = mImpulseMotor[srcIndex];
266 mInverseMassMatrixLimitMotor[destIndex] = mInverseMassMatrixLimitMotor[srcIndex];
267 mInverseMassMatrixMotor[destIndex] = mInverseMassMatrixMotor[srcIndex];
268 mBLowerLimit[destIndex] = mBLowerLimit[srcIndex];
269 mBUpperLimit[destIndex] = mBUpperLimit[srcIndex];
270 mIsLimitEnabled[destIndex] = mIsLimitEnabled[srcIndex];
271 mIsMotorEnabled[destIndex] = mIsMotorEnabled[srcIndex];
272 mLowerLimit[destIndex] = mLowerLimit[srcIndex];
273 mUpperLimit[destIndex] = mUpperLimit[srcIndex];
274 mIsLowerLimitViolated[destIndex] = mIsLowerLimitViolated[srcIndex];
275 mIsUpperLimitViolated[destIndex] = mIsUpperLimitViolated[srcIndex];
276 mMotorSpeed[destIndex] = mMotorSpeed[srcIndex];
277 mMaxMotorTorque[destIndex] = mMaxMotorTorque[srcIndex];
278
279 // Destroy the source component
280 destroyComponent(srcIndex);
281
282 assert(!mMapEntityToComponentIndex.containsKey(entity));
283
284 // Update the entity to component index mapping
285 mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity, destIndex));
286
287 assert(mMapEntityToComponentIndex[mJointEntities[destIndex]] == destIndex);
288 }
289
290 // Swap two components in the array
swapComponents(uint32 index1,uint32 index2)291 void HingeJointComponents::swapComponents(uint32 index1, uint32 index2) {
292
293 // Copy component 1 data
294 Entity jointEntity1(mJointEntities[index1]);
295 HingeJoint* joint1 = mJoints[index1];
296 Vector3 localAnchorPointBody1(mLocalAnchorPointBody1[index1]);
297 Vector3 localAnchorPointBody2(mLocalAnchorPointBody2[index1]);
298 Vector3 r1World1(mR1World[index1]);
299 Vector3 r2World1(mR2World[index1]);
300 Matrix3x3 i11(mI1[index1]);
301 Matrix3x3 i21(mI2[index1]);
302 Vector3 impulseTranslation1(mImpulseTranslation[index1]);
303 Vector2 impulseRotation1(mImpulseRotation[index1]);
304 Matrix3x3 inverseMassMatrixTranslation1(mInverseMassMatrixTranslation[index1]);
305 Matrix2x2 inverseMassMatrixRotation1(mInverseMassMatrixRotation[index1]);
306 Vector3 biasTranslation1(mBiasTranslation[index1]);
307 Vector2 biasRotation1(mBiasRotation[index1]);
308 Quaternion initOrientationDifferenceInv1(mInitOrientationDifferenceInv[index1]);
309 Vector3 hingeLocalAxisBody1(mHingeLocalAxisBody1[index1]);
310 Vector3 hingeLocalAxisBody2(mHingeLocalAxisBody2[index1]);
311 Vector3 a1(mA1[index1]);
312 Vector3 b2CrossA1(mB2CrossA1[index1]);
313 Vector3 c2CrossA1(mC2CrossA1[index1]);
314 decimal impulseLowerLimit(mImpulseLowerLimit[index1]);
315 decimal impulseUpperLimit(mImpulseUpperLimit[index1]);
316 decimal impulseMotor(mImpulseMotor[index1]);
317 decimal inverseMassMatrixLimitMotor(mInverseMassMatrixLimitMotor[index1]);
318 decimal inverseMassMatrixMotor(mInverseMassMatrixMotor[index1]);
319 decimal bLowerLimit(mBLowerLimit[index1]);
320 decimal bUpperLimit(mUpperLimit[index1]);
321 bool isLimitEnabled(mIsLimitEnabled[index1]);
322 bool isMotorEnabled(mIsMotorEnabled[index1]);
323 decimal lowerLimit(mLowerLimit[index1]);
324 decimal upperLimit(mUpperLimit[index1]);
325 bool isLowerLimitViolated(mIsLowerLimitViolated[index1]);
326 bool isUpperLimitViolated(mIsUpperLimitViolated[index1]);
327 decimal motorSpeed(mMotorSpeed[index1]);
328 decimal maxMotorTorque(mMaxMotorTorque[index1]);
329
330 // Destroy component 1
331 destroyComponent(index1);
332
333 moveComponentToIndex(index2, index1);
334
335 // Reconstruct component 1 at component 2 location
336 new (mJointEntities + index2) Entity(jointEntity1);
337 mJoints[index2] = joint1;
338 new (mLocalAnchorPointBody1 + index2) Vector3(localAnchorPointBody1);
339 new (mLocalAnchorPointBody2 + index2) Vector3(localAnchorPointBody2);
340 new (mR1World + index2) Vector3(r1World1);
341 new (mR2World + index2) Vector3(r2World1);
342 new (mI1 + index2) Matrix3x3(i11);
343 new (mI2 + index2) Matrix3x3(i21);
344 new (mImpulseTranslation + index2) Vector3(impulseTranslation1);
345 new (mImpulseRotation + index2) Vector2(impulseRotation1);
346 new (mInverseMassMatrixTranslation + index2) Matrix3x3(inverseMassMatrixTranslation1);
347 new (mInverseMassMatrixRotation + index2) Matrix2x2(inverseMassMatrixRotation1);
348 new (mBiasTranslation + index2) Vector3(biasTranslation1);
349 new (mBiasRotation + index2) Vector2(biasRotation1);
350 new (mInitOrientationDifferenceInv + index2) Quaternion(initOrientationDifferenceInv1);
351 new (mHingeLocalAxisBody1 + index2) Vector3(hingeLocalAxisBody1);
352 new (mHingeLocalAxisBody2 + index2) Vector3(hingeLocalAxisBody2);
353 new (mA1 + index2) Vector3(a1);
354 new (mB2CrossA1 + index2) Vector3(b2CrossA1);
355 new (mC2CrossA1 + index2) Vector3(c2CrossA1);
356 mImpulseLowerLimit[index2] = impulseLowerLimit;
357 mImpulseUpperLimit[index2] = impulseUpperLimit;
358 mImpulseMotor[index2] = impulseMotor;
359 mInverseMassMatrixLimitMotor[index2] = inverseMassMatrixLimitMotor;
360 mInverseMassMatrixMotor[index2] = inverseMassMatrixMotor;
361 mBLowerLimit[index2] = bLowerLimit;
362 mBUpperLimit[index2] = bUpperLimit;
363 mIsLimitEnabled[index2] = isLimitEnabled;
364 mIsMotorEnabled[index2] = isMotorEnabled;
365 mLowerLimit[index2] = lowerLimit;
366 mUpperLimit[index2] = upperLimit;
367 mIsLowerLimitViolated[index2] = isLowerLimitViolated;
368 mIsUpperLimitViolated[index2] = isUpperLimitViolated;
369 mMotorSpeed[index2] = motorSpeed;
370 mMaxMotorTorque[index2] = maxMotorTorque;
371
372 // Update the entity to component index mapping
373 mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity1, index2));
374
375 assert(mMapEntityToComponentIndex[mJointEntities[index1]] == index1);
376 assert(mMapEntityToComponentIndex[mJointEntities[index2]] == index2);
377 assert(mNbComponents == static_cast<uint32>(mMapEntityToComponentIndex.size()));
378 }
379
380 // Destroy a component at a given index
destroyComponent(uint32 index)381 void HingeJointComponents::destroyComponent(uint32 index) {
382
383 Components::destroyComponent(index);
384
385 assert(mMapEntityToComponentIndex[mJointEntities[index]] == index);
386
387 mMapEntityToComponentIndex.remove(mJointEntities[index]);
388
389 mJointEntities[index].~Entity();
390 mJoints[index] = nullptr;
391 mLocalAnchorPointBody1[index].~Vector3();
392 mLocalAnchorPointBody2[index].~Vector3();
393 mR1World[index].~Vector3();
394 mR2World[index].~Vector3();
395 mI1[index].~Matrix3x3();
396 mI2[index].~Matrix3x3();
397 mImpulseTranslation[index].~Vector3();
398 mImpulseRotation[index].~Vector2();
399 mInverseMassMatrixTranslation[index].~Matrix3x3();
400 mInverseMassMatrixRotation[index].~Matrix2x2();
401 mBiasTranslation[index].~Vector3();
402 mBiasRotation[index].~Vector2();
403 mInitOrientationDifferenceInv[index].~Quaternion();
404 mHingeLocalAxisBody1[index].~Vector3();
405 mHingeLocalAxisBody2[index].~Vector3();
406 mA1[index].~Vector3();
407 mB2CrossA1[index].~Vector3();
408 mC2CrossA1[index].~Vector3();
409 }
410