int main(int argc, char* argv[])
{
  WorldPtr world = std::make_shared<World>();
  world->addSkeleton(createGround());
  world->addSkeleton(createWall());

  MyWindow window(world, createBall(), createSoftBody(), createHybridBody(),
                  createRigidChain(), createRigidRing());

  std::cout << "space bar: simulation on/off" << std::endl;
  std::cout << "'1': toss a rigid ball" << std::endl;
  std::cout << "'2': toss a soft body" << std::endl;
  std::cout << "'3': toss a hybrid soft/rigid body" << std::endl;
  std::cout << "'4': toss a rigid chain" << std::endl;
  std::cout << "'5': toss a ring of rigid bodies" << std::endl;

  std::cout << "\n'd': delete the oldest object" << std::endl;
  std::cout <<   "'r': toggle randomness" << std::endl;

  std::cout << "\nWarning: Let objects settle before tossing a new one, or the simulation could explode." << std::endl;
  std::cout <<   "         If the simulation freezes, you may need to force quit the application.\n" << std::endl;

  glutInit(&argc, argv);
  window.initWindow(640, 480, "Collisions");
  glutMainLoop();
}

//==============================================================================
TEST(SKEL_PARSER, JointActuatorType)
{
  WorldPtr world = SkelParser::readWorld(
      DART_DATA_PATH"/skel/test/joint_actuator_type_test.skel");
  EXPECT_TRUE(world != nullptr);

  SkeletonPtr skel1 = world->getSkeleton("skeleton 1");
  EXPECT_TRUE(skel1 != nullptr);

  // Test for no actuator type attribute being specified
  Joint* joint0 = skel1->getJoint("joint0");
  EXPECT_EQ(joint0->getActuatorType(), Joint::DefaultActuatorType);
  EXPECT_EQ(joint0->getActuatorType(), Joint::FORCE);

  // Test for when actuator type attribute are specified
  Joint* joint1 = skel1->getJoint("joint1");
  EXPECT_EQ(joint1->getActuatorType(), Joint::FORCE);

  // Test for only a dof name being changed
  Joint* joint2 = skel1->getJoint("joint2");
  EXPECT_EQ(joint2->getActuatorType(), Joint::PASSIVE);
  joint2->setActuatorType(Joint::FORCE);
  EXPECT_EQ(joint2->getActuatorType(), Joint::FORCE);

  // Test for when actuator type attribute are specified
  Joint* joint3 = skel1->getJoint("joint3");
  EXPECT_EQ(joint3->getActuatorType(), Joint::ACCELERATION);

  // Test for when actuator type attribute are specified
  Joint* joint4 = skel1->getJoint("joint4");
  EXPECT_EQ(joint4->getActuatorType(), Joint::VELOCITY);}

void AbstractGLDisplayHelper::initRenderingContext(WorldPtr world)
{
	int height = world->getHeight();
	int width = world->getWidth();
	int depth = world->getDepth();
	int pixelResolution = world->getPixelResolution();
	_renderContext = std::make_shared<OpenGLRenderContext>(width, height, depth, pixelResolution);
}

collision_detection::WorldDiff::WorldDiff(WorldDiff &other)
{
  WorldPtr world = other.world_.lock();
  if (world)
  {
    changes_ = other.changes_;

    boost::weak_ptr<World>(world).swap(world_);
    observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2));
  }
}

// Load a biped model and enable joint limits and self-collision
SkeletonPtr loadBiped()
{
  // Lesson 1

  // Create the world with a skeleton
  WorldPtr world = SkelParser::readWorld("dart://sample/skel/biped.skel");
  assert(world != nullptr);

  SkeletonPtr biped = world->getSkeleton("biped");

  return biped;
}

// Load a skateboard model and connect it to the biped model via an Euler joint
// (Lesson 5 Answer)
void modifyBipedWithSkateboard(SkeletonPtr biped)
{
  // Load the Skeleton from a file
  WorldPtr world = SkelParser::readWorld("dart://sample/skel/skateboard.skel");

  SkeletonPtr skateboard = world->getSkeleton(0);
  
  EulerJoint::Properties properties = EulerJoint::Properties();
  properties.mT_ChildBodyToJoint.translation() = Eigen::Vector3d(0, 0.1, 0);
  
  skateboard->getRootBodyNode()->moveTo<EulerJoint>
      (biped->getBodyNode("h_heel_left"), properties);
}

