/** * Transforms the argument to root coordinates. * * @param rotation rotation that shall be transformed. * @return The <code>rotation</code> argument. */ private Rotation transformRotation(Rotation rotation) { Rotation.Builder transformedRotation = rotation.toBuilder().clone().clearQw().clearQx().clearQy().clearQz(); if (rotation.hasQw() && rotation.hasQx() && rotation.hasQy() && rotation.hasQz()) { Transform3D t = new Transform3D(); t.set(new Quat4d(rotation.getQw(), rotation.getQx(), rotation.getQy(), rotation.getQz())); Transform3D multiply = new Transform3D(); multiply.mul(transform, t); Quat4d result = new Quat4d(); multiply.get(result); transformedRotation.setQw(result.w).setQx(result.x).setQy(result.y).setQz(result.z); } return transformedRotation.build(); } }