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/RigidBodyComponents.h>
28 #include <reactphysics3d/engine/EntityManager.h>
29 #include <reactphysics3d/body/RigidBody.h>
30 #include <cassert>
31 #include <random>
32
33 // We want to use the ReactPhysics3D namespace
34 using namespace reactphysics3d;
35
36 // Constructor
RigidBodyComponents(MemoryAllocator & allocator)37 RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator)
38 :Components(allocator, sizeof(Entity) + sizeof(RigidBody*) +
39 sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(BodyType) +
40 sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
41 sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) +
42 sizeof(decimal) + sizeof(decimal) + sizeof(Vector3) +
43 sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
44 sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
45 sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) +
46 sizeof(bool) + sizeof(bool) + sizeof(List<Entity>)) {
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 RigidBodyComponents::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* newBodiesEntities = static_cast<Entity*>(newBuffer);
66 RigidBody** newBodies = reinterpret_cast<RigidBody**>(newBodiesEntities + nbComponentsToAllocate);
67 bool* newIsAllowedToSleep = reinterpret_cast<bool*>(newBodies + nbComponentsToAllocate);
68 bool* newIsSleeping = reinterpret_cast<bool*>(newIsAllowedToSleep + nbComponentsToAllocate);
69 decimal* newSleepTimes = reinterpret_cast<decimal*>(newIsSleeping + nbComponentsToAllocate);
70 BodyType* newBodyTypes = reinterpret_cast<BodyType*>(newSleepTimes + nbComponentsToAllocate);
71 Vector3* newLinearVelocities = reinterpret_cast<Vector3*>(newBodyTypes + nbComponentsToAllocate);
72 Vector3* newAngularVelocities = reinterpret_cast<Vector3*>(newLinearVelocities + nbComponentsToAllocate);
73 Vector3* newExternalForces = reinterpret_cast<Vector3*>(newAngularVelocities + nbComponentsToAllocate);
74 Vector3* newExternalTorques = reinterpret_cast<Vector3*>(newExternalForces + nbComponentsToAllocate);
75 decimal* newLinearDampings = reinterpret_cast<decimal*>(newExternalTorques + nbComponentsToAllocate);
76 decimal* newAngularDampings = reinterpret_cast<decimal*>(newLinearDampings + nbComponentsToAllocate);
77 decimal* newMasses = reinterpret_cast<decimal*>(newAngularDampings + nbComponentsToAllocate);
78 decimal* newInverseMasses = reinterpret_cast<decimal*>(newMasses + nbComponentsToAllocate);
79 Vector3* newInertiaTensorLocal = reinterpret_cast<Vector3*>(newInverseMasses + nbComponentsToAllocate);
80 Vector3* newInertiaTensorLocalInverses = reinterpret_cast<Vector3*>(newInertiaTensorLocal + nbComponentsToAllocate);
81 Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
82 Vector3* newConstrainedAngularVelocities = reinterpret_cast<Vector3*>(newConstrainedLinearVelocities + nbComponentsToAllocate);
83 Vector3* newSplitLinearVelocities = reinterpret_cast<Vector3*>(newConstrainedAngularVelocities + nbComponentsToAllocate);
84 Vector3* newSplitAngularVelocities = reinterpret_cast<Vector3*>(newSplitLinearVelocities + nbComponentsToAllocate);
85 Vector3* newConstrainedPositions = reinterpret_cast<Vector3*>(newSplitAngularVelocities + nbComponentsToAllocate);
86 Quaternion* newConstrainedOrientations = reinterpret_cast<Quaternion*>(newConstrainedPositions + nbComponentsToAllocate);
87 Vector3* newCentersOfMassLocal = reinterpret_cast<Vector3*>(newConstrainedOrientations + nbComponentsToAllocate);
88 Vector3* newCentersOfMassWorld = reinterpret_cast<Vector3*>(newCentersOfMassLocal + nbComponentsToAllocate);
89 bool* newIsGravityEnabled = reinterpret_cast<bool*>(newCentersOfMassWorld + nbComponentsToAllocate);
90 bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newIsGravityEnabled + nbComponentsToAllocate);
91 List<Entity>* newJoints = reinterpret_cast<List<Entity>*>(newIsAlreadyInIsland + nbComponentsToAllocate);
92
93 // If there was already components before
94 if (mNbComponents > 0) {
95
96 // Copy component data from the previous buffer to the new one
97 memcpy(newBodiesEntities, mBodiesEntities, mNbComponents * sizeof(Entity));
98 memcpy(newBodies, mRigidBodies, mNbComponents * sizeof(RigidBody*));
99 memcpy(newIsAllowedToSleep, mIsAllowedToSleep, mNbComponents * sizeof(bool));
100 memcpy(newIsSleeping, mIsSleeping, mNbComponents * sizeof(bool));
101 memcpy(newSleepTimes, mSleepTimes, mNbComponents * sizeof(bool));
102 memcpy(newBodyTypes, mBodyTypes, mNbComponents * sizeof(BodyType));
103 memcpy(newLinearVelocities, mLinearVelocities, mNbComponents * sizeof(Vector3));
104 memcpy(newAngularVelocities, mAngularVelocities, mNbComponents * sizeof(Vector3));
105 memcpy(newExternalForces, mExternalForces, mNbComponents * sizeof(Vector3));
106 memcpy(newExternalTorques, mExternalTorques, mNbComponents * sizeof(Vector3));
107 memcpy(newLinearDampings, mLinearDampings, mNbComponents * sizeof(decimal));
108 memcpy(newAngularDampings, mAngularDampings, mNbComponents * sizeof(decimal));
109 memcpy(newMasses, mMasses, mNbComponents * sizeof(decimal));
110 memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal));
111 memcpy(newInertiaTensorLocal, mLocalInertiaTensors, mNbComponents * sizeof(Vector3));
112 memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Vector3));
113 memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3));
114 memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3));
115 memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3));
116 memcpy(newSplitAngularVelocities, mSplitAngularVelocities, mNbComponents * sizeof(Vector3));
117 memcpy(newConstrainedPositions, mConstrainedPositions, mNbComponents * sizeof(Vector3));
118 memcpy(newConstrainedOrientations, mConstrainedOrientations, mNbComponents * sizeof(Quaternion));
119 memcpy(newCentersOfMassLocal, mCentersOfMassLocal, mNbComponents * sizeof(Vector3));
120 memcpy(newCentersOfMassWorld, mCentersOfMassWorld, mNbComponents * sizeof(Vector3));
121 memcpy(newIsGravityEnabled, mIsGravityEnabled, mNbComponents * sizeof(bool));
122 memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool));
123 memcpy(newJoints, mJoints, mNbComponents * sizeof(List<Entity>));
124
125 // Deallocate previous memory
126 mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
127 }
128
129 mBuffer = newBuffer;
130 mBodiesEntities = newBodiesEntities;
131 mRigidBodies = newBodies;
132 mIsAllowedToSleep = newIsAllowedToSleep;
133 mIsSleeping = newIsSleeping;
134 mSleepTimes = newSleepTimes;
135 mNbAllocatedComponents = nbComponentsToAllocate;
136 mBodyTypes = newBodyTypes;
137 mLinearVelocities = newLinearVelocities;
138 mAngularVelocities = newAngularVelocities;
139 mExternalForces = newExternalForces;
140 mExternalTorques = newExternalTorques;
141 mLinearDampings = newLinearDampings;
142 mAngularDampings = newAngularDampings;
143 mMasses = newMasses;
144 mInverseMasses = newInverseMasses;
145 mLocalInertiaTensors = newInertiaTensorLocal;
146 mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses;
147 mConstrainedLinearVelocities = newConstrainedLinearVelocities;
148 mConstrainedAngularVelocities = newConstrainedAngularVelocities;
149 mSplitLinearVelocities = newSplitLinearVelocities;
150 mSplitAngularVelocities = newSplitAngularVelocities;
151 mConstrainedPositions = newConstrainedPositions;
152 mConstrainedOrientations = newConstrainedOrientations;
153 mCentersOfMassLocal = newCentersOfMassLocal;
154 mCentersOfMassWorld = newCentersOfMassWorld;
155 mIsGravityEnabled = newIsGravityEnabled;
156 mIsAlreadyInIsland = newIsAlreadyInIsland;
157 mJoints = newJoints;
158 }
159
160 // Add a component
addComponent(Entity bodyEntity,bool isSleeping,const RigidBodyComponent & component)161 void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const RigidBodyComponent& component) {
162
163 // Prepare to add new component (allocate memory if necessary and compute insertion index)
164 uint32 index = prepareAddComponent(isSleeping);
165
166 // Insert the new component data
167 new (mBodiesEntities + index) Entity(bodyEntity);
168 mRigidBodies[index] = component.body;
169 mIsAllowedToSleep[index] = true;
170 mIsSleeping[index] = false;
171 mSleepTimes[index] = decimal(0);
172 mBodyTypes[index] = component.bodyType;
173 new (mLinearVelocities + index) Vector3(0, 0, 0);
174 new (mAngularVelocities + index) Vector3(0, 0, 0);
175 new (mExternalForces + index) Vector3(0, 0, 0);
176 new (mExternalTorques + index) Vector3(0, 0, 0);
177 mLinearDampings[index] = decimal(0.0);
178 mAngularDampings[index] = decimal(0.0);
179 mMasses[index] = decimal(1.0);
180 mInverseMasses[index] = decimal(1.0);
181 new (mLocalInertiaTensors + index) Vector3(1.0, 1.0, 1.0);
182 new (mInverseInertiaTensorsLocal + index) Vector3(1.0, 1.0, 1.0);
183 new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0);
184 new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0);
185 new (mSplitLinearVelocities + index) Vector3(0, 0, 0);
186 new (mSplitAngularVelocities + index) Vector3(0, 0, 0);
187 new (mConstrainedPositions + index) Vector3(0, 0, 0);
188 new (mConstrainedOrientations + index) Quaternion(0, 0, 0, 1);
189 new (mCentersOfMassLocal + index) Vector3(0, 0, 0);
190 new (mCentersOfMassWorld + index) Vector3(component.worldPosition);
191 mIsGravityEnabled[index] = true;
192 mIsAlreadyInIsland[index] = false;
193 new (mJoints + index) List<Entity>(mMemoryAllocator);
194
195 // Map the entity with the new component lookup index
196 mMapEntityToComponentIndex.add(Pair<Entity, uint32>(bodyEntity, index));
197
198 mNbComponents++;
199
200 assert(mDisabledStartIndex <= mNbComponents);
201 assert(mNbComponents == static_cast<uint32>(mMapEntityToComponentIndex.size()));
202 }
203
204 // Move a component from a source to a destination index in the components array
205 // The destination location must contain a constructed object
moveComponentToIndex(uint32 srcIndex,uint32 destIndex)206 void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) {
207
208 const Entity entity = mBodiesEntities[srcIndex];
209
210 // Copy the data of the source component to the destination location
211 new (mBodiesEntities + destIndex) Entity(mBodiesEntities[srcIndex]);
212 mRigidBodies[destIndex] = mRigidBodies[srcIndex];
213 mIsAllowedToSleep[destIndex] = mIsAllowedToSleep[srcIndex];
214 mIsSleeping[destIndex] = mIsSleeping[srcIndex];
215 mSleepTimes[destIndex] = mSleepTimes[srcIndex];
216 mBodyTypes[destIndex] = mBodyTypes[srcIndex];
217 new (mLinearVelocities + destIndex) Vector3(mLinearVelocities[srcIndex]);
218 new (mAngularVelocities + destIndex) Vector3(mAngularVelocities[srcIndex]);
219 new (mExternalForces + destIndex) Vector3(mExternalForces[srcIndex]);
220 new (mExternalTorques + destIndex) Vector3(mExternalTorques[srcIndex]);
221 mLinearDampings[destIndex] = mLinearDampings[srcIndex];
222 mAngularDampings[destIndex] = mAngularDampings[srcIndex];
223 mMasses[destIndex] = mMasses[srcIndex];
224 mInverseMasses[destIndex] = mInverseMasses[srcIndex];
225 new (mLocalInertiaTensors + destIndex) Vector3(mLocalInertiaTensors[srcIndex]);
226 new (mInverseInertiaTensorsLocal + destIndex) Vector3(mInverseInertiaTensorsLocal[srcIndex]);
227 new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]);
228 new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]);
229 new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]);
230 new (mSplitAngularVelocities + destIndex) Vector3(mSplitAngularVelocities[srcIndex]);
231 new (mConstrainedPositions + destIndex) Vector3(mConstrainedPositions[srcIndex]);
232 new (mConstrainedOrientations + destIndex) Quaternion(mConstrainedOrientations[srcIndex]);
233 new (mCentersOfMassLocal + destIndex) Vector3(mCentersOfMassLocal[srcIndex]);
234 new (mCentersOfMassWorld + destIndex) Vector3(mCentersOfMassWorld[srcIndex]);
235 mIsGravityEnabled[destIndex] = mIsGravityEnabled[srcIndex];
236 mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex];
237 new (mJoints + destIndex) List<Entity>(mJoints[srcIndex]);
238
239 // Destroy the source component
240 destroyComponent(srcIndex);
241
242 assert(!mMapEntityToComponentIndex.containsKey(entity));
243
244 // Update the entity to component index mapping
245 mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity, destIndex));
246
247 assert(mMapEntityToComponentIndex[mBodiesEntities[destIndex]] == destIndex);
248 }
249
250 // Swap two components in the array
swapComponents(uint32 index1,uint32 index2)251 void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) {
252
253 // Copy component 1 data
254 Entity entity1(mBodiesEntities[index1]);
255 RigidBody* body1 = mRigidBodies[index1];
256 bool isAllowedToSleep1 = mIsAllowedToSleep[index1];
257 bool isSleeping1 = mIsSleeping[index1];
258 decimal sleepTime1 = mSleepTimes[index1];
259 BodyType bodyType1 = mBodyTypes[index1];
260 Vector3 linearVelocity1(mLinearVelocities[index1]);
261 Vector3 angularVelocity1(mAngularVelocities[index1]);
262 Vector3 externalForce1(mExternalForces[index1]);
263 Vector3 externalTorque1(mExternalTorques[index1]);
264 decimal linearDamping1 = mLinearDampings[index1];
265 decimal angularDamping1 = mAngularDampings[index1];
266 decimal mass1 = mMasses[index1];
267 decimal inverseMass1 = mInverseMasses[index1];
268 Vector3 inertiaTensorLocal1 = mLocalInertiaTensors[index1];
269 Vector3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1];
270 Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]);
271 Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]);
272 Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]);
273 Vector3 splitAngularVelocity1(mSplitAngularVelocities[index1]);
274 Vector3 constrainedPosition1 = mConstrainedPositions[index1];
275 Quaternion constrainedOrientation1 = mConstrainedOrientations[index1];
276 Vector3 centerOfMassLocal1 = mCentersOfMassLocal[index1];
277 Vector3 centerOfMassWorld1 = mCentersOfMassWorld[index1];
278 bool isGravityEnabled1 = mIsGravityEnabled[index1];
279 bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1];
280 List<Entity> joints1 = mJoints[index1];
281
282 // Destroy component 1
283 destroyComponent(index1);
284
285 moveComponentToIndex(index2, index1);
286
287 // Reconstruct component 1 at component 2 location
288 new (mBodiesEntities + index2) Entity(entity1);
289 mRigidBodies[index2] = body1;
290 mIsAllowedToSleep[index2] = isAllowedToSleep1;
291 mIsSleeping[index2] = isSleeping1;
292 mSleepTimes[index2] = sleepTime1;
293 mBodyTypes[index2] = bodyType1;
294 new (mLinearVelocities + index2) Vector3(linearVelocity1);
295 new (mAngularVelocities + index2) Vector3(angularVelocity1);
296 new (mExternalForces + index2) Vector3(externalForce1);
297 new (mExternalTorques + index2) Vector3(externalTorque1);
298 mLinearDampings[index2] = linearDamping1;
299 mAngularDampings[index2] = angularDamping1;
300 mMasses[index2] = mass1;
301 mInverseMasses[index2] = inverseMass1;
302 mLocalInertiaTensors[index2] = inertiaTensorLocal1;
303 mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1;
304 new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1);
305 new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1);
306 new (mSplitLinearVelocities + index2) Vector3(splitLinearVelocity1);
307 new (mSplitAngularVelocities + index2) Vector3(splitAngularVelocity1);
308 mConstrainedPositions[index2] = constrainedPosition1;
309 mConstrainedOrientations[index2] = constrainedOrientation1;
310 mCentersOfMassLocal[index2] = centerOfMassLocal1;
311 mCentersOfMassWorld[index2] = centerOfMassWorld1;
312 mIsGravityEnabled[index2] = isGravityEnabled1;
313 mIsAlreadyInIsland[index2] = isAlreadyInIsland1;
314 new (mJoints + index2) List<Entity>(joints1);
315
316 // Update the entity to component index mapping
317 mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity1, index2));
318
319 assert(mMapEntityToComponentIndex[mBodiesEntities[index1]] == index1);
320 assert(mMapEntityToComponentIndex[mBodiesEntities[index2]] == index2);
321 assert(mNbComponents == static_cast<uint32>(mMapEntityToComponentIndex.size()));
322 }
323
324 // Destroy a component at a given index
destroyComponent(uint32 index)325 void RigidBodyComponents::destroyComponent(uint32 index) {
326
327 Components::destroyComponent(index);
328
329 assert(mMapEntityToComponentIndex[mBodiesEntities[index]] == index);
330
331 mMapEntityToComponentIndex.remove(mBodiesEntities[index]);
332
333 mBodiesEntities[index].~Entity();
334 mRigidBodies[index] = nullptr;
335 mLinearVelocities[index].~Vector3();
336 mAngularVelocities[index].~Vector3();
337 mExternalForces[index].~Vector3();
338 mExternalTorques[index].~Vector3();
339 mLocalInertiaTensors[index].~Vector3();
340 mInverseInertiaTensorsLocal[index].~Vector3();
341 mConstrainedLinearVelocities[index].~Vector3();
342 mConstrainedAngularVelocities[index].~Vector3();
343 mSplitLinearVelocities[index].~Vector3();
344 mSplitAngularVelocities[index].~Vector3();
345 mConstrainedPositions[index].~Vector3();
346 mConstrainedOrientations[index].~Quaternion();
347 mCentersOfMassLocal[index].~Vector3();
348 mCentersOfMassWorld[index].~Vector3();
349 mJoints[index].~List<Entity>();
350 }
351