/** Creates a volume enclosing a leaf-node */
Volume3d createLeafVolume(CTNode nd, double extension) {
switch (chainTree.volumeMode) {
case OBB:
PointSet3d points = chainTree.getPoints(nd);
Point3d anchor = nd.getPQ().multiply(0.5).toPoint();
Vector3d b1 = nd.getPQ().norm();
Vector3d b2 = nd.getPQ().cross(nd.getPQ().add(new Vector3d(1.01, 1.02, 1.03))).norm();
Vector3d b3 = b1.cross(b2);
Vector3d[] bases = {b1, b2, b3};
double[] extents = {nd.getPQ().length() + extension, extension, extension};
Box3d ret = new Box3d(anchor, bases, extents);
return ret;
case CAPSULE:
return new Capsule3d(new Point3d(0, 0, 0), new Point3d(nd.getPQ()), extension);
case RSS:
anchor = nd.getPQ().multiply(0.5).toPoint();
b1 = nd.getPQ().norm();
b2 = nd.getPQ().cross(nd.getPQ().add(new Vector3d(1.01, 1.02, 1.03))).norm();
b3 = b1.cross(b2);
bases = new Vector3d[] {b1.multiplyIn(nd.getPQ().length() / 2), b2.multiplyIn(0.00001)};
RSS3d rss = new RSS3d(anchor, bases, extension);
return rss;
case PSS:
return new Sphere3d(
nd.getPQ().multiply(0.5).toPoint(), nd.getPQ().length() / 2 + extension);
}
throw new Error("Unknown volume type chosen: " + chainTree.volumeMode);
}
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);
}
public Volume3d transform(int i, int j, Volume3d shape) {
RotationMatrix4x4 m = chainTree.getTransformMatrix4x4(i, j);
return transform(m, shape);
}