diff --git a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp index 8ca69622a..ddfd70d0a 100644 --- a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp @@ -33,6 +33,230 @@ void btMultiBodyWorldImporter::deleteAllData() } +static btCollisionObjectDoubleData* getBody0FromContactManifold(btPersistentManifoldDoubleData* manifold) +{ + return (btCollisionObjectDoubleData*)manifold->m_body0; +} +static btCollisionObjectDoubleData* getBody1FromContactManifold(btPersistentManifoldDoubleData* manifold) +{ + return (btCollisionObjectDoubleData*)manifold->m_body1; +} +static btCollisionObjectFloatData* getBody0FromContactManifold(btPersistentManifoldFloatData* manifold) +{ + return (btCollisionObjectFloatData*)manifold->m_body0; +} +static btCollisionObjectFloatData* getBody1FromContactManifold(btPersistentManifoldFloatData* manifold) +{ + return (btCollisionObjectFloatData*)manifold->m_body1; +} + + +template void syncContactManifolds(T** contactManifolds, int numContactManifolds, btMultiBodyWorldImporterInternalData* m_data) +{ + m_data->m_mbDynamicsWorld->updateAabbs(); + m_data->m_mbDynamicsWorld->computeOverlappingPairs(); + btDispatcher* dispatcher = m_data->m_mbDynamicsWorld->getDispatcher(); + + btDispatcherInfo& dispatchInfo = m_data->m_mbDynamicsWorld->getDispatchInfo(); + + + if (dispatcher) + { + btOverlappingPairCache* pairCache = m_data->m_mbDynamicsWorld->getBroadphase()->getOverlappingPairCache(); + if (dispatcher) + { + dispatcher->dispatchAllCollisionPairs(pairCache, dispatchInfo, dispatcher); + } + int numExistingManifolds = m_data->m_mbDynamicsWorld->getDispatcher()->getNumManifolds(); + btManifoldArray manifoldArray; + for (int i = 0; i < pairCache->getNumOverlappingPairs(); i++) + { + btBroadphasePair& pair = pairCache->getOverlappingPairArray()[i]; + if (pair.m_algorithm) + { + pair.m_algorithm->getAllContactManifolds(manifoldArray); + //for each existing manifold, search a matching manifoldData and reconstruct + for (int m = 0; m < manifoldArray.size(); m++) + { + btPersistentManifold* existingManifold = manifoldArray[m]; + int uid0 = existingManifold->getBody0()->getBroadphaseHandle()->m_uniqueId; + int uid1 = existingManifold->getBody1()->getBroadphaseHandle()->m_uniqueId; + int matchingManifoldIndex = -1; + for (int q = 0; q < numContactManifolds; q++) + { + if (uid0 == getBody0FromContactManifold(contactManifolds[q])->m_uniqueId && uid1 == getBody1FromContactManifold(contactManifolds[q])->m_uniqueId) + { + matchingManifoldIndex = q; + } + } + if (matchingManifoldIndex >= 0) + { + existingManifold->deSerialize(contactManifolds[matchingManifoldIndex]); + } + else + { + existingManifold->setNumContacts(0); + //printf("Issue: cannot find maching contact manifold (%d, %d), may cause issues in determinism.\n", uid0, uid1); + } + + manifoldArray.clear(); + } + } + } + } + +} + +template void syncMultiBody(T* mbd, btMultiBody* mb, btMultiBodyWorldImporterInternalData* m_data, btAlignedObjectArray& scratchQ, btAlignedObjectArray& scratchM) +{ + bool isFixedBase = mbd->m_baseMass == 0; + bool canSleep = false; + btVector3 baseInertia; + baseInertia.deSerialize(mbd->m_baseInertia); + + btVector3 baseWorldPos; + baseWorldPos.deSerialize(mbd->m_baseWorldPosition); + mb->setBasePos(baseWorldPos); + btQuaternion baseWorldRot; + baseWorldRot.deSerialize(mbd->m_baseWorldOrientation); + mb->setWorldToBaseRot(baseWorldRot.inverse()); + btVector3 baseLinVal; + baseLinVal.deSerialize(mbd->m_baseLinearVelocity); + btVector3 baseAngVel; + baseAngVel.deSerialize(mbd->m_baseAngularVelocity); + mb->setBaseVel(baseLinVal); + mb->setBaseOmega(baseAngVel); + + for (int i = 0; i < mbd->m_numLinks; i++) + { + switch (mbd->m_links[i].m_jointType) + { + case btMultibodyLink::eFixed: + { + break; + } + case btMultibodyLink::ePrismatic: + { + mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]); + mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]); + break; + } + case btMultibodyLink::eRevolute: + { + mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]); + mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]); + break; + } + case btMultibodyLink::eSpherical: + { + btScalar jointPos[3] = { (btScalar)mbd->m_links[i].m_jointPos[0], (btScalar)mbd->m_links[i].m_jointPos[1], (btScalar)mbd->m_links[i].m_jointPos[2] }; + btScalar jointVel[3] = { (btScalar)mbd->m_links[i].m_jointVel[0], (btScalar)mbd->m_links[i].m_jointVel[1], (btScalar)mbd->m_links[i].m_jointVel[2] }; + mb->setJointPosMultiDof(i, jointPos); + mb->setJointVelMultiDof(i, jointVel); + + break; + } + case btMultibodyLink::ePlanar: + { + break; + } + default: + { + } + } + } + mb->forwardKinematics(scratchQ, scratchM); + mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM); +} + +template void convertMultiBody(T* mbd, btMultiBodyWorldImporterInternalData* m_data) +{ + bool isFixedBase = mbd->m_baseMass == 0; + bool canSleep = false; + btVector3 baseInertia; + baseInertia.deSerialize(mbd->m_baseInertia); + btMultiBody* mb = new btMultiBody(mbd->m_numLinks, mbd->m_baseMass, baseInertia, isFixedBase, canSleep); + mb->setHasSelfCollision(false); + + btVector3 baseWorldPos; + baseWorldPos.deSerialize(mbd->m_baseWorldPosition); + + btQuaternion baseWorldOrn; + baseWorldOrn.deSerialize(mbd->m_baseWorldOrientation); + mb->setBasePos(baseWorldPos); + mb->setWorldToBaseRot(baseWorldOrn.inverse()); + + m_data->m_mbMap.insert(mbd, mb); + for (int i = 0; i < mbd->m_numLinks; i++) + { + btVector3 localInertiaDiagonal; + localInertiaDiagonal.deSerialize(mbd->m_links[i].m_linkInertia); + btQuaternion parentRotToThis; + parentRotToThis.deSerialize(mbd->m_links[i].m_zeroRotParentToThis); + btVector3 parentComToThisPivotOffset; + parentComToThisPivotOffset.deSerialize(mbd->m_links[i].m_parentComToThisComOffset); + btVector3 thisPivotToThisComOffset; + thisPivotToThisComOffset.deSerialize(mbd->m_links[i].m_thisPivotToThisComOffset); + + switch (mbd->m_links[i].m_jointType) + { + case btMultibodyLink::eFixed: + { + + + mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex, + parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset); + //search for the collider + //mbd->m_links[i].m_linkCollider + break; + } + case btMultibodyLink::ePrismatic: + { + btVector3 jointAxis; + jointAxis.deSerialize(mbd->m_links[i].m_jointAxisBottom[0]); + bool disableParentCollision = true;//todo + mb->setupPrismatic(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex, + parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision); + mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]); + mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]); + break; + } + case btMultibodyLink::eRevolute: + { + btVector3 jointAxis; + jointAxis.deSerialize(mbd->m_links[i].m_jointAxisTop[0]); + bool disableParentCollision = true;//todo + mb->setupRevolute(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex, + parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision); + mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]); + mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]); + break; + } + case btMultibodyLink::eSpherical: + { + btAssert(0); + bool disableParentCollision = true;//todo + mb->setupSpherical(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex, + parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision); + btScalar jointPos[3] = { (btScalar)mbd->m_links[i].m_jointPos[0], (btScalar)mbd->m_links[i].m_jointPos[1], (btScalar)mbd->m_links[i].m_jointPos[2] }; + btScalar jointVel[3] = { (btScalar)mbd->m_links[i].m_jointVel[0], (btScalar)mbd->m_links[i].m_jointVel[1], (btScalar)mbd->m_links[i].m_jointVel[2] }; + mb->setJointPosMultiDof(i, jointPos); + mb->setJointVelMultiDof(i, jointVel); + + break; + } + case btMultibodyLink::ePlanar: + { + btAssert(0); + break; + } + default: + { + btAssert(0); + } + } + } +} bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile2) { @@ -56,138 +280,77 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF { //for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++) - for (int i = bulletFile2->m_multiBodies.size()-1; i >=0; i--) + for (int i = bulletFile2->m_multiBodies.size() - 1; i >= 0; i--) { btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i]; - bool isFixedBase = mbd->m_baseMass == 0; - bool canSleep = false; - btVector3 baseInertia; - baseInertia.deSerializeDouble(mbd->m_baseInertia); - btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i); - btVector3 baseWorldPos; - baseWorldPos.deSerializeDouble(mbd->m_baseWorldPosition); - mb->setBasePos(baseWorldPos); - btQuaternion baseWorldRot; - baseWorldRot.deSerializeDouble(mbd->m_baseWorldOrientation); - mb->setWorldToBaseRot(baseWorldRot.inverse()); - btVector3 baseLinVal; - baseLinVal.deSerializeDouble(mbd->m_baseLinearVelocity); - btVector3 baseAngVel; - baseAngVel.deSerializeDouble(mbd->m_baseAngularVelocity); - mb->setBaseVel(baseLinVal); - mb->setBaseOmega(baseAngVel); - - for (int i = 0; i < mbd->m_numLinks; i++) - { - switch (mbd->m_links[i].m_jointType) - { - case btMultibodyLink::eFixed: - { - break; - } - case btMultibodyLink::ePrismatic: - { - mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]); - mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]); - break; - } - case btMultibodyLink::eRevolute: - { - mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]); - mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]); - break; - } - case btMultibodyLink::eSpherical: - { - btScalar jointPos[3] = { mbd->m_links[i].m_jointPos[0], mbd->m_links[i].m_jointPos[1], mbd->m_links[i].m_jointPos[2] }; - btScalar jointVel[3] = { mbd->m_links[i].m_jointVel[0], mbd->m_links[i].m_jointVel[1], mbd->m_links[i].m_jointVel[2] }; - mb->setJointPosMultiDof(i, jointPos); - mb->setJointVelMultiDof(i, jointVel); - - break; - } - case btMultibodyLink::ePlanar: - { - break; - } - default: - { - } - } - } - mb->forwardKinematics(scratchQ, scratchM); - mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM); + syncMultiBody(mbd, mb, m_data, scratchQ, scratchM); } //todo: check why body1 pointer is not properly deserialized for (int i = 0; i < bulletFile2->m_contactManifolds.size(); i++) { btPersistentManifoldDoubleData* manifoldData = (btPersistentManifoldDoubleData*)bulletFile2->m_contactManifolds[i]; - void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1); - if (ptr) { - manifoldData->m_body1 = ptr; - btCollisionObjectDoubleData* cdd = (btCollisionObjectDoubleData*)ptr; + void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0); + if (ptr) + { + manifoldData->m_body0 = ptr; + } + } + + { + void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1); + if (ptr) + { + manifoldData->m_body1 = ptr; + } } } - btDispatcherInfo& dispatchInfo = m_data->m_mbDynamicsWorld->getDispatchInfo(); + if (bulletFile2->m_contactManifolds.size()) { - m_data->m_mbDynamicsWorld->updateAabbs(); - m_data->m_mbDynamicsWorld->computeOverlappingPairs(); - btDispatcher* dispatcher = m_data->m_mbDynamicsWorld->getDispatcher(); - - if (dispatcher) - { - btOverlappingPairCache* pairCache = m_data->m_mbDynamicsWorld->getBroadphase()->getOverlappingPairCache(); - if (dispatcher) - { - dispatcher->dispatchAllCollisionPairs(pairCache, dispatchInfo, dispatcher); - } - int numExistingManifolds = m_data->m_mbDynamicsWorld->getDispatcher()->getNumManifolds(); - btManifoldArray manifoldArray; - for (int i = 0; i < pairCache->getNumOverlappingPairs(); i++) - { - btBroadphasePair& pair = pairCache->getOverlappingPairArray()[i]; - if (pair.m_algorithm) - { - pair.m_algorithm->getAllContactManifolds(manifoldArray); - //for each existing manifold, search a matching manifoldData and reconstruct - for (int m = 0; m < manifoldArray.size(); m++) - { - btPersistentManifold* existingManifold = manifoldArray[m]; - int uid0 = existingManifold->getBody0()->getBroadphaseHandle()->m_uniqueId; - int uid1 = existingManifold->getBody1()->getBroadphaseHandle()->m_uniqueId; - int matchingManifoldIndex = -1; - for (int q= 0; q < bulletFile2->m_contactManifolds.size(); q++) - { - btPersistentManifoldDoubleData* manifoldData = (btPersistentManifoldDoubleData*)bulletFile2->m_contactManifolds[q]; - btCollisionObjectDoubleData* cdd0 = (btCollisionObjectDoubleData*)manifoldData->m_body0; - btCollisionObjectDoubleData* cdd1 = (btCollisionObjectDoubleData*)manifoldData->m_body1; - if (uid0 == cdd0->m_uniqueId && uid1 == cdd1->m_uniqueId) - { - matchingManifoldIndex = q; - } - } - if (matchingManifoldIndex >= 0) - { - btPersistentManifoldDoubleData* manifoldData = (btPersistentManifoldDoubleData*)bulletFile2->m_contactManifolds[matchingManifoldIndex]; - existingManifold->deSerializeDouble(manifoldData); - } - else - { - existingManifold->setNumContacts(0); - //printf("Issue: cannot find maching contact manifold (%d, %d), may cause issues in determinism.\n", uid0, uid1); - } + syncContactManifolds((btPersistentManifoldDoubleData**)&bulletFile2->m_contactManifolds[0], bulletFile2->m_contactManifolds.size(), m_data); + } + } + else + { + //single precision version + //for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++) + for (int i = bulletFile2->m_multiBodies.size() - 1; i >= 0; i--) + { + btMultiBodyFloatData* mbd = (btMultiBodyFloatData*)bulletFile2->m_multiBodies[i]; + btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i); + syncMultiBody(mbd, mb, m_data, scratchQ, scratchM); + } - manifoldArray.clear(); - } - } + //todo: check why body1 pointer is not properly deserialized + for (int i = 0; i < bulletFile2->m_contactManifolds.size(); i++) + { + btPersistentManifoldFloatData* manifoldData = (btPersistentManifoldFloatData*)bulletFile2->m_contactManifolds[i]; + { + void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0); + if (ptr) + { + manifoldData->m_body0 = ptr; + } + } + { + void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1); + if (ptr) + { + manifoldData->m_body1 = ptr; } } } + + + if (bulletFile2->m_contactManifolds.size()) + { + syncContactManifolds((btPersistentManifoldFloatData**)&bulletFile2->m_contactManifolds[0], bulletFile2->m_contactManifolds.size(), m_data); + } + } } else @@ -202,91 +365,12 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) { btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i]; - bool isFixedBase = mbd->m_baseMass == 0; - bool canSleep = false; - btVector3 baseInertia; - baseInertia.deSerializeDouble(mbd->m_baseInertia); - btMultiBody* mb = new btMultiBody(mbd->m_numLinks, mbd->m_baseMass, baseInertia, isFixedBase, canSleep); - mb->setHasSelfCollision(false); - - btVector3 baseWorldPos; - baseWorldPos.deSerializeDouble(mbd->m_baseWorldPosition); - - btQuaternion baseWorldOrn; - baseWorldOrn.deSerializeDouble(mbd->m_baseWorldOrientation); - mb->setBasePos(baseWorldPos); - mb->setWorldToBaseRot(baseWorldOrn.inverse()); - - m_data->m_mbMap.insert(mbd, mb); - for (int i = 0; i < mbd->m_numLinks; i++) - { - btVector3 localInertiaDiagonal; - localInertiaDiagonal.deSerializeDouble(mbd->m_links[i].m_linkInertia); - btQuaternion parentRotToThis; - parentRotToThis.deSerializeDouble(mbd->m_links[i].m_zeroRotParentToThis); - btVector3 parentComToThisPivotOffset; - parentComToThisPivotOffset.deSerializeDouble(mbd->m_links[i].m_parentComToThisComOffset); - btVector3 thisPivotToThisComOffset; - thisPivotToThisComOffset.deSerializeDouble(mbd->m_links[i].m_thisPivotToThisComOffset); - - switch (mbd->m_links[i].m_jointType) - { - case btMultibodyLink::eFixed: - { - - - mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex, - parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset); - //search for the collider - //mbd->m_links[i].m_linkCollider - break; - } - case btMultibodyLink::ePrismatic: - { - btVector3 jointAxis; - jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisBottom[0]); - bool disableParentCollision = true;//todo - mb->setupPrismatic(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex, - parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision); - mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]); - mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]); - break; - } - case btMultibodyLink::eRevolute: - { - btVector3 jointAxis; - jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisTop[0]); - bool disableParentCollision = true;//todo - mb->setupRevolute(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex, - parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision); - mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]); - mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]); - break; - } - case btMultibodyLink::eSpherical: - { - btAssert(0); - bool disableParentCollision = true;//todo - mb->setupSpherical(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex, - parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision); - btScalar jointPos[3] = { mbd->m_links[i].m_jointPos[0], mbd->m_links[i].m_jointPos[1], mbd->m_links[i].m_jointPos[2] }; - btScalar jointVel[3] = { mbd->m_links[i].m_jointVel[0], mbd->m_links[i].m_jointVel[1], mbd->m_links[i].m_jointVel[2] }; - mb->setJointPosMultiDof(i, jointPos); - mb->setJointVelMultiDof(i, jointVel); - - break; - } - case btMultibodyLink::ePlanar: - { - btAssert(0); - break; - } - default: - { - btAssert(0); - } - } - } + convertMultiBody(mbd, m_data); + } + else + { + btMultiBodyFloatData* mbd = (btMultiBodyFloatData*)bulletFile2->m_multiBodies[i]; + convertMultiBody(mbd, m_data); } } diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index 54a190515..0a8ca02e1 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -361,7 +361,7 @@ const char* btPersistentManifold::serialize(const class btPersistentManifold* ma return btPersistentManifoldDataName; } -void btPersistentManifold::deSerializeDouble(const struct btPersistentManifoldDoubleData* manifoldDataPtr) +void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr) { m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold; m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold; @@ -405,4 +405,50 @@ void btPersistentManifold::deSerializeDouble(const struct btPersistentManifoldDo pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i]; } +} + +void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr) +{ + m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold; + m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold; + m_cachedPoints = manifoldDataPtr->m_numCachedPoints; + m_companionIdA = manifoldDataPtr->m_companionIdA; + m_companionIdB = manifoldDataPtr->m_companionIdB; + //m_index1a = manifoldDataPtr->m_index1a; + m_objectType = manifoldDataPtr->m_objectType; + + for (int i = 0; i < this->getNumContacts(); i++) + { + btManifoldPoint& pt = m_pointCache[i]; + + pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i]; + pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i]; + pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i]; + pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]); + pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]); + pt.m_normalWorldOnB.deSerialize(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]); + pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i]; + pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i]; + pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i]; + pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i]; + pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i]; + pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i]; + pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i]; + pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i]; + pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i]; + pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i]; + pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i]; + pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i]; + pt.m_positionWorldOnA.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnA[i]); + pt.m_positionWorldOnB.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnB[i]); + pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i]; + pt.m_lateralFrictionDir1.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]); + pt.m_lateralFrictionDir2.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]); + pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i]; + pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i]; + pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i]; + pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i]; + pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i]; + } + } \ No newline at end of file diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index bece77451..0afdd2602 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -261,7 +261,8 @@ public: int calculateSerializeBufferSize() const; const char* serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const; - void deSerializeDouble(const struct btPersistentManifoldDoubleData* manifoldDataPtr); + void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr); + void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr); }; @@ -321,8 +322,8 @@ struct btPersistentManifoldFloatData btVector3FloatData m_pointCachePositionWorldOnA[4]; btVector3FloatData m_pointCachePositionWorldOnB[4]; btVector3FloatData m_pointCacheNormalWorldOnB[4]; - btVector3FloatData m_pointCacheLateralFrictionDir1; - btVector3FloatData m_pointCacheLateralFrictionDir2; + btVector3FloatData m_pointCacheLateralFrictionDir1[4]; + btVector3FloatData m_pointCacheLateralFrictionDir2[4]; float m_pointCacheDistance[4]; float m_pointCacheAppliedImpulse[4]; float m_pointCacheCombinedFriction[4]; diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index 7bd39e6a3..c29b0be65 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -602,7 +602,9 @@ public: SIMD_FORCE_INLINE void serialize(struct btQuaternionData& dataOut) const; - SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionData& dataIn); + SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionFloatData& dataIn); + + SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionDoubleData& dataIn); SIMD_FORCE_INLINE void serializeFloat(struct btQuaternionFloatData& dataOut) const; @@ -1003,10 +1005,16 @@ SIMD_FORCE_INLINE void btQuaternion::serialize(struct btQuaternionData& dataOut) dataOut.m_floats[i] = m_floats[i]; } -SIMD_FORCE_INLINE void btQuaternion::deSerialize(const struct btQuaternionData& dataIn) +SIMD_FORCE_INLINE void btQuaternion::deSerialize(const struct btQuaternionFloatData& dataIn) +{ + for (int i = 0; i<4; i++) + m_floats[i] = (btScalar)dataIn.m_floats[i]; +} + +SIMD_FORCE_INLINE void btQuaternion::deSerialize(const struct btQuaternionDoubleData& dataIn) { for (int i=0;i<4;i++) - m_floats[i] = dataIn.m_floats[i]; + m_floats[i] = (btScalar)dataIn.m_floats[i]; } diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h index c69effa96..cfc9354ec 100644 --- a/src/LinearMath/btVector3.h +++ b/src/LinearMath/btVector3.h @@ -705,7 +705,9 @@ public: SIMD_FORCE_INLINE void serialize(struct btVector3Data& dataOut) const; - SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data& dataIn); + SIMD_FORCE_INLINE void deSerialize(const struct btVector3DoubleData& dataIn); + + SIMD_FORCE_INLINE void deSerialize(const struct btVector3FloatData& dataIn); SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData& dataOut) const; @@ -1354,10 +1356,18 @@ SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const dataOut.m_floats[i] = m_floats[i]; } -SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn) + +SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3FloatData& dataIn) +{ + for (int i = 0; i<4; i++) + m_floats[i] = (btScalar)dataIn.m_floats[i]; +} + + +SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3DoubleData& dataIn) { for (int i=0;i<4;i++) - m_floats[i] = dataIn.m_floats[i]; + m_floats[i] = (btScalar)dataIn.m_floats[i]; } #endif //BT_VECTOR3_H