public Volume3d transform(RotationMatrix4x4 m, Volume3d shape) {
switch (chainTree.volumeMode) {
case OBB:
Box3d box = (Box3d) shape;
Vector3d[] newBases = {
chainTree.getDirection(m, box.getBases()[0]),
chainTree.getDirection(m, box.getBases()[1]),
chainTree.getDirection(m, box.getBases()[2])
};
return new Box3d(chainTree.getPosition(m, box.getAnchor()), newBases, box.extents);
case CAPSULE:
Capsule3d caps = (Capsule3d) shape;
Capsule3d rotatedCaps =
new Capsule3d(
chainTree.getPosition(m, caps.segment.getA()),
chainTree.getPosition(m, caps.segment.getB()),
caps.rad);
return rotatedCaps;
case RSS:
RSS3d rss = (RSS3d) shape;
Point3d newCenter = chainTree.getPosition(m, rss.getCenter());
newBases =
new Vector3d[] {
chainTree.getDirection(m, rss.rectangle.bases[0]),
chainTree.getDirection(m, rss.rectangle.bases[1])
};
return new RSS3d(newCenter, newBases, rss.radius);
case PSS:
Sphere3d sphere = (Sphere3d) shape;
newCenter = chainTree.getPosition(m, sphere.center);
return new Sphere3d(newCenter, sphere.radius);
}
throw new Error("Unknown volume type chosen: " + chainTree.volumeMode);
}
