Swap two components in the array
| 201 | |
| 202 | // Swap two components in the array |
| 203 | void FixedJointComponents::swapComponents(uint32 index1, uint32 index2) { |
| 204 | |
| 205 | // Copy component 1 data |
| 206 | Entity jointEntity1(mJointEntities[index1]); |
| 207 | FixedJoint* joint1 = mJoints[index1]; |
| 208 | Vector3 localAnchorPointBody1(mLocalAnchorPointBody1[index1]); |
| 209 | Vector3 localAnchorPointBody2(mLocalAnchorPointBody2[index1]); |
| 210 | Vector3 r1World1(mR1World[index1]); |
| 211 | Vector3 r2World1(mR2World[index1]); |
| 212 | Matrix3x3 i11(mI1[index1]); |
| 213 | Matrix3x3 i21(mI2[index1]); |
| 214 | Vector3 impulseTranslation1(mImpulseTranslation[index1]); |
| 215 | Vector3 impulseRotation1(mImpulseRotation[index1]); |
| 216 | Matrix3x3 inverseMassMatrixTranslation1(mInverseMassMatrixTranslation[index1]); |
| 217 | Matrix3x3 inverseMassMatrixRotation1(mInverseMassMatrixRotation[index1]); |
| 218 | Vector3 biasTranslation1(mBiasTranslation[index1]); |
| 219 | Vector3 biasRotation1(mBiasRotation[index1]); |
| 220 | Quaternion initOrientationDifferenceInv1(mInitOrientationDifferenceInv[index1]); |
| 221 | |
| 222 | // Destroy component 1 |
| 223 | destroyComponent(index1); |
| 224 | |
| 225 | moveComponentToIndex(index2, index1); |
| 226 | |
| 227 | // Reconstruct component 1 at component 2 location |
| 228 | new (mJointEntities + index2) Entity(jointEntity1); |
| 229 | mJoints[index2] = joint1; |
| 230 | new (mLocalAnchorPointBody1 + index2) Vector3(localAnchorPointBody1); |
| 231 | new (mLocalAnchorPointBody2 + index2) Vector3(localAnchorPointBody2); |
| 232 | new (mR1World + index2) Vector3(r1World1); |
| 233 | new (mR2World + index2) Vector3(r2World1); |
| 234 | new (mI1 + index2) Matrix3x3(i11); |
| 235 | new (mI2 + index2) Matrix3x3(i21); |
| 236 | new (mImpulseTranslation + index2) Vector3(impulseTranslation1); |
| 237 | new (mImpulseRotation + index2) Vector3(impulseRotation1); |
| 238 | new (mInverseMassMatrixTranslation + index2) Matrix3x3(inverseMassMatrixTranslation1); |
| 239 | new (mInverseMassMatrixRotation + index2) Matrix3x3(inverseMassMatrixRotation1); |
| 240 | new (mBiasTranslation + index2) Vector3(biasTranslation1); |
| 241 | new (mBiasRotation + index2) Vector3(biasRotation1); |
| 242 | new (mInitOrientationDifferenceInv + index2) Quaternion(initOrientationDifferenceInv1); |
| 243 | |
| 244 | // Update the entity to component index mapping |
| 245 | mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity1, index2)); |
| 246 | |
| 247 | assert(mMapEntityToComponentIndex[mJointEntities[index1]] == index1); |
| 248 | assert(mMapEntityToComponentIndex[mJointEntities[index2]] == index2); |
| 249 | assert(mNbComponents == static_cast<uint32>(mMapEntityToComponentIndex.size())); |
| 250 | } |
| 251 | |
| 252 | // Destroy a component at a given index |
| 253 | void FixedJointComponents::destroyComponent(uint32 index) { |