Set the absolute pose on the way out to tighten up any interpolation

floating point drift

src/main/java/com/neuronrobotics/bowlerstudio/threed/BowlerStudio3dEngine.java

@@ -102,7 +102,7 @@
  */
 public class BowlerStudio3dEngine implements ICameraChangeListener {
  private boolean focusing = false;
- private double targetDepth = 30;
+ private double numberOfInterpolationSteps = 30;
 
  /**
  * 
@@ -1606,7 +1606,7 @@ public void focusToAffine(TransformNR poseToMove, Affine manipulator2) {
 
  ex.printStackTrace();
  }
- focusInterpolate(startSelectNr, targetNR,(int) targetDepth, interpolator);
+ focusInterpolate(startSelectNr, targetNR,(int) numberOfInterpolationSteps, interpolator);
  });
  }
 
@@ -1658,7 +1658,7 @@ public void targetAndFollow(TransformNR poseToMove, Affine manipulator2) {
  } catch (Exception ex) {
  ex.printStackTrace();
  }
- focusInterpolate(startSelectNr, targetNR, (int)targetDepth, interpolator);
+ focusInterpolate(startSelectNr, targetNR, (int)numberOfInterpolationSteps, interpolator);
  });
  }
 
@@ -1704,28 +1704,34 @@ private void runSyncFocus(TransformNR orent,TransformNR trans, double zoom) {
  z=trans.getZ()-getFlyingCamera().getGlobalZ();
  }
  try {
- double d = 1.0 / targetDepth;
- for (double i = 0; i <= 1+0.000001; i += d) {
+ double d = 1.0 / numberOfInterpolationSteps;
+ for (double i = 0; i < 1; i += d) {
+ if(i>1)
+ i=1;
 // double aztmp = getFlyingCamera().getPanAngle();
 // double eltmp = getFlyingCamera().getTiltAngle();
  //System.out.println("\tFocus to \n\t\taz:" + aztmp + " \n\t\tel:" + eltmp);
- double mx=x/ targetDepth;
- double my=y/ targetDepth;
- double mz=z/ targetDepth;
+ double mx=x/ numberOfInterpolationSteps;
+ double my=y/ numberOfInterpolationSteps;
+ double mz=z/ numberOfInterpolationSteps;
  BowlerStudio.runLater(() -> {
- moveCamera(new TransformNR(0, 0, 0, new RotationNR(-el / targetDepth, -az / targetDepth, 0)));
+ moveCamera(new TransformNR(0, 0, 0, new RotationNR(-el / numberOfInterpolationSteps, -az / numberOfInterpolationSteps, 0)));
  getFlyingCamera().DrivePositionAbsolute(mx, my, mz);
  if(!getFlyingCamera().isZoomLocked())
- getFlyingCamera().setZoomDepth(getFlyingCamera().getZoomDepth()+(zoomDelta/targetDepth));
+ getFlyingCamera().setZoomDepth(getFlyingCamera().getZoomDepth()+(zoomDelta/numberOfInterpolationSteps));
  });
  try {
- Thread.sleep(16);
+ Thread.sleep(36);
  } catch (InterruptedException e) {
  // TODO Auto-generated catch block
  e.printStackTrace();
  focusing = false;
  }
  }
+ BowlerStudio.runLater(() ->{
+ getFlyingCamera().SetOrentation(orent);
+ getFlyingCamera().SetPosition(trans);
+ });
 
  } catch (Throwable t) {
  t.printStackTrace();