bool testClosedKinematicLoop2()
{
  SharedPtr<Group> group = new Group("Group");

  MobileRootJoint* mechanicRootJoint = new MobileRootJoint("RootJoint");
  group->addChild(mechanicRootJoint);

  RigidBody* rigidBody = new RigidBody("RigidBody");
  group->addChild(rigidBody);
  group->connect(mechanicRootJoint->getPort("link"),
                 rigidBody->addLink("rootLink"));

  PrismaticJoint* prismaticJoint = new PrismaticJoint("PrismaticJoint");
  group->addChild(prismaticJoint);
  group->connect(rigidBody->addLink("jointLink0"),
                 prismaticJoint->getPort("link0"));
  group->connect(rigidBody->addLink("jointLink1"),
                 prismaticJoint->getPort("link1"));

  SharedPtr<System> system = new System("Closed kinematic Loop 1");
  system->setNode(group);

  if (system->init()) {
    std::cerr << "Detection of klosed kinematic loop failed!" << std::endl;
    return false;
  }
  return true;
}
bool testBodyTreeModel1(unsigned numLevels, unsigned numBodies)
{
  SharedPtr<Group> group = new Group("Group");

  MobileRootJoint* mechanicRootJoint = new MobileRootJoint("RootJoint");
  group->addChild(mechanicRootJoint);

  RigidBody* rigidBody = new RigidBody("RigidBody");
  group->addChild(rigidBody);
  group->connect(mechanicRootJoint->getPort("link"),
                 rigidBody->addLink("rootLink"));
  
  Mass* mass = new Mass("Mass", 1, InertiaMatrix(1, 0, 0, 1, 0, 1));
  group->addChild(mass);
  group->connect(mass->getPort("link"), rigidBody->addLink("massLink"));

  addBodiesRecursive(numLevels, numBodies, rigidBody, group);

  SharedPtr<System> system = new System("Closed kinematic Loop 1");
  system->setNode(group);

  if (!system->init()) {
    std::cerr << "Initialization of single body model failed!" << std::endl;
    return false;
  }
  return true;
}
Node* buildDrop()
{
  // A simple free falling mass.
  SharedPtr<Group> group = new Group("Group");

  MobileRootJoint* mobileRootJoint = new MobileRootJoint("Root Joint");
  mobileRootJoint->setInitialPosition(Vector3(0, 0, -10000));
  mobileRootJoint->setInitialAngularVelocity(Vector3(1, 1, 1));
  group->addChild(mobileRootJoint);

  RigidBody* rigidBody = new RigidBody("Rigid Body");
  rigidBody->addLink("externalInteractLink");
  group->addChild(rigidBody);

  Mass* mass = new Mass("Mass", 1, InertiaMatrix(1, 0, 0, 1, 0, 1));
  group->addChild(mass);

  ExternalInteract* externalInteract = new ExternalInteract("ExternalInteract");
  externalInteract->setPosition(mass->getPosition());
  externalInteract->setEnableAllOutputs(true);
  group->addChild(externalInteract);

  group->connect(mobileRootJoint->getPort("link"),
                 rigidBody->getPort("link0"));
  group->connect(rigidBody->getPort("link1"),
                 mass->getPort("link"));
  group->connect(rigidBody->getPort("externalInteractLink"),
                 externalInteract->getPort("link"));

  return group.release();
}
Node* buildSimpleMechanicExample()
{
  SharedPtr<Group> group = new Group("G");

  MobileRootJoint* mobileRootJoint = new MobileRootJoint("Root Joint");
  group->addChild(mobileRootJoint);

  RigidBody *rigidBody = new RigidBody("Rigid Body");
  rigidBody->addLink("link2");
  rigidBody->addLink("externalInteractLink");
  rigidBody->addLink("internalInteractLink");
  rigidBody->addLink("internalInteractLink2");
  group->addChild(rigidBody);
  InertiaMatrix inertia(1, 0, 0, 1, 0, 1);
  Mass* mass = new Mass("Mass", 1, inertia);
  group->addChild(mass);
  RevoluteJoint* revoluteJoint = new RevoluteJoint("Revolute Joint");
  revoluteJoint->setEnableExternalForce(true);
  group->addChild(revoluteJoint);
  RigidBody *rigidBody2 = new RigidBody("Rigid Body 2");
  rigidBody2->addLink("externalInteractLink");
  rigidBody2->addLink("internalInteractLink");
  rigidBody2->addLink("internalInteractLink2");
  group->addChild(rigidBody2);
  Mass* mass2 = new Mass("Mass 2", 1, inertia);
  group->addChild(mass2);

  ExternalInteract* externalInteract = new ExternalInteract("ExternalInteract");
  externalInteract->setPosition(mass->getPosition());
  externalInteract->setEnableAllOutputs(true);
  group->addChild(externalInteract);

  ExternalInteract* externalInteract2 = new ExternalInteract("ExternalInteract 2");
  externalInteract2->setPosition(mass2->getPosition());
  externalInteract2->setEnableAllOutputs(true);
  group->addChild(externalInteract2);

  group->connect(mobileRootJoint->getPort("link"), rigidBody->getPort("link0"));
  group->connect(rigidBody->getPort("link1"), mass->getPort("link"));
  group->connect(rigidBody->getPort("link2"), revoluteJoint->getPort("link0"));
  group->connect(revoluteJoint->getPort("link1"), rigidBody2->getPort("link0"));
  group->connect(rigidBody2->getPort("link1"), mass2->getPort("link"));
  group->connect(rigidBody->getPort("externalInteractLink"), externalInteract->getPort("link"));
  group->connect(rigidBody2->getPort("externalInteractLink"), externalInteract2->getPort("link"));

  ConstModel* jointForce = new ConstModel("Joint Force", 1);
  group->addChild(jointForce);

  group->connect(jointForce->getPort("output"),
                 revoluteJoint->getPort("force"));

  InternalInteract* internalInteract = new InternalInteract("InternalInteract");
  internalInteract->setPosition0(Vector3(0, 0, 1));
  internalInteract->setPosition1(Vector3(0, 0, 0.8));
  internalInteract->setEnableAllOutputs(true);
  internalInteract->setEnableForce(true);
  group->addChild(internalInteract);
  group->connect(internalInteract->getPort("link0"),
                 rigidBody->getPort("internalInteractLink"));
  group->connect(internalInteract->getPort("link1"),
                 rigidBody2->getPort("internalInteractLink"));


  InternalInteract* internalInteract2 = new InternalInteract("InternalInteract2");
  internalInteract2->setPosition0(Vector3(0, 0, 0.8));
  internalInteract2->setPosition1(Vector3(0, 0, 1));
  internalInteract2->setEnableAllOutputs(true);
  group->addChild(internalInteract2);
  group->connect(internalInteract2->getPort("link1"),
                 rigidBody->getPort("internalInteractLink2"));
  group->connect(internalInteract2->getPort("link0"),
                 rigidBody2->getPort("internalInteractLink2"));

  LinearSpringDamper* damper = new LinearSpringDamper("LinearSpringDamper");
  damper->setSpringConstant(0.5);
  damper->setDamperConstant(1);
  group->addChild(damper);
  group->connect(damper->getPort("velocity"),
                 internalInteract->getPort("velocity"));
  group->connect(damper->getPort("position"),
                 internalInteract->getPort("distance"));
  group->connect(damper->getPort("force"),
                 internalInteract->getPort("force"));

  return group.release();
}