fixing some issues: force needs to wake up objects, property sensor issue, island...

[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / btSimulationIslandManager.cpp

diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
index fa52d7e055af5fef464d6a1c514094b1c9de268d..5a642643eedc370ed05c51dfc1bd713c7b38edcd 100644 (file)
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
@@ -1,4 +1,6 @@
 
+
+#include "LinearMath/btScalar.h"
 #include "btSimulationIslandManager.h"
 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
@@ -40,8 +42,8 @@ void btSimulationIslandManager::findUnions(btDispatcher* dispatcher)
                                ((colObj1) && ((colObj1)->mergesSimulationIslands())))
                        {
 
-                               m_unionFind.unite((colObj0)->m_islandTag1,
-                                       (colObj1)->m_islandTag1);
+                               m_unionFind.unite((colObj0)->getIslandTag(),
+                                       (colObj1)->getIslandTag());
                        }
                }
        }
@@ -51,7 +53,7 @@ void btSimulationIslandManager::findUnions(btDispatcher* dispatcher)
 void   btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
 {
        
-       initUnionFind(colWorld->getCollisionObjectArray().size());
+       initUnionFind( int (colWorld->getCollisionObjectArray().size()));
        
        // put the index into m_controllers into m_tag  
        {
@@ -63,8 +65,8 @@ void  btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld
                {
                        
                        btCollisionObject*      collisionObject= (*i);
-                       collisionObject->m_islandTag1 = index;
-                       collisionObject->m_hitFraction = 1.f;
+                       collisionObject->setIslandTag(index);
+                       collisionObject->setHitFraction(1.f);
                        index++;
                        
                }
@@ -96,10 +98,10 @@ void        btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col
                        
                        if (collisionObject->mergesSimulationIslands())
                        {
-                               collisionObject->m_islandTag1 = m_unionFind.find(index);
+                               collisionObject->setIslandTag( m_unionFind.find(index) );
                        } else
                        {
-                               collisionObject->m_islandTag1 = -1;
+                               collisionObject->setIslandTag(-1);
                        }
                        index++;
                }
@@ -111,7 +113,7 @@ inline      int     getIslandId(const btPersistentManifold* lhs)
        int islandId;
        const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
        const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
-       islandId= rcolObj0->m_islandTag1>=0?rcolObj0->m_islandTag1:rcolObj1->m_islandTag1;
+       islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
        return islandId;
 
 }
@@ -156,25 +158,26 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
                        int i = getUnionFind().getElement(idx).m_sz;
 
                        btCollisionObject* colObj0 = collisionObjects[i];
-                       if ((colObj0->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1))
+                       if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
                        {
                                printf("error in island management\n");
                        }
 
-                       assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1));
-                       if (colObj0->m_islandTag1 == islandId)
+                       assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+                       if (colObj0->getIslandTag() == islandId)
                        {
-                               if (colObj0->GetActivationState()== ACTIVE_TAG)
+                               if (colObj0->getActivationState()== ACTIVE_TAG)
                                {
                                        allSleeping = false;
                                }
-                               if (colObj0->GetActivationState()== DISABLE_DEACTIVATION)
+                               if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
                                {
                                        allSleeping = false;
                                }
                        }
                }
-               
+                       
+
                if (allSleeping)
                {
                        int idx;
@@ -182,16 +185,16 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
                        {
                                int i = getUnionFind().getElement(idx).m_sz;
                                btCollisionObject* colObj0 = collisionObjects[i];
-                               if ((colObj0->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1))
+                               if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
                                {
                                        printf("error in island management\n");
                                }
 
-                               assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1));
+                               assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 
-                               if (colObj0->m_islandTag1 == islandId)
+                               if (colObj0->getIslandTag() == islandId)
                                {
-                                       colObj0->SetActivationState( ISLAND_SLEEPING );
+                                       colObj0->setActivationState( ISLAND_SLEEPING );
                                }
                        }
                } else