//==============================================================================
TEST(VskParser, EmptySkeleton)
{
  WorldPtr world = World::create();
  EXPECT_TRUE(world != nullptr);

  SkeletonPtr skeleton
      = VskParser::readSkeleton("dart://sample/vsk/test/empty.vsk");
  EXPECT_TRUE(skeleton == nullptr);

  world->addSkeleton(skeleton);
  EXPECT_EQ(world->getNumSkeletons(), 0u);

  world->step();
}

int main(int argc, char* argv[])
{
  SkeletonPtr floor = createFloor();

  // Lesson 1
  SkeletonPtr biped = loadBiped();

  // Lesson 2
  setInitialPose(biped);
  
  // Lesson 5
  modifyBipedWithSkateboard(biped);

  // Lesson 6
  setVelocityAccuators(biped);

  // Lesson 7
  Eigen::VectorXd balancedPose = solveIK(biped);
  biped->setPositions(balancedPose);
  
  WorldPtr world = std::make_shared<World>();
  world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0));

#if HAVE_BULLET_COLLISION
  world->getConstraintSolver()->setCollisionDetector(
      dart::collision::BulletCollisionDetector::create());
#endif
  
  world->addSkeleton(floor);
  world->addSkeleton(biped);
  
  // Create a window for rendering the world and handling user input
  MyWindow window(world);

  // Print instructions
  std::cout << "'.': forward push" << std::endl;
  std::cout << "',': backward push" << std::endl;
  std::cout << "'s': increase skateboard forward speed" << std::endl;
  std::cout << "'a': increase skateboard backward speed" << std::endl;
  std::cout << "space bar: simulation on/off" << std::endl;
  std::cout << "'p': replay simulation" << std::endl;
  std::cout << "'v': Turn contact force visualization on/off" << std::endl;
  std::cout <<
      "'[' and ']': replay one frame backward and forward" << std::endl;
 
  // Initialize glut, initialize the window, and begin the glut event loop
  glutInit(&argc, argv);
  window.initWindow(640, 480, "Multi-Pendulum Tutorial");
  glutMainLoop();
}

static WorldPtr createWorld()
{
    WorldPtr world = boost::make_shared<World>(500,500);

    world->addObstacle( boost::make_shared<SquareObstacle>(  0,  50, 400, 50) );
    world->addObstacle( boost::make_shared<SquareObstacle>(  0, 150, 200, 50) );
    world->addObstacle( boost::make_shared<SquareObstacle>(300, 150, 200, 50) );
    world->addObstacle( boost::make_shared<SquareObstacle>(100, 250, 400, 50) );
    world->addObstacle( boost::make_shared<SquareObstacle>(  0, 350, 250, 50) );
    world->addObstacle( boost::make_shared<SquareObstacle>(350, 350,  50, 50) );
    world->addObstacle( boost::make_shared<SquareObstacle>(150, 450, 350, 50) );

    return world;
}

// Load a biped model and enable joint limits and self-collision
// (Lesson 1 Answer)
SkeletonPtr loadBiped()
{
  // Create the world with a skeleton
  WorldPtr world = SkelParser::readWorld(DART_DATA_PATH"skel/biped.skel");
  assert(world != nullptr);

  SkeletonPtr biped = world->getSkeleton("biped");

  // Set joint limits
  for(size_t i = 0; i < biped->getNumJoints(); ++i)
    biped->getJoint(i)->setPositionLimitEnforced(true);
  
  // Enable self collision check but ignore adjacent bodies
  biped->enableSelfCollision();

  return biped;
}

std::vector<SkeletonPtr> getSkeletons()
{
  std::vector<std::string> fileList = getFileList();

  std::vector<WorldPtr> worlds;
  for(size_t i=0; i<fileList.size(); ++i)
    worlds.push_back(utils::SkelParser::readWorld(fileList[i]));

  std::vector<SkeletonPtr> skeletons;
  for(size_t i=0; i<worlds.size(); ++i)
  {
    WorldPtr world = worlds[i];
    for(size_t j=0; j<world->getNumSkeletons(); ++j)
      skeletons.push_back(world->getSkeleton(j));
  }

  return skeletons;
}

