예제 #1
0
void FluidMask::createObstacles(float spacing)
{
    cout<<"Parsing svg paths... ";
    
    int obsNum=0;
    
    int pathNum = svg.getNumPath();
    for (int i=0; i<pathNum; i++)
    {
        ofPath path = svg.getPathAt(i);
        
        if (path.getStrokeColor().r != 0) {
            continue;
        }
        // for each path get the outline
        vector<ofPolyline> outline = path.getOutline();
        for (int l=0; l<outline.size(); l++)
        {
            ofPolyline resampledLine = outline[l].getResampledBySpacing(spacing);
            vector<ofPoint> points = resampledLine.getVertices();
            for (int v=0; v<points.size(); v++)
            {
//                cout<<"vertex at "<<points[v].x<<"x"<<points[v].y<<endl;
                addObstacle(points[v].x/2, points[v].y/2 - 5, obstacleSize);
                obsNum++;
            }
        }
    }
    
    cout<<"created "<<obsNum<<" obstacles\n";
}
예제 #2
0
void Game::init() {
	objectMap.addLayer(Obstacle::STATIC, AABBf(V2f(-60, -60), V2f(60, 60)), V2i(16, 16));
	addObstacle(new CircleObject(Circf(V2f(-4, -4), 2), Obstacle::STATIC));
	addObstacle(new AABBObject(AABBf(V2f(-4, 1), V2f(-2, 5)), Obstacle::STATIC));
	addObstacle(new BoxObject(Boxf(V2f(4, 0), V2f(2, 5), V2f(1)), Obstacle::STATIC));
	addObstacle(new ConeObject(Conef(V2f(0, 1), 1.5f, (float)V_2PI * 1 / 8, (float)V_2PI * 3 / 8), Obstacle::STATIC)); // make the radius of the cone correct based on the calculated center
	V2f trapezoid[] = { V2f(-.5f, .5f), V2f(.5f, 1.2f), V2f(.5f, -1.5f), V2f(-.5f, -.8f) };
	int trapezoidCount = sizeof(trapezoid) / sizeof(trapezoid[0]);
	addObstacle(new PolygonObject(Polygon2f(V2f(5, -7), V2f((float)V_2PI*0.0f/6), trapezoid, trapezoidCount), Obstacle::STATIC)); // TODO raycast does not always hit the shape correctly from all sides
	V2f a(-2, -2), b(5, -4);
	addObstacle(new LineObject(Linef(&a, &b), Obstacle::STATIC));
	addObstacle(new PointObject(V2f(7, 7), Obstacle::STATIC));

	delaunyBoundary = new CircleObject(Circf(V2f::ZERO(), 100), Obstacle::EVERYTHING);
	delauny = new DelaunySet(delaunyBoundary);
	//delauny->addNode(V2f(3, 4));
	//delauny->addNode(V2f(-3, 4));
	//delauny->addNode(V2f(-3, -4));
	//delauny->addNode(V2f(0, -1));
//	for (int i = 0; i < 10; ++i) {
//		delauny->addNode(V2f::randomUnitVector() * Random::PRNGf() * 100);
//	}
	delauny->makeRandom(6);
	delauny->calculateAllTriangles();
}
예제 #3
0
void CliHandler::operator()(const int sockfd) {
  for (;;) {
    std::string line = fetchMessageString(sockfd);
    auto cmd = detachToken(line);
    makeLowerCase(cmd);
    if (cmd == "at")
      addTarget(line);
    else if (cmd == "ao")
      addObstacle(line);
    else if (cmd == "clear")
      clear(line);
    else if (cmd == "eot")
      return;
  }
}
예제 #4
0
파일: mapper.c 프로젝트: Chryel/PINT
void updateMap(struct comm_range_pose_data * data) {
  int i;
  double theta_pose;
  // Read pose
  struct point_d pose;
  pose.x = data->pose[INDEX_X];
  pose.y = data->pose[INDEX_Y];
  theta_pose = data->pose[INDEX_A];

  free(current_pose);
  current_pose = gridify(&pose);
  send_msg.obs_count = 0;

  // Convert ranges absolute positions
  for (i = 0; i < RANGE_COUNT; i++) {
    struct point_d obstacle_l, obstacle_g;

    // obstacle location relative to the robot
    double tao = (2.0 * M_PI * i) / RANGE_COUNT;
    obstacle_l.x = data->ranges[i] * cos(tao);
    obstacle_l.y = data->ranges[i] * sin(tao);
    
    // obstacle location in global coords
    obstacle_g.x = obstacle_l.x * cos(theta_pose) - obstacle_l.y * sin(theta_pose);
    obstacle_g.x += pose.x;
    obstacle_g.y = obstacle_l.x * sin(theta_pose) + obstacle_l.y * cos(theta_pose);
    obstacle_g.y += pose.y;

    addObstacle(gridify(&obstacle_g));
  }

  // TODO: What if a SDC messed with the obs_count sent?
  if (insertSDC) {
    insertSDC = false;
    current_pose->x++;
  }
  send_msg.pose_x = current_pose->x;
  send_msg.pose_y = current_pose->y;

  commSendMapUpdate(&pipes[update_index], &send_msg);
}
예제 #5
0
void
World::init()
{
    Material material = {
        Options::mu(),
        Options::lambda(),
        Options::density(),
        Options::hardening(),
        Options::flowRate(),
        Options::yieldPoint(),
    };
    SoftBody* body = new SoftBody(Options::particleFile(), material);

    Mesh* mesh = new Mesh;
    if (Mesh::loadObj(Options::meshFile(), *mesh)) {
        body->setMesh(mesh);
    }

    addSoftBody(body);
    addObstacle(new PlaneObstacle(Eigen::Vector3d(0, 1, 0), -1, Options::friction()));
}
예제 #6
0
void ModularStateEstimator::init(core::ConfigNode config,
                                 core::EventHubPtr eventHub)
{
    //Removed 7/13/2013 to test Combined DVL Acclerometer Kalman filter
    //modules.push_back(EstimationModulePtr(
    //                      new BasicDVLEstimationModule(
    //                          config["DVLEstimationModule"],
    //                          eventHub,
    //                          m_estimatedState)));
    modules.push_back(EstimationModulePtr(
                          new DVLAccelerometerEstimator(
                              eventHub,
                              m_estimatedState)));

    modules.push_back(EstimationModulePtr(
                          new BasicIMUEstimationModule(
                              config["IMUEstimationModule"],
                              eventHub,
                              m_estimatedState)));

    modules.push_back(EstimationModulePtr(
                          new DepthKalmanModule(
                              config["DepthEstimationModule"],
                              eventHub,
                              m_estimatedState)));


    if(config.exists("GreenBuoy"))
    {
        addObstacle(config["GreenBuoy"], Obstacle::GREEN_BUOY);
        modules.push_back(
            EstimationModulePtr(new SimpleBuoyEstimationModule(
                                    config["GreenBuoyEstimationModule"],
                                    eventHub,
                                    m_estimatedState,
                                    Obstacle::GREEN_BUOY, 
                                    vision::EventType::BUOY_FOUND)));
    }

    if(config.exists("RedBuoy"))
    {
        addObstacle(config["RedBuoy"], Obstacle::RED_BUOY);
        modules.push_back(
            EstimationModulePtr(new SimpleBuoyEstimationModule(
                                    config["RedBuoyEstimationModule"],
                                    eventHub, 
                                    m_estimatedState,
                                    Obstacle::RED_BUOY,
                                    vision::EventType::BUOY_FOUND)));
    }

    if(config.exists("YellowBuoy"))
    {
        addObstacle(config["YellowBuoy"], Obstacle::YELLOW_BUOY);
        modules.push_back(
            EstimationModulePtr(new SimpleBuoyEstimationModule(
                                    config["YellowBuoyEstimationModule"],
                                    eventHub, 
                                    m_estimatedState,
                                    Obstacle::YELLOW_BUOY,
                                    vision::EventType::BUOY_FOUND)));
    }

}
dtStatus dtTileCache::addObstacle(const float* pos, const float radius, const float height, dtObstacleRef* result)
{
	return addObstacle(pos, radius, height, 0, result);
}
예제 #8
0
/*!
  This is the main routine for parsing input on the clientSocket.
  There should be one command for each line of input.  This reads one
  line, and looks at the first word (up to the first space character) to
  determine the command.   Then if there are body or robot indices to read,
  it calls a support routine to read those and return a vector of bodies or
  robots.  These are then passed to the appropriate routine to carry out the
  action and write out any necessary results.
*/
void
ClientSocket::readClient()
{
  int i,numData,numBodies,numRobots;
  double time;
  std::vector<Body *> bodyVec;
  std::vector<Robot *> robVec;

  bool ok;

  while ( canReadLine() ) {
    line = readLine();
    line.truncate(line.length()-1); //strip newline character
    lineStrList =
      QStringList::split(' ',line);
    strPtr = lineStrList.begin();

#ifdef GRASPITDBG
    std::cout <<"Command parser line: "<<line << std::endl;
#endif
    
    if (*strPtr == "getContacts") {
      strPtr++; if (strPtr == lineStrList.end()) continue;
      numData = (*strPtr).toInt(&ok); strPtr++;
      if (!ok) continue;

#ifdef GRASPITDBG
      std::cout << "Num data: "<<numData<<std::endl;
#endif

      if (readBodyIndList(bodyVec)) continue;
      numBodies = bodyVec.size();
      for (i=0;i<numBodies;i++)
        sendContacts(bodyVec[i],numData);
    }
    
    else if (*strPtr == "getAverageContacts") {
      strPtr++;
      if (readBodyIndList(bodyVec)) continue;
      numBodies = bodyVec.size();
      for (i=0;i<numBodies;i++)
	sendAverageContacts(bodyVec[i]);
    }
    
    else if (*strPtr == "getBodyName") {
      strPtr++;
      if (readBodyIndList(bodyVec)) continue;
      numBodies = bodyVec.size();
      for (i=0;i<numBodies;i++)
	sendBodyName(bodyVec[i]);
    }
    else if(*strPtr == "setBodyName") {
      strPtr++;
      int body_index;
      if(strPtr != lineStrList.end()){
        body_index = strPtr->toInt(&ok);
        strPtr++;
        if(strPtr == lineStrList.end())
          return;
        if (body_index == -1 || body_index >= graspItGUI->getIVmgr()->getWorld()->getNumBodies())
        {
          body_index = graspItGUI->getIVmgr()->getWorld()->getNumBodies() - 1;          
        }
        graspItGUI->getIVmgr()->getWorld()->getBody(body_index)->setName(*strPtr);
      }
    }
    
    else if (*strPtr == "getRobotName") {
      strPtr++;
      if (readRobotIndList(robVec)) continue;
      numRobots = robVec.size();
      for (i=0;i<numRobots;i++)
	sendRobotName(robVec[i]);
    }
    
    else if (*strPtr == "getDOFVals") {
      strPtr++;
      if (readRobotIndList(robVec)) continue;
      numRobots = robVec.size();
      for (i=0;i<numRobots;i++)
	sendDOFVals(robVec[i]);
    }
    
    else if (*strPtr == "moveDOFs") {
      strPtr++;
      readDOFVals();
    }
    
    else if (*strPtr == "render")
      graspItGUI->getIVmgr()->getViewer()->render();
    
    else if (*strPtr == "setDOFForces") {
      strPtr++;
      if (readRobotIndList(robVec)) continue;
      numRobots = robVec.size();
      for (i=0;i<numRobots;i++)
	if (readDOFForces(robVec[i])==FAILURE) continue;
    }
    
    else if ((*strPtr) == "moveDynamicBodies") {
      strPtr++;
      if (strPtr == lineStrList.end()) ok = FALSE;
      else {
	time = (*strPtr).toDouble(&ok); strPtr++;
      }
      if (!ok)
	moveDynamicBodies(-1);
      else
	moveDynamicBodies(time);
    }
    
    else if (*strPtr == "computeNewVelocities") {

#ifdef GRASPITDBG
      std::cout << "cnv" << std::endl;
#endif

      strPtr++; if (strPtr == lineStrList.end()) continue;
      time = (*strPtr).toDouble(&ok); strPtr++;
      if (!ok) continue;

#ifdef GRASPITDBG
      std::cout << time <<std::endl;
#endif
      computeNewVelocities(time);
    }    
    else if ((*strPtr) == "outputPlannerResults"){      
      strPtr++;
      outputPlannerResults(0);      
    }
    else if ((*strPtr) == "outputCurrentGrasp"){      
      strPtr++;
      outputCurrentGrasp();
    }    
    else if ((*strPtr) == "sendBodyTransf"){            
      strPtr++;
      verifyInput(1);
      sendBodyTransf();
    }
    else if ((*strPtr) == "setBodyTransf"){            
      strPtr++;
      verifyInput(7);
      setBodyTransf();
    }
    else if ((*strPtr) == "addObstacle"){            
      strPtr++;
      verifyInput(1);
      addObstacle(*(strPtr+1));
      strPtr+=2;
    }
    else if ((*strPtr) == "addObject"){
      verifyInput(2);
      addGraspableBody(*(strPtr+1), *(strPtr+2));
      strPtr+=3;
      verifyInput(7);
      transf object_pose;
      readTransf(&object_pose);
      World * w = graspItGUI->getIVmgr()->getWorld();
      w->getGB(w->getNumGB() - 1)->setTran(object_pose);
    }
    
    else if ((*strPtr) == "getCurrentHandTran"){
      strPtr++;
      getCurrentHandTran();
    }
    else if ((*strPtr) == "signalGraspUnreachable"){
      strPtr+=4;
      std::cout << line.toStdString() << std::endl;
      graspItGUI->getIVmgr()->blinkBackground();
    }
    else if ((*strPtr) == "getPlannerTarget"){
      strPtr+=1;
      QTextStream os (this) ;
      os << graspItGUI->getIVmgr()->getWorld()->getCurrentHand()->getGrasp()->getObject()->getName() << "\n";
    } 
    else if ((*strPtr) == "setPlannerTarget"){
      QTextStream os(this);
      os << setPlannerTarget(*(strPtr+1)) << "\n";
      strPtr+=2;

    }    

    else if ((*strPtr) == "rotateHandLat"){
      strPtr+=1;
      rotateHandLat();
    }
    else if ((*strPtr) == "rotateHandLong"){
      strPtr+=1;
      rotateHandLong();
    }    
    else if ((*strPtr) == "exec"){
      strPtr+=1;
      exec();
    } 
    else if ((*strPtr) == "next"){
      strPtr+=1;
      next();
    }
    else if ((*strPtr) == "addPointCloud")
    {
      strPtr += 1;
      addPointCloud();
      //QTextStream os(this);  
      //os << addPointCloud() <<" \n";

    }
    else if ((*strPtr) == "setCameraOrigin")
      {
	strPtr += 1;
	setCameraOrigin();
      }
    else if ((*strPtr) == "removeBodies"){
      strPtr += 1;
      removeBodies();
    }
    else if ((*strPtr) == "clearGraspableBodies"){
      strPtr += 1;
      removeBodies(true);
            
    }
  }
}
예제 #9
0
//this function is used by the soundstream to output values to the soundcard. This implementation uses 
//code given in class though it has been extended.
void testApp::audioOut(float * output,int bufferSize, int nChannels)
{
	//if the game is running
	if(!gameOver)
	{
		for(int i=0; i<bufferSize; i++)
		{
			//use the counter to step from 0 to 1
			beat=((int) counter.phasor(16));
	
			//if the beat is different to the previous beat value
			if(lastBeat!=beat) 
			{
				//increase the counter and the points
				counterVal++;
				points++;

				//if this is the first beat in a sequence, start all currently created sequences
				if(counterVal%16==0)
				{
					for(int j=0; j<started.size(); j++)
					{
						started[j]= true;
					}
				}

				//if the counter has gone through a full sequence, increment the number
				//of plays for each sequence
				if(counterVal%16==15)
				{
					incrementPlays();
				}

				//if the counter has reached the next enemy timer
				if(counterVal%nextEnemyTime==0)
				{
					//add an enemy and reset the timer
					addCollidable();
					nextEnemyTime = rand()%300;
				}
				//if the counter has reached the next obstacle spawn timer
				if(counterVal%nextObstacleTime==0)
				{
					//add an obstacle and reset the timer
					addObstacle();
					nextObstacleTime = rand()%500;
				}
				//if the counter has reached the next powerup spawn timer
				if(counterVal%nextPowerupTime==0)
				{
					//add a powerup and reset the timer
					addPowerup();
					nextPowerupTime = rand()%700;
				}

				//trigger sounds to be played
				for(int j=0; j<sequences.size(); j++)
				{
					//for each sequence, if a sound needs to be played in that sequence
					if(sequences[j][counterVal%16]==1)
					{
						//if the sequence is the first sequence, then change the size of the player object
						if(j==0)
						{
							player1.changeBeat();
						}
						//trigger the sound sample, which resets it's playback position to 0
						//the sample to trigger is given by an associated value in a different collection
						if(started[j])
						{
							instTrigger(associatedSound[j]);
						}
					}
				}
			}

			//update the last beat
			lastBeat=beat;

			//get the sound value
			myOutput = getSound();

			//send the output to the soundcard (two values because of stereo)
			output[i*nChannels] = myOutput;
			output[i*nChannels+1] = myOutput;
		}
	}
}