/******************************************************************************** * ReactPhysics3D physics library, http://www.reactphysics3d.com * * Copyright (c) 2010-2018 Daniel Chappuis * ********************************************************************************* * * * This software is provided 'as-is', without any express or implied warranty. * * In no event will the authors be held liable for any damages arising from the * * use of this software. * * * * Permission is granted to anyone to use this software for any purpose, * * including commercial applications, and to alter it and redistribute it * * freely, subject to the following restrictions: * * * * 1. The origin of this software must not be misrepresented; you must not claim * * that you wrote the original software. If you use this software in a * * product, an acknowledgment in the product documentation would be * * appreciated but is not required. * * * * 2. Altered source versions must be plainly marked as such, and must not be * * misrepresented as being the original software. * * * * 3. This notice may not be removed or altered from any source distribution. * * * ********************************************************************************/ // Libraries #include "SliderJointComponents.h" #include "engine/EntityManager.h" #include "mathematics/Matrix3x3.h" #include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; // Constructor SliderJointComponents::SliderJointComponents(MemoryAllocator& allocator) :Components(allocator, sizeof(Entity) + sizeof(SliderJoint*) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector2) + sizeof(Vector3) + sizeof(Matrix2x2) + sizeof(Matrix3x3) + sizeof(Vector2) + sizeof(Vector3) + sizeof(Quaternion)/* + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(decimal) + sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(decimal)*/) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); } // Allocate memory for a given number of components void SliderJointComponents::allocate(uint32 nbComponentsToAllocate) { assert(nbComponentsToAllocate > mNbAllocatedComponents); // Size for the data of a single component (in bytes) const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize; // Allocate memory void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes); assert(newBuffer != nullptr); // New pointers to components data Entity* newJointEntities = static_cast(newBuffer); SliderJoint** newJoints = reinterpret_cast(newJointEntities + nbComponentsToAllocate); Vector3* newLocalAnchorPointBody1 = reinterpret_cast(newJoints + nbComponentsToAllocate); Vector3* newLocalAnchorPointBody2 = reinterpret_cast(newLocalAnchorPointBody1 + nbComponentsToAllocate); Matrix3x3* newI1 = reinterpret_cast(newLocalAnchorPointBody2 + nbComponentsToAllocate); Matrix3x3* newI2 = reinterpret_cast(newI1 + nbComponentsToAllocate); Vector2* newImpulseTranslation = reinterpret_cast(newI2 + nbComponentsToAllocate); Vector3* newImpulseRotation = reinterpret_cast(newImpulseTranslation + nbComponentsToAllocate); Matrix2x2* newInverseMassMatrixTranslation = reinterpret_cast(newImpulseRotation + nbComponentsToAllocate); Matrix3x3* newInverseMassMatrixRotation = reinterpret_cast(newInverseMassMatrixTranslation + nbComponentsToAllocate); Vector2* newBiasTranslation = reinterpret_cast(newInverseMassMatrixRotation + nbComponentsToAllocate); Vector3* newBiasRotation = reinterpret_cast(newBiasTranslation + nbComponentsToAllocate); Quaternion* newInitOrientationDifferenceInv = reinterpret_cast(newBiasRotation + nbComponentsToAllocate); /* Vector3* newHingeLocalAxisBody1 = reinterpret_cast(newInitOrientationDifferenceInv + nbComponentsToAllocate); Vector3* newHingeLocalAxisBody2 = reinterpret_cast(newHingeLocalAxisBody1 + nbComponentsToAllocate); Vector3* newA1 = reinterpret_cast(newHingeLocalAxisBody2 + nbComponentsToAllocate); Vector3* newB2CrossA1 = reinterpret_cast(newA1 + nbComponentsToAllocate); Vector3* newC2CrossA1 = reinterpret_cast(newB2CrossA1 + nbComponentsToAllocate); decimal* newImpulseLowerLimit = reinterpret_cast(newC2CrossA1 + nbComponentsToAllocate); decimal* newImpulseUpperLimit = reinterpret_cast(newImpulseLowerLimit + nbComponentsToAllocate); decimal* newImpulseMotor = reinterpret_cast(newImpulseUpperLimit + nbComponentsToAllocate); decimal* newInverseMassMatrixLimitMotor = reinterpret_cast(newImpulseMotor + nbComponentsToAllocate); decimal* newInverseMassMatrixMotor = reinterpret_cast(newInverseMassMatrixLimitMotor + nbComponentsToAllocate); decimal* newBLowerLimit = reinterpret_cast(newInverseMassMatrixMotor + nbComponentsToAllocate); decimal* newBUpperLimit = reinterpret_cast(newBLowerLimit + nbComponentsToAllocate); bool* newIsLimitEnabled = reinterpret_cast(newBUpperLimit + nbComponentsToAllocate); bool* newIsMotorEnabled = reinterpret_cast(newIsLimitEnabled + nbComponentsToAllocate); decimal* newLowerLimit = reinterpret_cast(newIsMotorEnabled + nbComponentsToAllocate); decimal* newUpperLimit = reinterpret_cast(newLowerLimit + nbComponentsToAllocate); bool* newIsLowerLimitViolated = reinterpret_cast(newUpperLimit + nbComponentsToAllocate); bool* newIsUpperLimitViolated = reinterpret_cast(newIsLowerLimitViolated + nbComponentsToAllocate); decimal* newMotorSpeed = reinterpret_cast(newIsUpperLimitViolated + nbComponentsToAllocate); decimal* newMaxMotorTorque = reinterpret_cast(newMotorSpeed + nbComponentsToAllocate); */ // If there was already components before if (mNbComponents > 0) { // Copy component data from the previous buffer to the new one memcpy(newJointEntities, mJointEntities, mNbComponents * sizeof(Entity)); memcpy(newJoints, mJoints, mNbComponents * sizeof(SliderJoint*)); memcpy(newLocalAnchorPointBody1, mLocalAnchorPointBody1, mNbComponents * sizeof(Vector3)); memcpy(newLocalAnchorPointBody2, mLocalAnchorPointBody2, mNbComponents * sizeof(Vector3)); memcpy(newI1, mI1, mNbComponents * sizeof(Matrix3x3)); memcpy(newI2, mI2, mNbComponents * sizeof(Matrix3x3)); memcpy(newImpulseTranslation, mImpulseTranslation, mNbComponents * sizeof(Vector2)); memcpy(newImpulseRotation, mImpulseRotation, mNbComponents * sizeof(Vector3)); memcpy(newInverseMassMatrixTranslation, mInverseMassMatrixTranslation, mNbComponents * sizeof(Matrix2x2)); memcpy(newInverseMassMatrixRotation, mInverseMassMatrixRotation, mNbComponents * sizeof(Matrix3x3)); memcpy(newBiasTranslation, mBiasTranslation, mNbComponents * sizeof(Vector2)); memcpy(newBiasRotation, mBiasRotation, mNbComponents * sizeof(Vector3)); memcpy(newInitOrientationDifferenceInv, mInitOrientationDifferenceInv, mNbComponents * sizeof(Quaternion)); /* memcpy(newHingeLocalAxisBody1, mHingeLocalAxisBody1, mNbComponents * sizeof(Vector3)); memcpy(newHingeLocalAxisBody2, mHingeLocalAxisBody2, mNbComponents * sizeof(Vector3)); memcpy(newA1, mA1, mNbComponents * sizeof(Vector3)); memcpy(newB2CrossA1, mB2CrossA1, mNbComponents * sizeof(Vector3)); memcpy(newC2CrossA1, mC2CrossA1, mNbComponents * sizeof(Vector3)); memcpy(newImpulseLowerLimit, mImpulseLowerLimit, mNbComponents * sizeof(decimal)); memcpy(newImpulseUpperLimit, mImpulseUpperLimit, mNbComponents * sizeof(decimal)); memcpy(newImpulseMotor, mImpulseMotor, mNbComponents * sizeof(decimal)); memcpy(newInverseMassMatrixLimitMotor, mInverseMassMatrixLimitMotor, mNbComponents * sizeof(decimal)); memcpy(newInverseMassMatrixMotor, mInverseMassMatrixMotor, mNbComponents * sizeof(decimal)); memcpy(newBLowerLimit, mBLowerLimit, mNbComponents * sizeof(decimal)); memcpy(newBUpperLimit, mBUpperLimit, mNbComponents * sizeof(decimal)); memcpy(newIsLimitEnabled, mIsLimitEnabled, mNbComponents * sizeof(bool)); memcpy(newIsMotorEnabled, mIsMotorEnabled, mNbComponents * sizeof(bool)); memcpy(newLowerLimit, mLowerLimit, mNbComponents * sizeof(decimal)); memcpy(newUpperLimit, mUpperLimit, mNbComponents * sizeof(decimal)); memcpy(newIsLowerLimitViolated, mIsLowerLimitViolated, mNbComponents * sizeof(bool)); memcpy(newIsUpperLimitViolated, mIsUpperLimitViolated, mNbComponents * sizeof(bool)); memcpy(newMotorSpeed, mMotorSpeed, mNbComponents * sizeof(decimal)); memcpy(newMaxMotorTorque, mMaxMotorTorque, mNbComponents * sizeof(decimal)); */ // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); } mBuffer = newBuffer; mJointEntities = newJointEntities; mJoints = newJoints; mNbAllocatedComponents = nbComponentsToAllocate; mLocalAnchorPointBody1 = newLocalAnchorPointBody1; mLocalAnchorPointBody2 = newLocalAnchorPointBody2; mI1 = newI1; mI2 = newI2; mImpulseTranslation = newImpulseTranslation; mImpulseRotation = newImpulseRotation; mInverseMassMatrixTranslation = newInverseMassMatrixTranslation; mInverseMassMatrixRotation = newInverseMassMatrixRotation; mBiasTranslation = newBiasTranslation; mBiasRotation = newBiasRotation; mInitOrientationDifferenceInv = newInitOrientationDifferenceInv; /* mHingeLocalAxisBody1 = newHingeLocalAxisBody1; mHingeLocalAxisBody2 = newHingeLocalAxisBody2; mA1 = newA1; mB2CrossA1 = newB2CrossA1; mC2CrossA1 = newC2CrossA1; mImpulseLowerLimit = newImpulseLowerLimit; mImpulseUpperLimit = newImpulseUpperLimit; mImpulseMotor = newImpulseMotor; mInverseMassMatrixLimitMotor = newInverseMassMatrixLimitMotor; mInverseMassMatrixMotor = newInverseMassMatrixMotor; mBLowerLimit = newBLowerLimit; mBUpperLimit = newBUpperLimit; mIsLimitEnabled = newIsLimitEnabled; mIsMotorEnabled = newIsMotorEnabled; mLowerLimit = newLowerLimit; mUpperLimit = newUpperLimit; mIsLowerLimitViolated = newIsLowerLimitViolated; mIsUpperLimitViolated = newIsUpperLimitViolated; mMotorSpeed = newMotorSpeed; mMaxMotorTorque = newMaxMotorTorque; */ } // Add a component void SliderJointComponents::addComponent(Entity jointEntity, bool isSleeping, const SliderJointComponent& component) { // Prepare to add new component (allocate memory if necessary and compute insertion index) uint32 index = prepareAddComponent(isSleeping); // Insert the new component data new (mJointEntities + index) Entity(jointEntity); mJoints[index] = nullptr; new (mLocalAnchorPointBody1 + index) Vector3(0, 0, 0); new (mLocalAnchorPointBody2 + index) Vector3(0, 0, 0); new (mI1 + index) Matrix3x3(); new (mI2 + index) Matrix3x3(); new (mImpulseTranslation + index) Vector2(0, 0); new (mImpulseRotation + index) Vector3(0, 0, 0); new (mInverseMassMatrixTranslation + index) Matrix2x2(); new (mInverseMassMatrixRotation + index) Matrix3x3(); new (mBiasTranslation + index) Vector2(0, 0); new (mBiasRotation + index) Vector3(0, 0, 0); new (mInitOrientationDifferenceInv + index) Quaternion(0, 0, 0, 0); /* new (mHingeLocalAxisBody1 + index) Vector3(0, 0, 0); new (mHingeLocalAxisBody2 + index) Vector3(0, 0, 0); new (mA1 + index) Vector3(0, 0, 0); new (mB2CrossA1 + index) Vector3(0, 0, 0); new (mC2CrossA1 + index) Vector3(0, 0, 0); mImpulseLowerLimit[index] = decimal(0.0); mImpulseUpperLimit[index] = decimal(0.0); mInverseMassMatrixLimitMotor[index] = decimal(0.0); mInverseMassMatrixMotor[index] = decimal(0.0); mBLowerLimit[index] = decimal(0.0); mBUpperLimit[index] = decimal(0.0); mIsLimitEnabled[index] = component.isLimitEnabled; mIsMotorEnabled[index] = component.isMotorEnabled; mLowerLimit[index] = component.lowerLimit; mUpperLimit[index] = component.upperLimit; mIsLowerLimitViolated[index] = false; mIsUpperLimitViolated[index] = false; mMotorSpeed[index] = component.motorSpeed; mMaxMotorTorque[index] = component.maxMotorTorque; */ // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(jointEntity, index)); mNbComponents++; assert(mDisabledStartIndex <= mNbComponents); assert(mNbComponents == static_cast(mMapEntityToComponentIndex.size())); } // Move a component from a source to a destination index in the components array // The destination location must contain a constructed object void SliderJointComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) { const Entity entity = mJointEntities[srcIndex]; // Copy the data of the source component to the destination location new (mJointEntities + destIndex) Entity(mJointEntities[srcIndex]); mJoints[destIndex] = mJoints[srcIndex]; new (mLocalAnchorPointBody1 + destIndex) Vector3(mLocalAnchorPointBody1[srcIndex]); new (mLocalAnchorPointBody2 + destIndex) Vector3(mLocalAnchorPointBody2[srcIndex]); new (mI1 + destIndex) Matrix3x3(mI1[srcIndex]); new (mI2 + destIndex) Matrix3x3(mI2[srcIndex]); new (mImpulseTranslation + destIndex) Vector2(mImpulseTranslation[srcIndex]); new (mImpulseRotation + destIndex) Vector3(mImpulseRotation[srcIndex]); new (mInverseMassMatrixTranslation + destIndex) Matrix2x2(mInverseMassMatrixTranslation[srcIndex]); new (mInverseMassMatrixRotation + destIndex) Matrix3x3(mInverseMassMatrixRotation[srcIndex]); new (mBiasTranslation + destIndex) Vector2(mBiasTranslation[srcIndex]); new (mBiasRotation + destIndex) Vector3(mBiasRotation[srcIndex]); new (mInitOrientationDifferenceInv + destIndex) Quaternion(mInitOrientationDifferenceInv[srcIndex]); /* new (mHingeLocalAxisBody1 + destIndex) Vector3(mHingeLocalAxisBody1[srcIndex]); new (mHingeLocalAxisBody2 + destIndex) Vector3(mHingeLocalAxisBody2[srcIndex]); new (mA1 + destIndex) Vector3(mA1[srcIndex]); new (mB2CrossA1 + destIndex) Vector3(mB2CrossA1[srcIndex]); new (mC2CrossA1 + destIndex) Vector3(mC2CrossA1[srcIndex]); mImpulseLowerLimit[destIndex] = mImpulseLowerLimit[srcIndex]; mImpulseUpperLimit[destIndex] = mImpulseUpperLimit[srcIndex]; mImpulseMotor[destIndex] = mImpulseMotor[srcIndex]; mInverseMassMatrixLimitMotor[destIndex] = mInverseMassMatrixLimitMotor[srcIndex]; mInverseMassMatrixMotor[destIndex] = mInverseMassMatrixMotor[srcIndex]; mBLowerLimit[destIndex] = mBLowerLimit[srcIndex]; mBUpperLimit[destIndex] = mBUpperLimit[srcIndex]; mIsLimitEnabled[destIndex] = mIsLimitEnabled[srcIndex]; mIsMotorEnabled[destIndex] = mIsMotorEnabled[srcIndex]; mLowerLimit[destIndex] = mLowerLimit[srcIndex]; mUpperLimit[destIndex] = mUpperLimit[srcIndex]; mIsLowerLimitViolated[destIndex] = mIsLowerLimitViolated[srcIndex]; mIsUpperLimitViolated[destIndex] = mIsUpperLimitViolated[srcIndex]; mMotorSpeed[destIndex] = mMotorSpeed[srcIndex]; mMaxMotorTorque[destIndex] = mMaxMotorTorque[srcIndex]; */ // Destroy the source component destroyComponent(srcIndex); assert(!mMapEntityToComponentIndex.containsKey(entity)); // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(entity, destIndex)); assert(mMapEntityToComponentIndex[mJointEntities[destIndex]] == destIndex); } // Swap two components in the array void SliderJointComponents::swapComponents(uint32 index1, uint32 index2) { // Copy component 1 data Entity jointEntity1(mJointEntities[index1]); SliderJoint* joint1 = mJoints[index1]; Vector3 localAnchorPointBody1(mLocalAnchorPointBody1[index1]); Vector3 localAnchorPointBody2(mLocalAnchorPointBody2[index1]); Matrix3x3 i11(mI1[index1]); Matrix3x3 i21(mI2[index1]); Vector2 impulseTranslation1(mImpulseTranslation[index1]); Vector3 impulseRotation1(mImpulseRotation[index1]); Matrix2x2 inverseMassMatrixTranslation1(mInverseMassMatrixTranslation[index1]); Matrix3x3 inverseMassMatrixRotation1(mInverseMassMatrixRotation[index1]); Vector2 biasTranslation1(mBiasTranslation[index1]); Vector3 biasRotation1(mBiasRotation[index1]); Quaternion initOrientationDifferenceInv1(mInitOrientationDifferenceInv[index1]); /* Vector3 hingeLocalAxisBody1(mHingeLocalAxisBody1[index1]); Vector3 hingeLocalAxisBody2(mHingeLocalAxisBody2[index1]); Vector3 a1(mA1[index1]); Vector3 b2CrossA1(mB2CrossA1[index1]); Vector3 c2CrossA1(mC2CrossA1[index1]); decimal impulseLowerLimit(mImpulseLowerLimit[index1]); decimal impulseUpperLimit(mImpulseUpperLimit[index1]); decimal impulseMotor(mImpulseMotor[index1]); decimal inverseMassMatrixLimitMotor(mInverseMassMatrixLimitMotor[index1]); decimal inverseMassMatrixMotor(mInverseMassMatrixMotor[index1]); decimal bLowerLimit(mBLowerLimit[index1]); decimal bUpperLimit(mUpperLimit[index1]); bool isLimitEnabled(mIsLimitEnabled[index1]); bool isMotorEnabled(mIsMotorEnabled[index1]); decimal lowerLimit(mLowerLimit[index1]); decimal upperLimit(mUpperLimit[index1]); bool isLowerLimitViolated(mIsLowerLimitViolated[index1]); bool isUpperLimitViolated(mIsUpperLimitViolated[index1]); decimal motorSpeed(mMotorSpeed[index1]); decimal maxMotorTorque(mMaxMotorTorque[index1]); */ // Destroy component 1 destroyComponent(index1); moveComponentToIndex(index2, index1); // Reconstruct component 1 at component 2 location new (mJointEntities + index2) Entity(jointEntity1); mJoints[index2] = joint1; new (mLocalAnchorPointBody1 + index2) Vector3(localAnchorPointBody1); new (mLocalAnchorPointBody2 + index2) Vector3(localAnchorPointBody2); new (mI1 + index2) Matrix3x3(i11); new (mI2 + index2) Matrix3x3(i21); new (mImpulseTranslation + index2) Vector2(impulseTranslation1); new (mImpulseRotation + index2) Vector3(impulseRotation1); new (mInverseMassMatrixTranslation + index2) Matrix2x2(inverseMassMatrixTranslation1); new (mInverseMassMatrixRotation + index2) Matrix3x3(inverseMassMatrixRotation1); new (mBiasTranslation + index2) Vector2(biasTranslation1); new (mBiasRotation + index2) Vector3(biasRotation1); new (mInitOrientationDifferenceInv + index2) Quaternion(initOrientationDifferenceInv1); /* new (mHingeLocalAxisBody1 + index2) Vector3(hingeLocalAxisBody1); new (mHingeLocalAxisBody2 + index2) Vector3(hingeLocalAxisBody2); new (mA1 + index2) Vector3(a1); new (mB2CrossA1 + index2) Vector3(b2CrossA1); new (mC2CrossA1 + index2) Vector3(c2CrossA1); mImpulseLowerLimit[index2] = impulseLowerLimit; mImpulseUpperLimit[index2] = impulseUpperLimit; mImpulseMotor[index2] = impulseMotor; mInverseMassMatrixLimitMotor[index2] = inverseMassMatrixLimitMotor; mInverseMassMatrixMotor[index2] = inverseMassMatrixMotor; mBLowerLimit[index2] = bLowerLimit; mBUpperLimit[index2] = bUpperLimit; mIsLimitEnabled[index2] = isLimitEnabled; mIsMotorEnabled[index2] = isMotorEnabled; mLowerLimit[index2] = lowerLimit; mUpperLimit[index2] = upperLimit; mIsLowerLimitViolated[index2] = isLowerLimitViolated; mIsUpperLimitViolated[index2] = isUpperLimitViolated; mMotorSpeed[index2] = motorSpeed; mMaxMotorTorque[index2] = maxMotorTorque; */ // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(jointEntity1, index2)); assert(mMapEntityToComponentIndex[mJointEntities[index1]] == index1); assert(mMapEntityToComponentIndex[mJointEntities[index2]] == index2); assert(mNbComponents == static_cast(mMapEntityToComponentIndex.size())); } // Destroy a component at a given index void SliderJointComponents::destroyComponent(uint32 index) { Components::destroyComponent(index); assert(mMapEntityToComponentIndex[mJointEntities[index]] == index); mMapEntityToComponentIndex.remove(mJointEntities[index]); mJointEntities[index].~Entity(); mJoints[index] = nullptr; mLocalAnchorPointBody1[index].~Vector3(); mLocalAnchorPointBody2[index].~Vector3(); mI1[index].~Matrix3x3(); mI2[index].~Matrix3x3(); mImpulseTranslation[index].~Vector2(); mImpulseRotation[index].~Vector3(); mInverseMassMatrixTranslation[index].~Matrix2x2(); mInverseMassMatrixRotation[index].~Matrix3x3(); mBiasTranslation[index].~Vector2(); mBiasRotation[index].~Vector3(); mInitOrientationDifferenceInv[index].~Quaternion(); /* mHingeLocalAxisBody1[index].~Vector3(); mHingeLocalAxisBody2[index].~Vector3(); mA1[index].~Vector3(); mB2CrossA1[index].~Vector3(); mC2CrossA1[index].~Vector3(); */ }