コード例 #1
0
/*! This is a helper function for grasp force optimization routines. Given a
	set of contacts, and a matrix of contact wrenches expressed in *world*
	coordinates and relative to object origin (as computed in gfo routines)
	this function will convert them to object coordinate system and accumulate
	them in the object's external wrench accumulator. This can serve as an
	output of the gfo functions and also for rendering purposes, as the 
	external wrench accumulator can then be rendered.
*/
void
Grasp::accumulateAndDisplayObjectWrenches(std::list<Contact*> *contacts, 
										  const Matrix &objectWrenches)
{
	int count = 0;
	std::list<Contact*>::iterator it;
	for (it=contacts->begin(); it!=contacts->end(); it++, count++) {
		Contact *contact = *it;
		vec3 force(objectWrenches.elem(6*count+0,0), 
					objectWrenches.elem(6*count+1,0), 
					objectWrenches.elem(6*count+2,0));
		vec3 torque(objectWrenches.elem(6*count+3,0), 
					objectWrenches.elem(6*count+4,0), 
					objectWrenches.elem(6*count+5,0));
		if (contact->getBody2()->isDynamic()) {
			DynamicBody *object = (DynamicBody*)(contact->getBody2());
			//compute force and torque in body reference frame
			//and scale them down for now for rendering and output purposes
			force = 1.0e-6 * force * object->getTran().inverse();
			//torque is also scaled by maxRadius in conversion matrix
			torque = 1.0e-6 * torque * object->getTran().inverse();
			//accumulate them on object
			object->addForce(force);
			object->addTorque(torque);
		}
	}
}
コード例 #2
0
ファイル: Space.cpp プロジェクト: adammead/pioneer
// temporary one-point version
static void CollideWithTerrain(Body *body)
{
    if (!body->IsType(Object::DYNAMICBODY)) return;
    DynamicBody *dynBody = static_cast<DynamicBody*>(body);
    if (!dynBody->IsMoving()) return;

    Frame *f = body->GetFrame();
    if (!f || !f->GetBody() || f != f->GetBody()->GetFrame()) return;
    if (!f->GetBody()->IsType(Object::TERRAINBODY)) return;
    TerrainBody *terrain = static_cast<TerrainBody*>(f->GetBody());

    const Aabb &aabb = dynBody->GetAabb();
    double altitude = body->GetPosition().Length() + aabb.min.y;
    if (altitude >= terrain->GetMaxFeatureRadius()) return;

    double terrHeight = terrain->GetTerrainHeight(body->GetPosition().Normalized());
    if (altitude >= terrHeight) return;

    CollisionContact c;
    c.pos = body->GetPosition();
    c.normal = c.pos.Normalized();
    c.depth = terrHeight - altitude;
    c.userData1 = static_cast<void*>(body);
    c.userData2 = static_cast<void*>(f->GetBody());
    hitCallback(&c);
}
コード例 #3
0
int createEntityWithSize(const char* entityResource, LuaPlus::LuaObject luaPosition, LuaPlus::LuaObject luaSize)
{
	if(!luaPosition.IsTable())
	{
		CORE_LOG("LUA", "Invalid position object passed to create_entity.");
		return INVALID_ENTITY_ID;
	}

	if(!luaSize.IsTable())
	{
		CORE_LOG("LUA", "Invalid size object passed to create_entity.");
		return INVALID_ENTITY_ID;
	}

	sf::Vector2f position = tableToVec2<sf::Vector2f>(luaPosition);
	sf::Vector2f size = tableToVec2<sf::Vector2f>(luaSize);

	artemis::Entity& entity = WorldLocator::getObject()->createEntity();

	// Create entity
	if(!EntityFactory::get().loadFromFile(entityResource, entity))
	{
		CORE_ERROR("Failed to load entity from file: " + std::string(entityResource));
		return INVALID_ENTITY_ID;
	}

	Transform* pTransform = safeGetComponent<Transform>(&entity);

	if(!pTransform)
	{
		entity.addComponent(new Transform(position.x, position.y));
	}
	else 
	{
		pTransform->position = position;
	}

	// Get physics comp
	DynamicBody* pDynamicBody = safeGetComponent<DynamicBody>(&entity);

	if(pDynamicBody)
	{
		// Set dimensions, will be initialized later
		pDynamicBody->setDimensions(size.x, size.y);
	}
	else
	{
		entity.addComponent(new DynamicBody(size.x, size.y));
	}

	// commit entity changes
	entity.refresh();

	CORE_LOG("LUA", "Created entity from: " + std::string(entityResource));

	return entity.getId();
}
コード例 #4
0
void Space::CollideFrame(Frame *f)
{
	if (f->m_astroBody && (f->m_astroBody->IsType(Object::TERRAINBODY))) {
		// this is pretty retarded
		for (BodyIterator i = m_bodies.begin(); i!=m_bodies.end(); ++i) {
			if ((*i)->GetFrame() != f) continue;
			if (!(*i)->IsType(Object::DYNAMICBODY)) continue;
			DynamicBody *dynBody = static_cast<DynamicBody*>(*i);

			Aabb aabb;
			dynBody->GetAabb(aabb);
			const matrix4x4d &trans = dynBody->GetGeom()->GetTransform();

			const vector3d aabbCorners[8] = {
				vector3d(aabb.min.x, aabb.min.y, aabb.min.z),
				vector3d(aabb.min.x, aabb.min.y, aabb.max.z),
				vector3d(aabb.min.x, aabb.max.y, aabb.min.z),
				vector3d(aabb.min.x, aabb.max.y, aabb.max.z),
				vector3d(aabb.max.x, aabb.min.y, aabb.min.z),
				vector3d(aabb.max.x, aabb.min.y, aabb.max.z),
				vector3d(aabb.max.x, aabb.max.y, aabb.min.z),
				vector3d(aabb.max.x, aabb.max.y, aabb.max.z)
			};

			CollisionContact c;

			for (int j=0; j<8; j++) {
				const vector3d &s = aabbCorners[j];
				vector3d pos = trans * s;
				double terrain_height = static_cast<Planet*>(f->m_astroBody)->GetTerrainHeight(pos.Normalized());
				double altitude = pos.Length();
				double hitDepth = terrain_height - altitude;
				if (altitude < terrain_height) {
					c.pos = pos;
					c.normal = pos.Normalized();
					c.depth = hitDepth;
					c.userData1 = static_cast<void*>(dynBody);
					c.userData2 = static_cast<void*>(f->m_astroBody);
					hitCallback(&c);
				}
			}
		}
	}
	f->GetCollisionSpace()->Collide(&hitCallback);
	for (std::list<Frame*>::iterator i = f->m_children.begin(); i != f->m_children.end(); ++i) {
		CollideFrame(*i);
	}
}
コード例 #5
0
ファイル: graspitDynamics.cpp プロジェクト: iretiayo/graspit
/*! Asks the dynamics engine to compute the velocities of all bodies at
the current time step. These will be used in the next time step when
the bodies are moved by World::moveDynamicBodies.

The bodies are separated into "islands" of bodies connected by contacts
or joints.  Two dynamic bodies are connected if they share a contact or
a joint.  Then for each island, this calls the iterate dynamics routine
to build and solve the LCP to find the velocities of all the bodies
in the next iteration.
*/
int GraspitDynamics::computeNewVelocities(double timeStep) {
  bool allDynamicsComputed;
  static std::list<Contact *> contactList;
  std::list<Contact *>::iterator cp;
  std::vector<DynamicBody *> robotLinks;
  std::vector<DynamicBody *> dynIsland;
  std::vector<Robot *> islandRobots;
  int i, j, numLinks, numDynBodies, lemkeErrCode;

  int numBodies = mWorld->getNumBodies();

#ifdef GRASPITDBG
  int islandCount = 0;
#endif

  do {
    // seed the island with one dynamic body
    for (i = 0; i < numBodies; i++)
      if (mWorld->getBody(i)->isDynamic() &&
          !((DynamicBody *)mWorld->getBody(i))->dynamicsComputed()) {
        // if this body is a link, add all robots connected to the link
        if (mWorld->getBody(i)->inherits("Link")) {
          Robot *robot = ((Robot *)((Link *)mWorld->getBody(i))->getOwner())->getBaseRobot();
          robot->getAllLinks(dynIsland);
          robot->getAllAttachedRobots(islandRobots);
        } else {
          dynIsland.push_back((DynamicBody *)mWorld->getBody(i));
        }
        break;
      }
    numDynBodies = dynIsland.size();
    for (i = 0; i < numDynBodies; i++) {
      dynIsland[i]->setDynamicsFlag();
    }

    // add any bodies that contact any body already in the dynamic island
    for (i = 0; i < numDynBodies; i++) {
      contactList = dynIsland[i]->getContacts();
      for (cp = contactList.begin(); cp != contactList.end(); cp++) {
        // if the contacting body is dynamic and not already in the list, add it
        if ((*cp)->getBody2()->isDynamic() &&
            !((DynamicBody *)(*cp)->getBody2())->dynamicsComputed()) {
          DynamicBody *contactedBody = (DynamicBody *)(*cp)->getBody2();

          // is this body is a link, add all robots connected to the link
          if (contactedBody->isA("Link")) {
            Robot *robot = ((Robot *)((Link *)contactedBody)->getOwner())->getBaseRobot();
            robot->getAllLinks(robotLinks);
            robot->getAllAttachedRobots(islandRobots);
            numLinks = robotLinks.size();
            for (j = 0; j < numLinks; j++)
              if (!robotLinks[j]->dynamicsComputed()) {
                dynIsland.push_back(robotLinks[j]);
                robotLinks[j]->setDynamicsFlag();
                numDynBodies++;
              }
            robotLinks.clear();
          } else {
            dynIsland.push_back(contactedBody);
            contactedBody->setDynamicsFlag();
            numDynBodies++;
          }
        }
      }
    }



#ifdef GRASPITDBG
    int numIslandRobots = islandRobots.size();
    std::cout << "Island " << ++islandCount << " Bodies: ";
    for (i = 0; i < numDynBodies; i++) {
      std::cout << dynIsland[i]->getName().toStdString().c_str() << " ";
    }
    std::cout << std::endl;
    std::cout << "Island Robots" << islandCount << " Robots: ";
    for (i = 0; i < numIslandRobots; i++) {
      std::cout << islandRobots[i]->getName().toStdString().c_str() << " ";
    }
    std::cout << std::endl << std::endl;
#endif

    for (i = 0; i < numDynBodies; i++) {
      dynIsland[i]->markState();
    }

    DynamicParameters dp;
    if (numDynBodies > 0) {
      dp.timeStep = timeStep;
      dp.useContactEps = true;
      dp.gravityMultiplier = 1.0;
      lemkeErrCode = iterateDynamics(islandRobots, dynIsland, &dp);

      if (lemkeErrCode == 1) { // dynamics could not be solved
        std::cerr << "LCP COULD NOT BE SOLVED!" << std::endl << std::endl;
        turnOffDynamics();
        return -1;
      }
    }

    dynIsland.clear();
    islandRobots.clear();
    allDynamicsComputed = true;
    for (i = 0; i < numBodies; i++)
      if (mWorld->getBody(i)->isDynamic() &&
          !((DynamicBody *)mWorld->getBody(i))->dynamicsComputed()) {
        allDynamicsComputed = false;
        break;
      }
  }  while (!allDynamicsComputed);

  // clear all the dynamicsComputed flags
  for (i = 0; i < numBodies; i++)
    if (mWorld->getBody(i)->isDynamic()) {
      ((DynamicBody *)mWorld->getBody(i))->resetDynamicsFlag();
    }

  mWorld->emitDynamicStepTaken();
  return 0;
}
コード例 #6
0
ファイル: Space.cpp プロジェクト: adammead/pioneer
static void hitCallback(CollisionContact *c)
{
    //printf("OUCH! %x (depth %f)\n", SDL_GetTicks(), c->depth);

    Object *po1 = static_cast<Object*>(c->userData1);
    Object *po2 = static_cast<Object*>(c->userData2);

    const bool po1_isDynBody = po1->IsType(Object::DYNAMICBODY);
    const bool po2_isDynBody = po2->IsType(Object::DYNAMICBODY);
    // collision response
    assert(po1_isDynBody || po2_isDynBody);

    if (po1_isDynBody && po2_isDynBody) {
        DynamicBody *b1 = static_cast<DynamicBody*>(po1);
        DynamicBody *b2 = static_cast<DynamicBody*>(po2);
        const vector3d linVel1 = b1->GetVelocity();
        const vector3d linVel2 = b2->GetVelocity();
        const vector3d angVel1 = b1->GetAngVelocity();
        const vector3d angVel2 = b2->GetAngVelocity();

        const double coeff_rest = 0.5;
        // step back
//		mover->UndoTimestep();

        const double invMass1 = 1.0 / b1->GetMass();
        const double invMass2 = 1.0 / b2->GetMass();
        const vector3d hitPos1 = c->pos - b1->GetPosition();
        const vector3d hitPos2 = c->pos - b2->GetPosition();
        const vector3d hitVel1 = linVel1 + angVel1.Cross(hitPos1);
        const vector3d hitVel2 = linVel2 + angVel2.Cross(hitPos2);
        const double relVel = (hitVel1 - hitVel2).Dot(c->normal);
        // moving away so no collision
        if (relVel > 0) return;
        if (!OnCollision(po1, po2, c, -relVel)) return;
        const double invAngInert1 = 1.0 / b1->GetAngularInertia();
        const double invAngInert2 = 1.0 / b2->GetAngularInertia();
        const double numerator = -(1.0 + coeff_rest) * relVel;
        const double term1 = invMass1;
        const double term2 = invMass2;
        const double term3 = c->normal.Dot((hitPos1.Cross(c->normal)*invAngInert1).Cross(hitPos1));
        const double term4 = c->normal.Dot((hitPos2.Cross(c->normal)*invAngInert2).Cross(hitPos2));

        const double j = numerator / (term1 + term2 + term3 + term4);
        const vector3d force = j * c->normal;

        b1->SetVelocity(linVel1 + force*invMass1);
        b1->SetAngVelocity(angVel1 + hitPos1.Cross(force)*invAngInert1);
        b2->SetVelocity(linVel2 - force*invMass2);
        b2->SetAngVelocity(angVel2 - hitPos2.Cross(force)*invAngInert2);
    } else {
        // one body is static
        vector3d hitNormal;
        DynamicBody *mover;

        if (po1_isDynBody) {
            mover = static_cast<DynamicBody*>(po1);
            hitNormal = c->normal;
        } else {
            mover = static_cast<DynamicBody*>(po2);
            hitNormal = -c->normal;
        }

        const double coeff_rest = 0.5;
        const vector3d linVel1 = mover->GetVelocity();
        const vector3d angVel1 = mover->GetAngVelocity();

        // step back
//		mover->UndoTimestep();

        const double invMass1 = 1.0 / mover->GetMass();
        const vector3d hitPos1 = c->pos - mover->GetPosition();
        const vector3d hitVel1 = linVel1 + angVel1.Cross(hitPos1);
        const double relVel = hitVel1.Dot(c->normal);
        // moving away so no collision
        if (relVel > 0 && !c->geomFlag) return;
        if (!OnCollision(po1, po2, c, -relVel)) return;
        const double invAngInert = 1.0 / mover->GetAngularInertia();
        const double numerator = -(1.0 + coeff_rest) * relVel;
        const double term1 = invMass1;
        const double term3 = c->normal.Dot((hitPos1.Cross(c->normal)*invAngInert).Cross(hitPos1));

        const double j = numerator / (term1 + term3);
        const vector3d force = j * c->normal;

        mover->SetVelocity(linVel1 + force*invMass1);
        mover->SetAngVelocity(angVel1 + hitPos1.Cross(force)*invAngInert);
    }
}
コード例 #7
0
/*!
  Returns the correct coefficient of friction for this contact.  If either
  body is dynamic, and the relative velocity between them is greater than
  1.0 mm/sec (should be made a parameter), then it returns the kinetic COF,
  otherwise it returns the static COF.
*/
double
Contact::getCof() const
{
  DynamicBody *db;
  vec3 radius,vel1(vec3::ZERO),vel2(vec3::ZERO),rotvel;

  if (body1->isDynamic()) {
    db = (DynamicBody *)body1;
    radius = db->getTran().rotation() * (loc - db->getCoG());
    vel1.set(db->getVelocity()[0],db->getVelocity()[1],db->getVelocity()[2]);
    rotvel.set(db->getVelocity()[3],db->getVelocity()[4],db->getVelocity()[5]);
    vel1 += radius * rotvel;
  }
  if (body2->isDynamic()) {
    db = (DynamicBody *)body2;
    radius = db->getTran().rotation() * (mate->loc - db->getCoG());
    vel2.set(db->getVelocity()[0],db->getVelocity()[1],db->getVelocity()[2]);
    rotvel.set(db->getVelocity()[3],db->getVelocity()[4],db->getVelocity()[5]);
    vel2 += radius * rotvel;
  }
  if ((vel1 - vel2).len() > 1.0) {
    DBGP("SLIDING!");
    return kcof;
  }
  else return cof;
}