/**
* Get Line over given ids of saved motion states
*
* @param ids
* @return
*/
protected ArrayList<VectorLine> getLineToId(ArrayList<Integer> ids) {
if (savedMovementFlow.size() < 1) {
return new ArrayList<VectorLine>();
}
ArrayList<VectorLine> lines = new ArrayList<VectorLine>();
int numberOfChildren = getMaxMovementStep().getMove().children.size();
if (numberOfChildren == 0) {
ArrayList<Vector3f> points = new ArrayList<Vector3f>();
for (Integer i : ids) {
MovementStep m = savedMovementFlow.get(i);
Quaternion quat = m.getMove().getFingerTopPosition();
points.add(Utils.quatToVecPos(quat));
}
lines.add(new VectorLine(points));
} else {
for (int j = 0; j < numberOfChildren; j++) {
ArrayList<Vector3f> points = new ArrayList<Vector3f>();
for (Integer i : ids) {
MovementStep m = savedMovementFlow.get(i);
Quaternion quat = m.getMove().children.get(j).getLatestChild().getFingerTopPosition();
points.add(Utils.quatToVecPos(quat));
}
lines.add(new VectorLine(points));
}
}
return lines;
}
public void update(Joint updatedBy) {
for (MovementStep s : savedMovementFlow) {
s.getMove().updateWorldOrientation();
}
// check if updateBy is parent of currently observed joint
// if yes its not neccessary to update
if (observedJoint.hasParent(updatedBy)) {
return;
}
StoredJointState newState = new StoredJointState(observedJoint, observedJoint.parent, true);
if (lastChanged != null) {
StoredJointState lastState = lastChanged.getMove();
if (!newState.hasAngelDifferenceGreaterThan(lastState, MIN_ANGLE_DIFFERENCE)) {
// LOGGER.debug("Difference to low");
return;
}
}
// check if an almost same position is already saved and its not
// the last one (not increasing counter if we move slightly on
// position)
// if yes increase counter
for (int i = 0; i < savedMovementFlow.size() - 1; i++) {
MovementStep m = savedMovementFlow.get(i);
if (m != lastChanged && m.getMove().equals(newState)) {
m.incCount();
maxCount = Math.max(maxCount, m.getCount());
// LOGGER.debug("Find existing position - Increase count");
return;
}
}
// LOGGER.debug("Created new one");
lastChanged = new MovementStep(newState);
savedMovementFlow.addLast(lastChanged);
// get right parent
// save extrema
checkExtrema(newState);
}
