private void checkExtrema(StoredJointState newState) {
if (newState.compareTo(getMinMovementStep().move) < 0) {
minId = savedMovementFlow.size() - 1;
minIds.add(minId);
}
if (newState.compareTo(getMaxMovementStep().move) > 0) {
maxId = savedMovementFlow.size() - 1;
maxIds.add(maxId);
}
}
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);
}