public JointAxisVisualizer(RigidBodyBasics rootBody, YoGraphicsListRegistry yoGraphicsListRegistry, double length)
{
YoGraphicsList yoGraphicsList = new YoGraphicsList(name);
List<JointBasics> jointStack = new ArrayList<JointBasics>(rootBody.getChildrenJoints());
while (!jointStack.isEmpty())
{
JointBasics joint = jointStack.get(0);
if(joint instanceof OneDoFJointBasics)
{
FrameVector3DReadOnly jAxis=((OneDoFJointBasics)joint).getJointAxis();
ReferenceFrame referenceFrame = GeometryTools.constructReferenceFrameFromPointAndZAxis(joint.getName()+"JointAxis", new FramePoint3D(jAxis.getReferenceFrame()), new FrameVector3D(jAxis.getReferenceFrame(),jAxis));
YoGraphicReferenceFrame yoGraphicReferenceFrame = new YoGraphicReferenceFrame(referenceFrame , registry, false, length, YoAppearance.Gold());
yoGraphicsList.add(yoGraphicReferenceFrame);
yoGraphicReferenceFrames.add(yoGraphicReferenceFrame);
}
List<? extends JointBasics> childrenJoints = joint.getSuccessor().getChildrenJoints();
jointStack.addAll(childrenJoints);
jointStack.remove(joint);
}
yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList);
}