void GL2DDisplayHelper::paintSingleSolution(WorldPtr world, PartLocationListPtr parts)
{
	for (tuple<PartOrientationPtr, Point>& partLocation : *parts)
	{
		PartOrientationPtr currentOrientation = std::get<0>(partLocation);
		Point anchorPartPoint = std::get<1>(partLocation);

		RGB_COLOR orientationColor = _colorManager->getColor(*currentOrientation);
		float r = std::get<0>(orientationColor);
		float g = std::get<1>(orientationColor);
		float b = std::get<2>(orientationColor);
		float a;
		r /= 255.0;
		g /= 255.0;
		b /= 255.0;
		a = 1.0f;

		for (Point& relativePartCoordinate : *(currentOrientation->getPointList()))
		{
			Point point = anchorPartPoint + relativePartCoordinate;

			// Paint a square for each unmasked pixel in the world
			float v1x = point.getX();
			float v1y = world->getHeight() - point.getY() - 1; // OpenGL Y axis is inverted
			float v2x = v1x + SQUARE_SIZE;
			float v2y = v1y;
			float v3x = v1x;
			float v3y = v1y + SQUARE_SIZE;
			float v4x = v1x + SQUARE_SIZE;
			float v4y = v1y + SQUARE_SIZE;

			_renderContext->write2DTriangle(v1x, v1y, v2x, v2y, v3x, v3y, r, g, b, a);
			_renderContext->write2DTriangle(v2x, v2y, v3x, v3y, v4x, v4y, r, g, b, a);

			if (currentOrientation->isContainsPointAbovePoint(relativePartCoordinate))
				_renderContext->writeLine(v1x, v1y, v2x, v2y);
			else
				_renderContext->writeBoldLine(v1x, v1y, v2x, v2y);

			if (currentOrientation->isContainsPointRightOfPoint(relativePartCoordinate))
				_renderContext->writeLine(v2x, v2y, v4x, v4y);
			else
				_renderContext->writeBoldLine(v2x, v2y, v4x, v4y);

			if (currentOrientation->isContainsPointBelowPoint(relativePartCoordinate))
				_renderContext->writeLine(v3x, v3y, v4x, v4y);
			else
				_renderContext->writeBoldLine(v3x, v3y, v4x, v4y);

			if (currentOrientation->isContainsPointLeftPoint(relativePartCoordinate))
				_renderContext->writeLine(v3x, v3y, v1x, v1y);
			else
				_renderContext->writeBoldLine(v3x, v3y, v1x, v1y);
		}
	}
}

  SimulationWorld(WorldPtr world, const std::vector<Eigen::VectorXd>& trajectory)
    : osgDart::WorldNode(world), mTrajectory(trajectory)
  {
    mHubo = world->getSkeleton("drchubo");

    double height = mHubo->getPosition(5);
    mHubo->setPositions(mTrajectory[0]);
    mHubo->setPosition(5, height);
//    index = 0;
  }

/// Create a new World instance.
///
/// @param[in] pSceneDefinition  The SceneDefinition from which to create the new World.
///
/// @return  Newly created world instance.
Helium::World* WorldManager::CreateWorld( SceneDefinition* pSceneDefinition )
{
	// Any scene that creates a world must define a world definition so that the world knows what components to init on itself
	HELIUM_ASSERT(pSceneDefinition);

	const WorldDefinition *pWorldDefinition = pSceneDefinition->GetWorldDefinition();

	WorldPtr spWorld;
	if (pWorldDefinition)
	{
		// Let the world definition provide the world
		spWorld = pWorldDefinition->CreateWorld();
	}
	else
	{
		// Make a blank, default world.
		// TODO: Consider having the concept of a "default" world definition. Generally, not loading a world from a definition
		// is bad because pretty much any useful world should be initialized with world components
		HELIUM_ASSERT( 0 );
		spWorld = new World();
		spWorld->Initialize();
	}

	HELIUM_ASSERT(spWorld.Get());

	m_worlds.Push( spWorld );

	if ( pSceneDefinition && spWorld )
	{
		Slice *pRootSlice = spWorld->GetRootSlice();
		size_t entityDefinitionCount = pSceneDefinition->GetEntityDefinitionCount();

		for ( size_t i = 0; i < entityDefinitionCount; ++i )
		{
			EntityDefinition *pEntityDefinition = pSceneDefinition->GetEntityDefinition( i );
			HELIUM_ASSERT( pEntityDefinition );
			Entity *pEntity = pRootSlice->CreateEntity(pEntityDefinition);
		}
	}

	return spWorld;
}

