diff options
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp | 355 |
1 files changed, 0 insertions, 355 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp deleted file mode 100644 index 329bd19d71..0000000000 --- a/thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp +++ /dev/null @@ -1,355 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -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. -*/ - -#include "btSoftMultiBodyDynamicsWorld.h" -#include "LinearMath/btQuickprof.h" - -//softbody & helpers -#include "BulletSoftBody/btSoftBody.h" -#include "BulletSoftBody/btSoftBodyHelpers.h" -#include "BulletSoftBody/btSoftBodySolvers.h" -#include "BulletSoftBody/btDefaultSoftBodySolver.h" -#include "LinearMath/btSerializer.h" - -btSoftMultiBodyDynamicsWorld::btSoftMultiBodyDynamicsWorld( - btDispatcher* dispatcher, - btBroadphaseInterface* pairCache, - btMultiBodyConstraintSolver* constraintSolver, - btCollisionConfiguration* collisionConfiguration, - btSoftBodySolver* softBodySolver) : btMultiBodyDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), - m_softBodySolver(softBodySolver), - m_ownsSolver(false) -{ - if (!m_softBodySolver) - { - void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver), 16); - m_softBodySolver = new (ptr) btDefaultSoftBodySolver(); - m_ownsSolver = true; - } - - m_drawFlags = fDrawFlags::Std; - m_drawNodeTree = true; - m_drawFaceTree = false; - m_drawClusterTree = false; - m_sbi.m_broadphase = pairCache; - m_sbi.m_dispatcher = dispatcher; - m_sbi.m_sparsesdf.Initialize(); - m_sbi.m_sparsesdf.Reset(); - - m_sbi.air_density = (btScalar)1.2; - m_sbi.water_density = 0; - m_sbi.water_offset = 0; - m_sbi.water_normal = btVector3(0, 0, 0); - m_sbi.m_gravity.setValue(0, -10, 0); - - m_sbi.m_sparsesdf.Initialize(); -} - -btSoftMultiBodyDynamicsWorld::~btSoftMultiBodyDynamicsWorld() -{ - if (m_ownsSolver) - { - m_softBodySolver->~btSoftBodySolver(); - btAlignedFree(m_softBodySolver); - } -} - -void btSoftMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); - { - BT_PROFILE("predictUnconstraintMotionSoftBody"); - m_softBodySolver->predictMotion(float(timeStep)); - } -} - -void btSoftMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) -{ - // Let the solver grab the soft bodies and if necessary optimize for it - m_softBodySolver->optimize(getSoftBodyArray()); - - if (!m_softBodySolver->checkInitialized()) - { - btAssert("Solver initialization failed\n"); - } - - btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep); - - ///solve soft bodies constraints - solveSoftBodiesConstraints(timeStep); - - //self collisions - for (int i = 0; i < m_softBodies.size(); i++) - { - btSoftBody* psb = (btSoftBody*)m_softBodies[i]; - psb->defaultCollisionHandler(psb); - } - - ///update soft bodies - m_softBodySolver->updateSoftBodies(); - - for (int i = 0; i < m_softBodies.size(); i++) - { - btSoftBody* psb = (btSoftBody*)m_softBodies[i]; - psb->interpolateRenderMesh(); - } - // End solver-wise simulation step - // /////////////////////////////// -} - -void btSoftMultiBodyDynamicsWorld::solveSoftBodiesConstraints(btScalar timeStep) -{ - BT_PROFILE("solveSoftConstraints"); - - if (m_softBodies.size()) - { - btSoftBody::solveClusters(m_softBodies); - } - - // Solve constraints solver-wise - m_softBodySolver->solveConstraints(timeStep * m_softBodySolver->getTimeScale()); -} - -void btSoftMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) -{ - m_softBodies.push_back(body); - - // Set the soft body solver that will deal with this body - // to be the world's solver - body->setSoftBodySolver(m_softBodySolver); - - btCollisionWorld::addCollisionObject(body, - collisionFilterGroup, - collisionFilterMask); -} - -void btSoftMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body) -{ - m_softBodies.remove(body); - - btCollisionWorld::removeCollisionObject(body); -} - -void btSoftMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) -{ - btSoftBody* body = btSoftBody::upcast(collisionObject); - if (body) - removeSoftBody(body); - else - btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); -} - -void btSoftMultiBodyDynamicsWorld::debugDrawWorld() -{ - btMultiBodyDynamicsWorld::debugDrawWorld(); - - if (getDebugDrawer()) - { - int i; - for (i = 0; i < this->m_softBodies.size(); i++) - { - btSoftBody* psb = (btSoftBody*)this->m_softBodies[i]; - if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) - { - btSoftBodyHelpers::DrawFrame(psb, m_debugDrawer); - btSoftBodyHelpers::Draw(psb, m_debugDrawer, m_drawFlags); - } - - if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) - { - if (m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb, m_debugDrawer); - if (m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb, m_debugDrawer); - if (m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb, m_debugDrawer); - } - } - } -} - -struct btSoftSingleRayCallback : public btBroadphaseRayCallback -{ - btVector3 m_rayFromWorld; - btVector3 m_rayToWorld; - btTransform m_rayFromTrans; - btTransform m_rayToTrans; - btVector3 m_hitNormal; - - const btSoftMultiBodyDynamicsWorld* m_world; - btCollisionWorld::RayResultCallback& m_resultCallback; - - btSoftSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btSoftMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback) - : m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld), - m_world(world), - m_resultCallback(resultCallback) - { - m_rayFromTrans.setIdentity(); - m_rayFromTrans.setOrigin(m_rayFromWorld); - m_rayToTrans.setIdentity(); - m_rayToTrans.setOrigin(m_rayToWorld); - - btVector3 rayDir = (rayToWorld - rayFromWorld); - - rayDir.normalize(); - ///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[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_signs[0] = m_rayDirectionInverse[0] < 0.0; - m_signs[1] = m_rayDirectionInverse[1] < 0.0; - m_signs[2] = m_rayDirectionInverse[2] < 0.0; - - m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); - } - - virtual bool process(const btBroadphaseProxy* proxy) - { - ///terminate further ray tests, once the closestHitFraction reached zero - if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) - return false; - - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; - - //only perform raycast if filterMask matches - if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) - { - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; -#if 0 -#ifdef RECALCULATE_AABB - btVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); -#else - //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax); - const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin; - const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; -#endif -#endif - //btScalar hitLambda = m_resultCallback.m_closestHitFraction; - //culling already done by broadphase - //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) - { - m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback); - } - } - return true; - } -}; - -void btSoftMultiBodyDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const -{ - BT_PROFILE("rayTest"); - /// 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 - btSoftSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); - -#ifndef USE_BRUTEFORCE_RAYBROADPHASE - m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); -#else - for (int i = 0; i < this->getNumCollisionObjects(); i++) - { - rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); - } -#endif //USE_BRUTEFORCE_RAYBROADPHASE -} - -void btSoftMultiBodyDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) -{ - if (collisionShape->isSoftBody()) - { - btSoftBody* softBody = btSoftBody::upcast(collisionObject); - if (softBody) - { - btSoftBody::sRayCast softResult; - if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) - { - if (softResult.fraction <= resultCallback.m_closestHitFraction) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = 0; - shapeInfo.m_triangleIndex = softResult.index; - // get the normal - btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); - btVector3 normal = -rayDir; - normal.normalize(); - - if (softResult.feature == btSoftBody::eFeature::Face) - { - normal = softBody->m_faces[softResult.index].m_normal; - if (normal.dot(rayDir) > 0) - { - // normal always point toward origin of the ray - normal = -normal; - } - } - - btCollisionWorld::LocalRayResult rayResult(collisionObject, - &shapeInfo, - normal, - softResult.fraction); - bool normalInWorldSpace = true; - resultCallback.addSingleResult(rayResult, normalInWorldSpace); - } - } - } - } - else - { - btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback); - } -} - -void btSoftMultiBodyDynamicsWorld::serializeSoftBodies(btSerializer* serializer) -{ - int i; - //serialize all collision objects - for (i = 0; i < m_collisionObjects.size(); i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - if (colObj->getInternalType() & btCollisionObject::CO_SOFT_BODY) - { - int len = colObj->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len, 1); - const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk, structType, BT_SOFTBODY_CODE, colObj); - } - } -} - -void btSoftMultiBodyDynamicsWorld::serialize(btSerializer* serializer) -{ - serializer->startSerialization(); - - serializeDynamicsWorldInfo(serializer); - - serializeSoftBodies(serializer); - - serializeMultiBodies(serializer); - - serializeRigidBodies(serializer); - - serializeCollisionObjects(serializer); - - serializeContactManifolds(serializer); - - serializer->finishSerialization(); -} |