private void buildMultiBodyTree(IterativeStep parent, Collection<? extends JointReadOnly> jointsToIgnore) { for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints()) { if (jointsToIgnore.contains(childJoint)) continue; RigidBodyReadOnly childBody = childJoint.getSuccessor(); if (childBody != null) { int[] jointIndices = input.getJointMatrixIndexProvider().getJointDoFIndices(childJoint); IterativeStep child = new IterativeStep(childBody, jointIndices); rigidBodyToIterativeStepMap.put(childBody, child); parent.children.add(child); buildMultiBodyTree(child, jointsToIgnore); } } }
private void buildMultiBodyTree(RecursionStep parent, Collection<? extends JointReadOnly> jointsToIgnore) { for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints()) { if (jointsToIgnore.contains(childJoint)) continue; RigidBodyReadOnly childBody = childJoint.getSuccessor(); if (childBody != null) { int[] jointIndices = input.getJointMatrixIndexProvider().getJointDoFIndices(childJoint); RecursionStep child = new RecursionStep(childBody, jointIndices); rigidBodyToRecursionStepMap.put(childBody, child); parent.children.add(child); buildMultiBodyTree(child, jointsToIgnore); } } }
private List<CompositeRigidBodyInertia> buildMultiBodyTree(CompositeRigidBodyInertia parent) { List<CompositeRigidBodyInertia> inertiaList = new ArrayList<>(); for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints()) { RigidBodyReadOnly childBody = childJoint.getSuccessor(); if (childBody != null) { int[] jointIndices = input.getJointMatrixIndexProvider().getJointDoFIndices(childJoint); CompositeRigidBodyInertia child = new CompositeRigidBodyInertia(childBody, parent, jointIndices); inertiaList.add(child); inertiaList.addAll(buildMultiBodyTree(child)); } } return inertiaList; }
private void buildMultiBodyTree(RecursionStep parent, Collection<? extends JointReadOnly> jointsToIgnore) { for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints()) { if (jointsToIgnore.contains(childJoint)) continue; RigidBodyReadOnly childBody = childJoint.getSuccessor(); if (childBody != null) { int[] jointIndices = input.getJointMatrixIndexProvider().getJointDoFIndices(childJoint); RecursionStep child = new RecursionStep(childBody, parent, jointIndices); rigidBodyToRecursionStepMap.put(childBody, child); buildMultiBodyTree(child, jointsToIgnore); } } }
private void buildMultiBodyTree(ArticulatedBodyRecursionStep parent, Collection<? extends JointReadOnly> jointsToIgnore) { for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints()) { if (jointsToIgnore.contains(childJoint)) continue; RigidBodyReadOnly childBody = childJoint.getSuccessor(); if (childBody != null) { int[] jointIndices = input.getJointMatrixIndexProvider().getJointDoFIndices(childJoint); ArticulatedBodyRecursionStep child = new ArticulatedBodyRecursionStep(childBody, parent, jointIndices); rigidBodyToRecursionStepMap.put(childBody, child); buildMultiBodyTree(child, jointsToIgnore); } } }
private void buildMultiBodyTree(RecursionStep parent, Collection<? extends JointReadOnly> jointsToIgnore) { for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints()) { if (jointsToIgnore.contains(childJoint)) continue; RigidBodyReadOnly childBody = childJoint.getSuccessor(); if (childBody != null) { int[] jointIndices = input.getJointMatrixIndexProvider().getJointDoFIndices(childJoint); RecursionStep child = new RecursionStep(childBody, parent, jointIndices); rigidBodyToRecursionStepMap.put(childBody, child); buildMultiBodyTree(child, jointsToIgnore); } } }