Changeset View
Changeset View
Standalone View
Standalone View
extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
| /* | /* | ||||
| Bullet Continuous Collision Detection and Physics Library | Bullet Continuous Collision Detection and Physics Library | ||||
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ | Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ | ||||
| This software is provided 'as-is', without any express or implied warranty. | 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. | 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, | Permission is granted to anyone to use this software for any purpose, | ||||
| including commercial applications, and to alter it and redistribute it freely, | including commercial applications, and to alter it and redistribute it freely, | ||||
| subject to the following restrictions: | 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. | 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. | 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. | 3. This notice may not be removed or altered from any source distribution. | ||||
| */ | */ | ||||
| #include "btSoftRigidDynamicsWorld.h" | #include "btSoftRigidDynamicsWorld.h" | ||||
| #include "LinearMath/btQuickprof.h" | #include "LinearMath/btQuickprof.h" | ||||
| //softbody & helpers | //softbody & helpers | ||||
| #include "btSoftBody.h" | #include "btSoftBody.h" | ||||
| #include "btSoftBodyHelpers.h" | #include "btSoftBodyHelpers.h" | ||||
| #include "btSoftBodySolvers.h" | #include "btSoftBodySolvers.h" | ||||
| #include "btDefaultSoftBodySolver.h" | #include "btDefaultSoftBodySolver.h" | ||||
| #include "LinearMath/btSerializer.h" | #include "LinearMath/btSerializer.h" | ||||
| btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld( | btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld( | ||||
| btDispatcher* dispatcher, | btDispatcher* dispatcher, | ||||
| btBroadphaseInterface* pairCache, | btBroadphaseInterface* pairCache, | ||||
| btConstraintSolver* constraintSolver, | btConstraintSolver* constraintSolver, | ||||
| btCollisionConfiguration* collisionConfiguration, | btCollisionConfiguration* collisionConfiguration, | ||||
| btSoftBodySolver *softBodySolver ) : | btSoftBodySolver* softBodySolver) : btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), | ||||
| btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), | |||||
| m_softBodySolver( softBodySolver ), | m_softBodySolver(softBodySolver), | ||||
| m_ownsSolver(false) | m_ownsSolver(false) | ||||
| { | { | ||||
| if( !m_softBodySolver ) | if (!m_softBodySolver) | ||||
| { | { | ||||
| void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16); | void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver), 16); | ||||
| m_softBodySolver = new(ptr) btDefaultSoftBodySolver(); | m_softBodySolver = new (ptr) btDefaultSoftBodySolver(); | ||||
| m_ownsSolver = true; | m_ownsSolver = true; | ||||
| } | } | ||||
| m_drawFlags = fDrawFlags::Std; | m_drawFlags = fDrawFlags::Std; | ||||
| m_drawNodeTree = true; | m_drawNodeTree = true; | ||||
| m_drawFaceTree = false; | m_drawFaceTree = false; | ||||
| m_drawClusterTree = false; | m_drawClusterTree = false; | ||||
| m_sbi.m_broadphase = pairCache; | m_sbi.m_broadphase = pairCache; | ||||
| m_sbi.m_dispatcher = dispatcher; | m_sbi.m_dispatcher = dispatcher; | ||||
| m_sbi.m_sparsesdf.Initialize(); | m_sbi.m_sparsesdf.Initialize(); | ||||
| m_sbi.m_sparsesdf.Reset(); | m_sbi.m_sparsesdf.Reset(); | ||||
| m_sbi.air_density = (btScalar)1.2; | m_sbi.air_density = (btScalar)1.2; | ||||
| m_sbi.water_density = 0; | m_sbi.water_density = 0; | ||||
| m_sbi.water_offset = 0; | m_sbi.water_offset = 0; | ||||
| m_sbi.water_normal = btVector3(0,0,0); | m_sbi.water_normal = btVector3(0, 0, 0); | ||||
| m_sbi.m_gravity.setValue(0,-10,0); | m_sbi.m_gravity.setValue(0, -10, 0); | ||||
| m_sbi.m_sparsesdf.Initialize(); | m_sbi.m_sparsesdf.Initialize(); | ||||
| } | } | ||||
| btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() | btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() | ||||
| { | { | ||||
| if (m_ownsSolver) | if (m_ownsSolver) | ||||
| { | { | ||||
| m_softBodySolver->~btSoftBodySolver(); | m_softBodySolver->~btSoftBodySolver(); | ||||
| btAlignedFree(m_softBodySolver); | btAlignedFree(m_softBodySolver); | ||||
| } | } | ||||
| } | } | ||||
| void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) | void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) | ||||
| { | { | ||||
| btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep ); | btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); | ||||
| { | { | ||||
| BT_PROFILE("predictUnconstraintMotionSoftBody"); | BT_PROFILE("predictUnconstraintMotionSoftBody"); | ||||
| m_softBodySolver->predictMotion( float(timeStep) ); | m_softBodySolver->predictMotion(float(timeStep)); | ||||
| } | } | ||||
| } | } | ||||
| void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep ) | void btSoftRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) | ||||
| { | { | ||||
| // Let the solver grab the soft bodies and if necessary optimize for it | // Let the solver grab the soft bodies and if necessary optimize for it | ||||
| m_softBodySolver->optimize( getSoftBodyArray() ); | m_softBodySolver->optimize(getSoftBodyArray()); | ||||
| if( !m_softBodySolver->checkInitialized() ) | if (!m_softBodySolver->checkInitialized()) | ||||
| { | { | ||||
| btAssert( "Solver initialization failed\n" ); | btAssert("Solver initialization failed\n"); | ||||
| } | } | ||||
| btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); | btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep); | ||||
| ///solve soft bodies constraints | ///solve soft bodies constraints | ||||
| solveSoftBodiesConstraints( timeStep ); | solveSoftBodiesConstraints(timeStep); | ||||
| //self collisions | //self collisions | ||||
| for ( int i=0;i<m_softBodies.size();i++) | for (int i = 0; i < m_softBodies.size(); i++) | ||||
| { | { | ||||
| btSoftBody* psb=(btSoftBody*)m_softBodies[i]; | btSoftBody* psb = (btSoftBody*)m_softBodies[i]; | ||||
| psb->defaultCollisionHandler(psb); | psb->defaultCollisionHandler(psb); | ||||
| } | } | ||||
| ///update soft bodies | ///update soft bodies | ||||
| m_softBodySolver->updateSoftBodies( ); | m_softBodySolver->updateSoftBodies(); | ||||
| // End solver-wise simulation step | // End solver-wise simulation step | ||||
| // /////////////////////////////// | // /////////////////////////////// | ||||
| } | } | ||||
| void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep ) | void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints(btScalar timeStep) | ||||
| { | { | ||||
| BT_PROFILE("solveSoftConstraints"); | BT_PROFILE("solveSoftConstraints"); | ||||
| if(m_softBodies.size()) | if (m_softBodies.size()) | ||||
| { | { | ||||
| btSoftBody::solveClusters(m_softBodies); | btSoftBody::solveClusters(m_softBodies); | ||||
| } | } | ||||
| // Solve constraints solver-wise | // Solve constraints solver-wise | ||||
| m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() ); | m_softBodySolver->solveConstraints(timeStep * m_softBodySolver->getTimeScale()); | ||||
| } | } | ||||
| void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask) | void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) | ||||
| { | { | ||||
| m_softBodies.push_back(body); | m_softBodies.push_back(body); | ||||
| // Set the soft body solver that will deal with this body | // Set the soft body solver that will deal with this body | ||||
| // to be the world's solver | // to be the world's solver | ||||
| body->setSoftBodySolver( m_softBodySolver ); | body->setSoftBodySolver(m_softBodySolver); | ||||
| btCollisionWorld::addCollisionObject(body, | btCollisionWorld::addCollisionObject(body, | ||||
| collisionFilterGroup, | collisionFilterGroup, | ||||
| collisionFilterMask); | collisionFilterMask); | ||||
| } | } | ||||
| void btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body) | void btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body) | ||||
| { | { | ||||
| m_softBodies.remove(body); | m_softBodies.remove(body); | ||||
| btCollisionWorld::removeCollisionObject(body); | btCollisionWorld::removeCollisionObject(body); | ||||
| } | } | ||||
| void btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) | void btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) | ||||
| { | { | ||||
| btSoftBody* body = btSoftBody::upcast(collisionObject); | btSoftBody* body = btSoftBody::upcast(collisionObject); | ||||
| if (body) | if (body) | ||||
| removeSoftBody(body); | removeSoftBody(body); | ||||
| else | else | ||||
| btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); | btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); | ||||
| } | } | ||||
| void btSoftRigidDynamicsWorld::debugDrawWorld() | void btSoftRigidDynamicsWorld::debugDrawWorld() | ||||
| { | { | ||||
| btDiscreteDynamicsWorld::debugDrawWorld(); | btDiscreteDynamicsWorld::debugDrawWorld(); | ||||
| if (getDebugDrawer()) | if (getDebugDrawer()) | ||||
| { | { | ||||
| int i; | int i; | ||||
| for ( i=0;i<this->m_softBodies.size();i++) | for (i = 0; i < this->m_softBodies.size(); i++) | ||||
| { | { | ||||
| btSoftBody* psb=(btSoftBody*)this->m_softBodies[i]; | btSoftBody* psb = (btSoftBody*)this->m_softBodies[i]; | ||||
| if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) | if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) | ||||
| { | { | ||||
| btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); | btSoftBodyHelpers::DrawFrame(psb, m_debugDrawer); | ||||
| btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags); | btSoftBodyHelpers::Draw(psb, m_debugDrawer, m_drawFlags); | ||||
| } | } | ||||
| if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) | if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) | ||||
| { | { | ||||
| if(m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); | if (m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb, m_debugDrawer); | ||||
| if(m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer); | if (m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb, m_debugDrawer); | ||||
| if(m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer); | if (m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb, m_debugDrawer); | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| struct btSoftSingleRayCallback : public btBroadphaseRayCallback | struct btSoftSingleRayCallback : public btBroadphaseRayCallback | ||||
| { | { | ||||
| btVector3 m_rayFromWorld; | btVector3 m_rayFromWorld; | ||||
| btVector3 m_rayToWorld; | btVector3 m_rayToWorld; | ||||
| btTransform m_rayFromTrans; | btTransform m_rayFromTrans; | ||||
| btTransform m_rayToTrans; | btTransform m_rayToTrans; | ||||
| btVector3 m_hitNormal; | btVector3 m_hitNormal; | ||||
| const btSoftRigidDynamicsWorld* m_world; | const btSoftRigidDynamicsWorld* m_world; | ||||
| btCollisionWorld::RayResultCallback& m_resultCallback; | btCollisionWorld::RayResultCallback& m_resultCallback; | ||||
| btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftRigidDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback) | btSoftSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btSoftRigidDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback) | ||||
| :m_rayFromWorld(rayFromWorld), | : m_rayFromWorld(rayFromWorld), | ||||
| m_rayToWorld(rayToWorld), | m_rayToWorld(rayToWorld), | ||||
| m_world(world), | m_world(world), | ||||
| m_resultCallback(resultCallback) | m_resultCallback(resultCallback) | ||||
| { | { | ||||
| m_rayFromTrans.setIdentity(); | m_rayFromTrans.setIdentity(); | ||||
| m_rayFromTrans.setOrigin(m_rayFromWorld); | m_rayFromTrans.setOrigin(m_rayFromWorld); | ||||
| m_rayToTrans.setIdentity(); | m_rayToTrans.setIdentity(); | ||||
| m_rayToTrans.setOrigin(m_rayToWorld); | m_rayToTrans.setOrigin(m_rayToWorld); | ||||
| btVector3 rayDir = (rayToWorld-rayFromWorld); | btVector3 rayDir = (rayToWorld - rayFromWorld); | ||||
| rayDir.normalize (); | rayDir.normalize(); | ||||
| ///what about division by zero? --> just set rayDirection[i] to INF/1e30 | ///what about division by zero? --> just set rayDirection[i] to INF/1e30 | ||||
| m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; | m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; | ||||
| m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; | m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; | ||||
| m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; | m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; | ||||
| m_signs[0] = m_rayDirectionInverse[0] < 0.0; | m_signs[0] = m_rayDirectionInverse[0] < 0.0; | ||||
| m_signs[1] = m_rayDirectionInverse[1] < 0.0; | m_signs[1] = m_rayDirectionInverse[1] < 0.0; | ||||
| m_signs[2] = m_rayDirectionInverse[2] < 0.0; | m_signs[2] = m_rayDirectionInverse[2] < 0.0; | ||||
| m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld); | m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); | ||||
| } | } | ||||
| virtual bool process(const btBroadphaseProxy* proxy) | virtual bool process(const btBroadphaseProxy* proxy) | ||||
| { | { | ||||
| ///terminate further ray tests, once the closestHitFraction reached zero | ///terminate further ray tests, once the closestHitFraction reached zero | ||||
| if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) | if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) | ||||
| return false; | return false; | ||||
| btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; | btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; | ||||
| //only perform raycast if filterMask matches | //only perform raycast if filterMask matches | ||||
| if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) | if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) | ||||
| { | { | ||||
| //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); | //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); | ||||
| //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; | //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; | ||||
| #if 0 | #if 0 | ||||
| #ifdef RECALCULATE_AABB | #ifdef RECALCULATE_AABB | ||||
| btVector3 collisionObjectAabbMin,collisionObjectAabbMax; | btVector3 collisionObjectAabbMin,collisionObjectAabbMax; | ||||
| collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); | collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); | ||||
| #else | #else | ||||
| //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax); | //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax); | ||||
| const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin; | const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin; | ||||
| const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; | const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; | ||||
| #endif | #endif | ||||
| #endif | #endif | ||||
| //btScalar hitLambda = m_resultCallback.m_closestHitFraction; | //btScalar hitLambda = m_resultCallback.m_closestHitFraction; | ||||
| //culling already done by broadphase | //culling already done by broadphase | ||||
| //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) | //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) | ||||
| { | { | ||||
| m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans, | m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, | ||||
| collisionObject, | collisionObject, | ||||
| collisionObject->getCollisionShape(), | collisionObject->getCollisionShape(), | ||||
| collisionObject->getWorldTransform(), | collisionObject->getWorldTransform(), | ||||
| m_resultCallback); | m_resultCallback); | ||||
| } | } | ||||
| } | } | ||||
| return true; | return true; | ||||
| } | } | ||||
| }; | }; | ||||
| void btSoftRigidDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const | void btSoftRigidDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const | ||||
| { | { | ||||
| BT_PROFILE("rayTest"); | BT_PROFILE("rayTest"); | ||||
| /// use the broadphase to accelerate the search for objects, based on their aabb | /// use the broadphase to accelerate the search for objects, based on their aabb | ||||
| /// and for each object with ray-aabb overlap, perform an exact ray test | /// and for each object with ray-aabb overlap, perform an exact ray test | ||||
| btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback); | btSoftSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); | ||||
| #ifndef USE_BRUTEFORCE_RAYBROADPHASE | #ifndef USE_BRUTEFORCE_RAYBROADPHASE | ||||
| m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); | m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); | ||||
| #else | #else | ||||
| for (int i=0;i<this->getNumCollisionObjects();i++) | for (int i = 0; i < this->getNumCollisionObjects(); i++) | ||||
| { | { | ||||
| rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); | rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); | ||||
| } | } | ||||
| #endif //USE_BRUTEFORCE_RAYBROADPHASE | #endif //USE_BRUTEFORCE_RAYBROADPHASE | ||||
| } | } | ||||
| void btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, | void btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, | ||||
| btCollisionObject* collisionObject, | btCollisionObject* collisionObject, | ||||
| const btCollisionShape* collisionShape, | const btCollisionShape* collisionShape, | ||||
| const btTransform& colObjWorldTransform, | const btTransform& colObjWorldTransform, | ||||
| RayResultCallback& resultCallback) | RayResultCallback& resultCallback) | ||||
| { | { | ||||
| if (collisionShape->isSoftBody()) { | if (collisionShape->isSoftBody()) | ||||
| { | |||||
| btSoftBody* softBody = btSoftBody::upcast(collisionObject); | btSoftBody* softBody = btSoftBody::upcast(collisionObject); | ||||
| if (softBody) { | if (softBody) | ||||
| { | |||||
| btSoftBody::sRayCast softResult; | btSoftBody::sRayCast softResult; | ||||
| if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) | if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) | ||||
| { | { | ||||
| if (softResult.fraction<= resultCallback.m_closestHitFraction) | if (softResult.fraction <= resultCallback.m_closestHitFraction) | ||||
| { | { | ||||
| btCollisionWorld::LocalShapeInfo shapeInfo; | btCollisionWorld::LocalShapeInfo shapeInfo; | ||||
| shapeInfo.m_shapePart = 0; | shapeInfo.m_shapePart = 0; | ||||
| shapeInfo.m_triangleIndex = softResult.index; | shapeInfo.m_triangleIndex = softResult.index; | ||||
| // get the normal | // get the normal | ||||
| btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); | btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); | ||||
| btVector3 normal=-rayDir; | btVector3 normal = -rayDir; | ||||
| normal.normalize(); | normal.normalize(); | ||||
| if (softResult.feature == btSoftBody::eFeature::Face) | if (softResult.feature == btSoftBody::eFeature::Face) | ||||
| { | { | ||||
| normal = softBody->m_faces[softResult.index].m_normal; | normal = softBody->m_faces[softResult.index].m_normal; | ||||
| if (normal.dot(rayDir) > 0) { | if (normal.dot(rayDir) > 0) | ||||
| { | |||||
| // normal always point toward origin of the ray | // normal always point toward origin of the ray | ||||
| normal = -normal; | normal = -normal; | ||||
| } | } | ||||
| } | } | ||||
| btCollisionWorld::LocalRayResult rayResult | btCollisionWorld::LocalRayResult rayResult(collisionObject, | ||||
| (collisionObject, | |||||
| &shapeInfo, | &shapeInfo, | ||||
| normal, | normal, | ||||
| softResult.fraction); | softResult.fraction); | ||||
| bool normalInWorldSpace = true; | bool normalInWorldSpace = true; | ||||
| resultCallback.addSingleResult(rayResult,normalInWorldSpace); | resultCallback.addSingleResult(rayResult, normalInWorldSpace); | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| else { | else | ||||
| { | |||||
| btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback); | btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback); | ||||
| } | } | ||||
| } | } | ||||
| void btSoftRigidDynamicsWorld::serializeSoftBodies(btSerializer* serializer) | void btSoftRigidDynamicsWorld::serializeSoftBodies(btSerializer* serializer) | ||||
| { | { | ||||
| int i; | int i; | ||||
| //serialize all collision objects | //serialize all collision objects | ||||
| for (i=0;i<m_collisionObjects.size();i++) | for (i = 0; i < m_collisionObjects.size(); i++) | ||||
| { | { | ||||
| btCollisionObject* colObj = m_collisionObjects[i]; | btCollisionObject* colObj = m_collisionObjects[i]; | ||||
| if (colObj->getInternalType() & btCollisionObject::CO_SOFT_BODY) | if (colObj->getInternalType() & btCollisionObject::CO_SOFT_BODY) | ||||
| { | { | ||||
| int len = colObj->calculateSerializeBufferSize(); | int len = colObj->calculateSerializeBufferSize(); | ||||
| btChunk* chunk = serializer->allocate(len,1); | btChunk* chunk = serializer->allocate(len, 1); | ||||
| const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); | const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); | ||||
| serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj); | serializer->finalizeChunk(chunk, structType, BT_SOFTBODY_CODE, colObj); | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| void btSoftRigidDynamicsWorld::serialize(btSerializer* serializer) | void btSoftRigidDynamicsWorld::serialize(btSerializer* serializer) | ||||
| { | { | ||||
| serializer->startSerialization(); | serializer->startSerialization(); | ||||
| serializeDynamicsWorldInfo( serializer); | serializeDynamicsWorldInfo(serializer); | ||||
| serializeSoftBodies(serializer); | serializeSoftBodies(serializer); | ||||
| serializeRigidBodies(serializer); | serializeRigidBodies(serializer); | ||||
| serializeCollisionObjects(serializer); | serializeCollisionObjects(serializer); | ||||
| serializer->finishSerialization(); | serializer->finishSerialization(); | ||||
| } | } | ||||