/** 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);
 }