Fix BodyQueue

src/BodyQueue.java

@@ -27,15 +27,15 @@ public class BodyQueue {
      * Initializes this queue as an independent copy of the specified queue.
      * Calling methods of this queue will not affect the specified queue
      * and vice versa.
-     * Precondition: q != null.
+     * Precondition: other != null.
      */
-    public BodyQueue(BodyQueue q) {
-        this.capacity = q.capacity;
-        this.head = q.size();
+    public BodyQueue(BodyQueue other) {
+        this.capacity = other.capacity;
+        this.head = other.size();
         this.tail = 0;
         this.queue = new Body[this.capacity];
-        for (int i = 0; i < q.size(); i++) {
-            this.queue[i] = q.queue[i];
+        for (int i = 0, j = other.tail; i < this.head; i++, j++) {
+            this.queue[i] = other.queue[j % other.capacity];
         }
     }
 
@@ -43,12 +43,11 @@ public class BodyQueue {
      * Adds the specified body 'b' to this queue.
      */
     public void add(Body b) {
+        if ((head + 1) % capacity == tail) {
+            doubleCapacity();
+        }
         queue[head] = b;
         head = (head + 1) % capacity;
-        if (head == tail) {
-            doubleCapacity();
-            head = capacity / 2;
-        }
     }
 
     /**
@@ -57,6 +56,8 @@ public class BodyQueue {
      */
     public Body poll() {
         if (tail == head) {
+            tail = 0;
+            head = 0;
             return null;
         }
         Body b = queue[tail];
@@ -77,10 +78,10 @@ public class BodyQueue {
      */
     private void doubleCapacity() {
         Body[] tmp = new Body[capacity * 2];
-        for (int i = head, j = 0; i < tail + capacity; i++, j++) {
-            tmp[j] = queue[i % capacity];
+        head = size();
+        for (int i = 0, j = tail; i < head; i++, j++) {
+            tmp[i] = queue[j % capacity];
         }
-        head = capacity;
         tail = 0;
         capacity *= 2;
         queue = tmp;