debugging (possibly PVector/ArrayList)

AiSard

This is a snippet of a server I'm writing that's supposed to interface with a robot, but have been bogged down by this bug..

The problem (probably?) revolves around getFinalVec() which checks if vecBuffer[] (an ArrayList of PVectors) isn't empty and is valid (not too close to a certain point). I also included computeNextVector() just in case as its called from within getFinalVec()

Assume RSol-RIst is basically (0,0,0) As far as I understand it, vecBuffer[] is acting weirdly. When I checked nextDistance (which should be a PVector close to vecBuffer.get(0) ) I found it to alternate between the correct value and a wildly different one. So even though I input 18 PVectors in to vecBuffer[] I was seeing 36, but .size() was never greater than 18. The superflous PVector would trigger (nextDistance.mag() < cullDistance) and so remove an element from vecBuffer every other pass until it was empty :(

I've only done the basics with ArrayLists and PVectors so I'm assuming that I've messed up in some really simple (but hopefully immediately recognizable) way with one of them that some one can see?

Thanks in advance.

import java.util.*;
import hypermedia.net.*;
import controlP5.*;

ArrayList<PVector> vecBuffer = new ArrayList();

//...

void draw() {
  background(0);

  if (kukaBuffer!="") {
    kukaRef = splitTokens(kukaBuffer, "=");   //load kuka data
    extractVectors();                         //extract RIst/RSol
    getFinalVec();                            //defines finalVec and ensures valid input

    extractIPOC();
    sendtoKuka();
  }
}

void getFinalVec() {
  boolean check = false;
  while (check==false) {
    if (vecBuffer.size()==0) {
      finalVec = nullVec;
      check = true;      //get out of check, null vec
    } else {
      PVector nextDistance = PVector.sub(RSol, RIst);
      nextDistance.add(vecBuffer.get(0));
      println(nextDistance);
      println("{n="+vecBuffer.size());
      println(nextDistance.mag()<cullDistance);
      if (nextDistance.mag() < cullDistance) {
        vecBuffer.remove(0);
        println("removed!!");
        check = false;   //redo check
      } else {
        finalVec = computeNewVector();
        check = true;    //get out of check, new vec
      }
    }
  }
}

PVector computeNewVector() {
  //travel vector
  PVector nextDistance = PVector.sub(RSol, RIst);
  nextDistance.add(vecBuffer.get(0));
  //println("distance: "+nextDistance.mag());
  if (nextDistance.mag() < slowLim) {
    println("reached point");
    prevVec = currVec;
    currVec = nullVec;
    vecBuffer.remove(0);
  }

  float prevScalar = prevVec.mag() - acc;
  if (prevScalar<=0) {
    prevVec = nullVec;
  } else {
    prevVec.setMag(prevScalar);
  }

  float currScalar = currVec.mag() + acc;
  currVec = vecBuffer.get(0);
  if (currScalar >= spd) {
    currVec.setMag(spd);
  } else {
    currVec.setMag(currScalar);
  }

  //println(vecBuffer.get(0));
  PVector newVec = PVector.add(currVec, prevVec);
  newVec.limit(spd);
  return newVec; //finalVec
}