@@ -203,18 +206,18 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
                                int i = getUnionFind().getElement(idx).m_sz;
 
                                btCollisionObject* colObj0 = collisionObjects[i];
-                               if ((colObj0->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1))
+                               if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
                                {
                                        printf("error in island management\n");
                                }
 
-                               assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1));
+                               assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 
-                               if (colObj0->m_islandTag1 == islandId)
+                               if (colObj0->getIslandTag() == islandId)
                                {
-                                       if ( colObj0->GetActivationState() == ISLAND_SLEEPING)
+                                       if ( colObj0->getActivationState() == ISLAND_SLEEPING)
                                        {
-                                               colObj0->SetActivationState( WANTS_DEACTIVATION);
+                                               colObj0->setActivationState( WANTS_DEACTIVATION);
                                        }
                                }
                        }
@@ -234,17 +237,18 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
                 btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
                
                 //todo: check sleeping conditions!
-                if (((colObj0) && colObj0->GetActivationState() != ISLAND_SLEEPING) ||
-                       ((colObj1) && colObj1->GetActivationState() != ISLAND_SLEEPING))
+                if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
+                       ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
                {
+               
                        //kinematic objects don't merge islands, but wake up all connected objects
-                       if (colObj0->isKinematicObject() && colObj0->GetActivationState() != ISLAND_SLEEPING)
+                       if (colObj0->isStaticOrKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
                        {
-                               colObj1->SetActivationState(ACTIVE_TAG);
+                               colObj1->activate();
                        }
-                       if (colObj1->isKinematicObject() && colObj1->GetActivationState() != ISLAND_SLEEPING)
+                       if (colObj1->isStaticOrKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
                        {
-                               colObj0->SetActivationState(ACTIVE_TAG);
+                               colObj0->activate();
                        }
 
                        //filtering for response
@@ -253,7 +257,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
                }
        }
 
-       int numManifolds = islandmanifold.size();
+       int numManifolds = int (islandmanifold.size());
 
        // Sort manifolds, based on islands
        // Sort the vector using predicate and std::sort
@@ -264,17 +268,53 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
        int startManifoldIndex = 0;
        int endManifoldIndex = 1;
 
-       for (startManifoldIndex=0;startManifoldIndex<numManifolds;startManifoldIndex = endManifoldIndex)
+       int islandId;
+
+
+       //solve the constraint for each islands, if there are contacts/constraints
+       for (int startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
        {
-               int islandId = getIslandId(islandmanifold[startManifoldIndex]);
-               for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++)
+               int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+
+               bool islandSleeping = false;
+
+               for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
                {
+                       int i = getUnionFind().getElement(endIslandIndex).m_sz;
+                       btCollisionObject* colObj0 = collisionObjects[i];
+                       if (!colObj0->isActive())
+                               islandSleeping = true;
                }
-               /// Process the actual simulation, only if not sleeping/deactivated
-               int numIslandManifolds = endManifoldIndex-startManifoldIndex;
-               if (numIslandManifolds)
+
+               if (!islandSleeping)
                {
-                       callback->ProcessIsland(&islandmanifold[startManifoldIndex],numIslandManifolds);
+                       //find the accompanying contact manifold for this islandId
+                       int numIslandManifolds = 0;
+                       btPersistentManifold** startManifold = 0;
+
+                       if (startManifoldIndex<numManifolds)
+                       {
+                               int curIslandId = getIslandId(islandmanifold[startManifoldIndex]);
+                               if (curIslandId == islandId)
+                               {
+                                       startManifold = &islandmanifold[startManifoldIndex];
+                               
+                                       for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++)
+                                       {
+
+                                       }
+                                       /// Process the actual simulation, only if not sleeping/deactivated
+                                       numIslandManifolds = endManifoldIndex-startManifoldIndex;
+                               }
+
+                       }
+
+                       callback->ProcessIsland(startManifold,numIslandManifolds, islandId);
+
+                       if (numIslandManifolds)
+                       {
+                               startManifoldIndex = endManifoldIndex;
+                       }
                }
        }
 }