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