private static void cloneSubtree(RigidBodyReadOnly originalStart, RigidBodyBasics cloneStart, String cloneSuffix, RigidBodyBuilder rigidBodyBuilder, JointBuilder jointBuilder) { Map<RigidBodyReadOnly, RigidBodyBasics> originalToCloneBodyMap = new HashMap<>(); originalToCloneBodyMap.put(originalStart, cloneStart); for (JointReadOnly originalJoint : originalStart.childrenSubtreeIterable()) { RigidBodyReadOnly originalPredecessor = originalJoint.getPredecessor(); // Retrieve the right predecessor for the joint to clone. The map has to contain the clone predecessor. RigidBodyBasics clonePredecessor = originalToCloneBodyMap.get(originalPredecessor); // Clone the joint JointBasics cloneJoint = cloneJoint(originalJoint, cloneSuffix, clonePredecessor, originalStart.isRootBody(), jointBuilder); // Clone the successor RigidBodyReadOnly originalSuccessor = originalJoint.getSuccessor(); RigidBodyBasics cloneSuccessor = cloneRigidBody(originalSuccessor, null, cloneSuffix, cloneJoint, rigidBodyBuilder); originalToCloneBodyMap.put(originalSuccessor, cloneSuccessor); } }
/** * Performs a deep copy of an entire multi-body system. * <p> * The clone of the multi-body system has its own root body which reference frame shares the same * parent as {@code originalRootBody.getBodyFixedFrame()}. * </p> * * @param originalRootBody the root of the multi-body system to clone. Not modified. * @param cloneStationaryFrame the reference frame to which the cloned system is attached to. The * given frame is expected to be stationary. * @param cloneSuffix suffix to append to the cloned joints and rigid-bodies. * @param rigidBodyBuilder the builder to use for creating rigid-bodies. If {@code null}, * {@link #DEFAULT_RIGID_BODY_BUILDER} is used. * @param jointBuilder the builder to use for creating joints. If {@code null}, * {@link #DEFAULT_JOINT_BUILDER} is used. * @return the clone multi-body system. * @throws IllegalArgumentException if the given {@code originalRootBody} is not the root body of * its system. */ public static RigidBodyBasics cloneMultiBodySystem(RigidBodyReadOnly originalRootBody, ReferenceFrame cloneStationaryFrame, String cloneSuffix, RigidBodyBuilder rigidBodyBuilder, JointBuilder jointBuilder) { if (!originalRootBody.isRootBody()) throw new IllegalArgumentException("The given rigid-body is not the root-body of its multi-body system: " + originalRootBody.getName()); RigidBodyBasics cloneSubtreeStartBody = cloneRigidBody(originalRootBody, cloneStationaryFrame, cloneSuffix, null, rigidBodyBuilder); cloneSubtree(originalRootBody, cloneSubtreeStartBody, cloneSuffix, rigidBodyBuilder, jointBuilder); return cloneSubtreeStartBody; }
RigidBodyBasics cloneAncestor; if (originalAncestor.isRootBody()) cloneAncestor = cloneRigidBody(originalAncestor, chainRootFrame, cloneSuffix, null, null); else if (chainRootFrame != null) cloneAncestor = new RigidBody(originalAncestor.getName() + cloneSuffix, chainRootFrame); RigidBodyBasics cloneSuccessor = cloneRigidBody(originalSuccessor, null, cloneSuffix, cloneJoint, null); originalToCloneBodyMap.put(originalSuccessor, cloneSuccessor);