private void updateContactPoints() { for(int i = 0; i < contactPoints.size(); i++) { GroundContactPoint groundContactPoint = contactPoints.get(i); boolean isAboveDisableThreshold = groundContactPoint.getZ() > zHeightDisableThreshold.getDoubleValue(); if(isAboveDisableThreshold && !groundContactPoint.isDisabled()) { groundContactPoint.disable(); } else if(!isAboveDisableThreshold && groundContactPoint.isDisabled()) { groundContactPoint.setNotInContact(); } } }
for (double alpha = 0.0; allGCPs.stream().anyMatch(gc -> gc.getZ() > 0.01); alpha += 0.1)