public void createDebrisContactController() {
for (int i = 0; i < debrisRobots.size(); i++) {
ContactableSelectableBoxRobot debrisRobot = debrisRobots.get(i);
GroundContactModel groundContactModel =
createGroundContactModel(debrisRobot, combinedTerrainObject);
debrisRobot.createAvailableContactPoints(1, 20, forceVectorScale, true);
debrisRobot.setGroundContactModel(groundContactModel);
}
}
private ContactableSelectableBoxRobot createDebrisRobot(FramePose debrisPose) {
debrisPose.checkReferenceFrameMatch(constructionWorldFrame);
ContactableSelectableBoxRobot debris =
ContactableSelectableBoxRobot.createContactable2By4Robot(
debrisName + String.valueOf(id++), debrisDepth, debrisWidth, debrisLength, debrisMass);
debris.setPosition(debrisPose.getX(), debrisPose.getY(), debrisPose.getZ());
debris.setYawPitchRoll(debrisPose.getYaw(), debrisPose.getPitch(), debrisPose.getRoll());
return debris;
}