Real-Time Physics Simulation Forum

Code: Select all

Index: btDiscreteDynamicsWorld.cpp

===================================================================

--- btDiscreteDynamicsWorld.cpp	(revision 1696)

+++ btDiscreteDynamicsWorld.cpp	(working copy)

@@ -591,9 +591,9 @@

 		}
 };
 
+#include <iostream>
+using namespace std;
 
-
-
 void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
 	BT_PROFILE("solveConstraints");
@@ -647,9 +647,41 @@

 			{
 					//also add all non-contact constraints/joints for this island
 				btTypedConstraint** startConstraint = 0;
+				btTypedConstraint** tmpstartConstraint = 0;
 				int numCurConstraints = 0;
 				int i;
 				
+                //Use binary search to find the first constraint for this island
+			    int high, low;
+			    for ( low=(-1), high=m_numConstraints-1;  high-low > 1;  )
+			    {
+				    i = (high+low) / 2;
+                    int curID = btGetConstraintIslandId(m_sortedConstraints[i]);
+				    if (
+                        curID == islandId &&
+                        (i==0 || btGetConstraintIslandId(m_sortedConstraints[i-1]) != islandId)
+                        )
+				    {
+					    tmpstartConstraint = &m_sortedConstraints[i];
+                        break;
+				    }
+				    else if ( islandId <= curID  )
+					    high = i;
+				    else
+					    low  = i;
+			    }
+                if(
+                    high>=0 && 
+                   btGetConstraintIslandId(m_sortedConstraints[high]) == islandId &&
+                   (high==0 || btGetConstraintIslandId(m_sortedConstraints[high-1]) != islandId)
+                   )
+                {
+                    tmpstartConstraint = &m_sortedConstraints[high];
+                    i = high;
+                }
+				int tmpI = i;
+
+				/*
 				//find the first constraint for this island
 				for (i=0;i<m_numConstraints;i++)
 				{
@@ -659,6 +691,21 @@

 						break;
 					}
 				}
+				*/
+
+				startConstraint = tmpstartConstraint;
+				if(tmpstartConstraint != startConstraint)
+				{
+					cout << __FILE__ << " ERROR! " << tmpI << " != " << i << " Island ID: " << islandId << endl;
+
+					int j=0;
+					for (j=0;j<m_numConstraints;j++)
+					{
+						cout << btGetConstraintIslandId(m_sortedConstraints[j]) << ", ";
+					}
+					cout << endl;
+				}
+
 				//count the number of constraints in this island
 				for (;i<m_numConstraints;i++)
 				{
@@ -666,8 +713,13 @@

 					{
 						numCurConstraints++;
 					}
+					else
+					{
+						break;
+					}
 				}
 
+
 				///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
 				if (numManifolds + numCurConstraints)
 				{

Code: Select all

+#include <iostream>
+using namespace std;