void collision_detection::WorldDiff::reset(const WorldPtr& world)
{
  clearChanges();

  WorldPtr old_world = world_.lock();
  if (old_world)
    old_world->removeObserver(observer_handle_);

  boost::weak_ptr<World>(world).swap(world_);
  observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2));
}

//==============================================================================
TEST(SdfParser, SDFSingleBodyWithoutJoint)
{
  // Regression test for #444
  WorldPtr world
      = SdfParser::readSdfFile(
            DART_DATA_PATH"/sdf/test/single_bodynode_skeleton.world");
  EXPECT_TRUE(world != nullptr);

  SkeletonPtr skel = world->getSkeleton(0);
  EXPECT_TRUE(skel != nullptr);
  EXPECT_EQ(skel->getNumBodyNodes(), 1u);
  EXPECT_EQ(skel->getNumJoints(), 1u);

  BodyNodePtr bodyNode = skel->getBodyNode(0);
  EXPECT_TRUE(bodyNode != nullptr);
  EXPECT_EQ(bodyNode->getNumVisualizationShapes(), 1u);
  EXPECT_EQ(bodyNode->getNumCollisionShapes(), 1u);

  JointPtr joint = skel->getJoint(0);
  EXPECT_TRUE(joint != nullptr);
  EXPECT_EQ(joint->getType(), FreeJoint::getStaticType());
}

  /// Constructor
  MyWindow(WorldPtr world)
    : mBallConstraint(nullptr),
      mPositiveSign(true),
      mBodyForce(false)
  {
    setWorld(world);

    // Find the Skeleton named "pendulum" within the World
    mPendulum = world->getSkeleton("pendulum");

    // Make sure that the pendulum was found in the World
    assert(mPendulum != nullptr);

    mForceCountDown.resize(mPendulum->getNumDofs(), 0);

    ArrowShape::Properties arrow_properties;
    arrow_properties.mRadius = 0.05;
    mArrow = std::shared_ptr<ArrowShape>(new ArrowShape(
             Eigen::Vector3d(-default_height, 0.0, default_height / 2.0),
             Eigen::Vector3d(-default_width / 2.0, 0.0, default_height / 2.0),
             arrow_properties, dart::Color::Orange(1.0)));
  }

//==============================================================================
TEST(FileInfoWorld, Basic)
{
  const size_t numFrames = 100;
  const std::string fileName = "testWorld.txt";
  double tol = 1e-6;
  bool result = false;
  FileInfoWorld worldFile;

  WorldPtr world = SkelParser::readWorld(
                   DART_DATA_PATH"/skel/test/file_info_world_test.skel");
  EXPECT_TRUE(world != nullptr);

  Recording* recording1 = nullptr;
  Recording* recording2 = nullptr;

  // Do some simulation with recording
  for (size_t i = 0; i < numFrames; ++i)
  {
    world->step();
    world->bake();
  }

  recording1 = world->getRecording();
  EXPECT_TRUE(recording1 != nullptr);

  // Save the recording to a file
  result = worldFile.saveFile(fileName.c_str(), world->getRecording());
  EXPECT_TRUE(result);

  // Load the file
  result = worldFile.loadFile(fileName.c_str());
  EXPECT_TRUE(result);
  recording2 = worldFile.getRecording();
  EXPECT_TRUE(recording2 != nullptr);

  // Check number of frames
  EXPECT_EQ(recording1->getNumFrames(), (int)numFrames);
  EXPECT_EQ(recording2->getNumFrames(), (int)numFrames);

  // Check number of skeletons
  size_t numSkeletons = recording1->getNumSkeletons();
  EXPECT_EQ(recording1->getNumSkeletons(), recording2->getNumSkeletons());

  // Check number of dofs of the skeletons
  for (size_t i = 0; i < numSkeletons; ++i)
    EXPECT_EQ(recording1->getNumDofs(i), recording2->getNumDofs(i));

  // Check generalized positions and contact info
  for (size_t i = 0; i < numFrames; ++i)
  {
    // Check generalized positions
    for (size_t j = 0; j < numSkeletons; ++j)
    {
      size_t dofs = recording1->getNumDofs(j);

      for (size_t k = 0; k < dofs; ++k)
      {
        EXPECT_NEAR(recording1->getGenCoord(i, j, k),
                    recording2->getGenCoord(i, j, k), tol);
      }
    }

    // Check contact info
    tol = 1e-3;
    size_t numContacts = recording1->getNumContacts(i);
    EXPECT_EQ(recording1->getNumContacts(i), recording2->getNumContacts(i));

    for (size_t j = 0; j < numContacts; ++j)
    {
      for (size_t k = 0; k < 3; ++k)
      {
        EXPECT_NEAR(recording1->getContactForce(i, j)[k],
                    recording2->getContactForce(i, j)[k], tol);

        EXPECT_NEAR(recording1->getContactPoint(i, j)[k],
                    recording2->getContactPoint(i, j)[k], tol);
      }
    }
  }
}

