void CollisionInterface::postProcess() {
    mContacts.clear();
    int numContacts = mCollisionChecker->getNumContacts();
    mContacts.resize(numContacts);
    for (int i = 0; i < numContacts; i++) {
        mContacts[i].point = mCollisionChecker->getContact(i).point;
        mContacts[i].normal = mCollisionChecker->getContact(i).normal;
        mContacts[i].rb1 = mNodeMap[mCollisionChecker->getContact(i).bodyNode1];
        mContacts[i].rb2 = mNodeMap[mCollisionChecker->getContact(i).bodyNode2];
        if(mContacts[i].rb1 == NULL) {
          BodyNode* bd = mCollisionChecker->getContact(i).bodyNode1;
          Vector3d localPoint = bd->getTransform().inverse() * mContacts[i].point;
          mContacts[i].pinataVelocity = bd->getWorldLinearVelocity(localPoint);
        } else if(mContacts[i].rb2 == NULL) {
          BodyNode* bd = mCollisionChecker->getContact(i).bodyNode2;
          Vector3d localPoint = bd->getTransform().inverse() * mContacts[i].point;
          mContacts[i].pinataVelocity = bd->getWorldLinearVelocity(localPoint);
        } else {
          mContacts[i].pinataVelocity.setZero();
        }        
    }
}
Esempio n. 2
0
//==============================================================================
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::utils;

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

  World* world = new World;
  EXPECT_TRUE(world != NULL);
  world->setGravity(Vector3d(0.0, -10.00, 0.0));
  world->setTimeStep(0.001);
  world->getConstraintSolver()->setCollisionDetector(
        new DARTCollisionDetector());

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

  Skeleton* 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(-2.0, 2.0));  // x-axis
  boxJoint->setVelocity(5, random(-2.0, 2.0));  // z-axis
//  world->addSkeleton(boxSkel);
//  EXPECT_EQ(boxSkel->getGravity(), world->getGravity());
//  assert(box);

  Skeleton* 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(world->getNumSkeletons(), 2);

  ConstraintSolver* cs = world->getConstraintSolver();
  CollisionDetector* cd = cs->getCollisionDetector();

  // Lower and upper bound of configuration for system
  // double lb = -1.5 * DART_PI;
  // double ub =  1.5 * DART_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;

    cd->detectCollision(true, true);
    if (cd->getNumContacts() == 0)
    {
      world->step();
      continue;
    }

    // for (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();

    for (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 << "pos1[1]: " << pos1[1] << std::endl;

//      std::cout << "pos1:" << pos1.transpose() << std::endl;
      std::cout << "vel1:" << vel1.transpose() << ", pos1[1]: " << pos1[1] << std::endl;

//      EXPECT_NEAR(pos1[0], 0.0, 1e-9);
//      EXPECT_NEAR(pos1[1], -0.05, 1e-2);
//      EXPECT_NEAR(pos1[2], 0.0, 1e-9);

//      EXPECT_NEAR(vel1[0], 0.0, 1e-9);
//      EXPECT_NEAR(vel1[1], 0.0, 1e-9);
//      EXPECT_NEAR(vel1[2], 0.0, 1e-9);

//      if (!equals(vel1, Vector3d(0.0, 0.0, 0.0)))
//        std::cout << "vel1:" << vel1.transpose() << std::endl;

//      EXPECT_EQ(vel1, Vector3d::Zero());
    }

//    std::cout << std::endl;

    break;
  }

  delete world;
}