Merge pull request #7 from kitskub/master

Fix two issues

Author: DDoS

src/main/java/org/spout/physics/collision/broadphase/SweepAndPruneAlgorithm.java

@@ -393,6 +393,7 @@ private void shrinkArrays() {
                 newMapBodyToBoxIndex.put(body, boxIndex);
             }
         }
+        mFreeBoxIndices.clear();
         if (newMapBodyToBoxIndex.size() != mMapBodyToBoxIndex.size()) {
             throw new IllegalStateException("The size of the new map from body to box index must be the same as the old one");
         }

src/main/java/org/spout/physics/engine/DynamicsWorld.java

@@ -364,27 +364,32 @@ public void update() {
         mTimer.update();
         isTicking = true;
         while (mTimer.isPossibleToTakeStep()) {
-            mContactManifolds.clear();
-            resetContactManifoldListsOfBodies();
-            mCollisionDetection.computeCollisionDetection();
-            computeIslands();
-            integrateRigidBodiesVelocities();
-            resetBodiesMovementVariable();
-            mTimer.nextStep();
-            solveContactsAndConstraints();
-            integrateRigidBodiesPositions();
-            solvePositionCorrection();
-            if (mIsSleepingEnabled) {
-                updateSleepingBodies();
-            }
-            updateRigidBodiesAABB();
+            tick();
         }
         isTicking = false;
         resetBodiesForceAndTorque();
         setInterpolationFactorToAllBodies();
         disperseCache();
     }
 
+    // LinkedDynamicsWorld needs to clearLinkedBodies at the end of a tick, not update
+    protected void tick() {
+        mContactManifolds.clear();
+        resetContactManifoldListsOfBodies();
+        mCollisionDetection.computeCollisionDetection();
+        computeIslands();
+        integrateRigidBodiesVelocities();
+        resetBodiesMovementVariable();
+        mTimer.nextStep();
+        solveContactsAndConstraints();
+        integrateRigidBodiesPositions();
+        solvePositionCorrection();
+        if (mIsSleepingEnabled) {
+            updateSleepingBodies();
+        }
+        updateRigidBodiesAABB();
+    }
+
     // Resets the boolean movement variable for each body.
     private void resetBodiesMovementVariable() {
         for (RigidBody rigidBody : mRigidBodies) {
@@ -628,21 +633,25 @@ public void addRigidBody(RigidBody body) {
      */
     public void destroyRigidBody(RigidBody rigidBody) {
         if (!isTicking) {
-            mCollisionDetection.removeBody(rigidBody);
-            mFreeBodiesIDs.push(rigidBody.getID());
-            mBodies.remove(rigidBody);
-            mRigidBodies.remove(rigidBody);
-            removeCollisionShape(rigidBody.getCollisionShape());
-            JointListElement element;
-            for (element = rigidBody.getJointsList(); element != null; element = element.getNext()) {
-                destroyJoint(element.getJoint());
-            }
-            rigidBody.resetContactManifoldsList();
+            destroyRigidBodyImmediately(rigidBody);
         } else {
             mRigidBodiesToDeleteCache.add(rigidBody);
         }
     }
 
+    protected void destroyRigidBodyImmediately(RigidBody rigidBody) {
+        mCollisionDetection.removeBody(rigidBody);
+        mFreeBodiesIDs.push(rigidBody.getID());
+        mBodies.remove(rigidBody);
+        mRigidBodies.remove(rigidBody);
+        removeCollisionShape(rigidBody.getCollisionShape());
+        JointListElement element;
+        for (element = rigidBody.getJointsList(); element != null; element = element.getNext()) {
+            destroyJoint(element.getJoint());
+        }
+        rigidBody.resetContactManifoldsList();
+    }
+
     /**
      * Creates a joint between two bodies in the world and returns the new joint.
      *

src/main/java/org/spout/physics/engine/linked/LinkedDynamicsWorld.java

@@ -65,8 +65,8 @@ public LinkedDynamicsWorld(Vector3 gravity, LinkedWorldInfo info) {
     }
 
     @Override
-    public void update() {
-        super.update();
+    public void tick() {
+        super.tick();
         clearLinkedBodies();
     }
 
@@ -94,7 +94,8 @@ public void addLinkedBodies(Collection<RigidBody> bodies) {
     // Clears all bodies tracked in the world.
     private void clearLinkedBodies() {
         for (RigidBody linkedBody : linkedBodies) {
-            destroyRigidBody(linkedBody);
+            // We're in a safe part of the tick so we can clear them immediately
+            destroyRigidBodyImmediately(linkedBody);
         }
         linkedBodies.clear();
     }