//==============================================================================
void ConstraintTest::SingleContactTest(const std::string& /*_fileName*/)
{
  using namespace std;
  using namespace Eigen;
  using namespace dart::math;
  using namespace dart::collision;
  using namespace dart::constraint;
  using namespace dart::dynamics;
  using namespace dart::simulation;
  using namespace dart::io;

  //----------------------------------------------------------------------------
  // Settings
  //----------------------------------------------------------------------------
  // Number of random state tests for each skeletons
#ifndef NDEBUG  // Debug mode
  // std::size_t testCount = 1;
#else
  // std::size_t testCount = 1;
#endif

  WorldPtr world = World::create();
  EXPECT_TRUE(world != nullptr);
  world->setGravity(Vector3d(0.0, -10.00, 0.0));
  world->setTimeStep(0.001);
  world->getConstraintSolver()->setCollisionDetector(
        DARTCollisionDetector::create());

  SkeletonPtr sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0));
  BodyNode* sphere = sphereSkel->getBodyNode(0);
  Joint* sphereJoint = sphere->getParentJoint();
  sphereJoint->setVelocity(3, Random::uniform(-2.0, 2.0));  // x-axis
  sphereJoint->setVelocity(5, Random::uniform(-2.0, 2.0));  // z-axis
  world->addSkeleton(sphereSkel);
  EXPECT_EQ(sphereSkel->getGravity(), world->getGravity());
  assert(sphere);

  SkeletonPtr boxSkel = createBox(Vector3d(1.0, 1.0, 1.0),
                                  Vector3d(0.0, 1.0, 0.0));
  BodyNode* box = boxSkel->getBodyNode(0);
  Joint* boxJoint = box->getParentJoint();
  boxJoint->setVelocity(3, Random::uniform(-2.0, 2.0));  // x-axis
  boxJoint->setVelocity(5, Random::uniform(-2.0, 2.0));  // z-axis
//  world->addSkeleton(boxSkel);
//  EXPECT_EQ(boxSkel->getGravity(), world->getGravity());
//  assert(box);

  SkeletonPtr groundSkel = createGround(Vector3d(10000.0, 0.1, 10000.0),
                                      Vector3d(0.0, -0.05, 0.0));
  groundSkel->setMobile(false);
  // BodyNode* ground = groundSkel->getBodyNode(0);
  world->addSkeleton(groundSkel);
  EXPECT_EQ(groundSkel->getGravity(), world->getGravity());
  // assert(ground);

  EXPECT_EQ((int)world->getNumSkeletons(), 2);

  // Lower and upper bound of configuration for system
  // double lb = -1.5 * constantsd::pi();
  // double ub =  1.5 * constantsd::pi();

  int maxSteps = 500;
  for (int i = 0; i < maxSteps; ++i)
  {
//    Vector3d pos1 = sphere->getWorldTransform().translation();
//    Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);

//    std::cout << "pos1:" << pos1.transpose() << std::endl;
//    std::cout << "vel1:" << vel1.transpose() << std::endl;

    if (!world->checkCollision())
    {
      world->step();
      continue;
    }

    // for (std::size_t j = 0; j < cd->getNumContacts(); ++j)
    // {
      // Contact contact = cd->getContact(j);
      // Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
      // Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);

      // std::cout << "pos1:" << pos1.transpose() << std::endl;
      // std::cout << "vel1:" << vel1.transpose() << std::endl;
    // }

    world->step();

    const auto& result = world->getConstraintSolver()->getLastCollisionResult();

    for (const auto& contact : result.getContacts())
    {
      Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
      Vector3d vel1 = sphere->getLinearVelocity(pos1);

//      std::cout << "pos1:" << pos1.transpose() << std::endl;

//      std::cout << "pos1[1]: " << pos1[1] << std::endl;

//.........这里部分代码省略.........