void FCScene::doFrame(void) { FCFighter* fighter = (FCFighter*)_player.get(); //_camNode->setPosition(fighter->getPosition() + fighter->getAxis()*Ogre::Vector3(-0.5, 3, -1)); ////_camNode->setDirection(fighter->getAxis()*Ogre::Vector3::UNIT_Z, Ogre::Node::TS_WORLD); //_camNode->setOrientation(fighter->getNode()->getOrientation()*Ogre::Quaternion(0, 0, 1, 0)); //FCKnowledge::getSingleton().setPlayerPosition(fighter->getPosition()); DataLogger::getSingleton().getPlayerData(fighter->getID(), fighter->getPosition().ptr(), fighter->getOrientation().ptr(), fighter->getHealthPoint()); for(unsigned int i = 0; i < fighter->getBulletList().size(); i++) { FCBullet* bullet = (FCBullet*)fighter->getBulletList()[i].get(); if(bullet->isActive()) DataLogger::getSingleton().getPlayerProjectileData(bullet->getID(), bullet->getPosition().ptr(), bullet->getOrientation().ptr()); } for(unsigned int i = 0; i < _enemies.size(); i++) { FCFighter* enemy = (FCFighter*)_enemies[i].get(); DataLogger::getSingleton().getBotData(enemy->getID(), enemy->getPosition().ptr(), enemy->getOrientation().ptr(), enemy->getHealthPoint()); for(unsigned int i = 0; i < enemy->getBulletList().size(); i++) { FCBullet* bullet = (FCBullet*)enemy->getBulletList()[i].get(); if(bullet->isActive()) DataLogger::getSingleton().getBotProjectileData(bullet->getID(), enemy->getID(), bullet->getPosition().ptr(), bullet->getOrientation().ptr()); } } Obstacle* obs = (Obstacle*)_obstacle1.get(); DataLogger::getSingleton().getObstacleData(obs->getID(), obs->getPosition().ptr(), obs->getScale()); obs = (Obstacle*)_obstacle2.get(); DataLogger::getSingleton().getObstacleData(obs->getID(), obs->getPosition().ptr(), obs->getScale()); obs = (Obstacle*)_obstacle3.get(); DataLogger::getSingleton().getObstacleData(obs->getID(), obs->getPosition().ptr(), obs->getScale()); DataLogger::getSingleton().writeTest(); }
core::SharedPointer<core::wrapper::Image> OpenGLGrabber::getNextImage() { if ( (m_sharedMemory.isValid()) && (m_sharedMemory->isValid()) ) { m_sharedMemory->lock(); // Render the image right before grabbing it. switch (m_render) { case OpenGLGrabber::IN_CAR: renderNextImageInCar(); m_FIFO_Obstacles.clear(); break; case OpenGLGrabber::CHASE_CAR: renderNextImageChaseCar(); m_FIFO_Obstacles.clear(); break; case OpenGLGrabber::CHASE_CAR_SENSORS: { const uint32_t size = m_FIFO_Obstacles.getSize(); for(uint32_t i = 0; i < size; i++) { Container c = m_FIFO_Obstacles.leave(); if (c.getDataType() == Container::OBSTACLE) { Obstacle obstacle = c.getData<Obstacle>(); // Check if sensor FOV-"Obstacle": if (obstacle.getID() >= 9000) { switch (obstacle.getState()) { case Obstacle::REMOVE: { // Remove obstacle. map<uint32_t, Node*>::iterator result = m_mapOfObstacles.find(obstacle.getID()); if (result != m_mapOfObstacles.end()) { // Remove child from scene graph node. m_sensors->removeChild(result->second); // Remove entry from map. m_mapOfObstacles.erase(result); } } break; case Obstacle::UPDATE: { map<uint32_t, Node*>::iterator result = m_mapOfObstacles.find(obstacle.getID()); if (result != m_mapOfObstacles.end()) { // Remove child from scene graph node. m_sensors->removeChild(result->second); // Remove entry from map. m_mapOfObstacles.erase(result); } // Update obstacle. TransformGroup *contourTG = new TransformGroup(); vector<Point3> contour = obstacle.getPolygon().getVertices(); // Close polygons. Point3 p = contour.at(0); contour.push_back(p); for (uint32_t k = 0; k < contour.size() - 1; k++) { Point3 A = contour.at(k); A.setZ(0.5); Point3 B = contour.at(k+1); B.setZ(0.5); contourTG->addChild(new hesperia::threeD::models::Line(NodeDescriptor(), A, B, Point3(0, 1, 0), 2)); } m_mapOfObstacles[obstacle.getID()] = contourTG; m_sensors->addChild(contourTG); } break; } } } } renderNextImageChaseCarSensors(); } break; } // TODO Read pixels using BGRA!!! glReadBuffer(GL_BACK); glPixelStorei(GL_PACK_ALIGNMENT, 1); glReadPixels(0, 0, m_image->getWidth(), m_image->getHeight(), GL_BGR, GL_UNSIGNED_BYTE, m_sharedMemory->getSharedMemory()); // Flip the image horizontally. m_image->flipHorizontally(); m_sharedMemory->unlock(); } return m_image; }
void EnvironmentViewerGLWidget::nextContainer(Container &c) { if (c.getDataType() == Container::EGOSTATE) { m_numberOfReceivedEgoStates++; if (m_egoStateNode != NULL) { Lock l(m_rootMutex); EgoState egostate = c.getData<EgoState>(); Point3 dir(0, 0, egostate.getRotation().getAngleXY()); m_egoStateNode->setRotation(dir); m_egoStateNode->setTranslation(egostate.getPosition()); Position egoPosition; egoPosition.setPosition(egostate.getPosition()); egoPosition.setRotation(egostate.getRotation()); m_mapOfCurrentPositions[m_egoStateNodeDescriptor] = egoPosition; if ( (m_numberOfReceivedEgoStates % 30) == 0 ) { NodeDescriptor nd("EgoCar (Trace)"); TransformGroup *tg = m_mapOfTraceablePositions[nd]; if (tg != NULL) { Point3 color(0, 0, 1); hesperia::threeD::models::Point *p = new hesperia::threeD::models::Point(NodeDescriptor("Trace"), egostate.getPosition(), color, 5); tg->addChild(p); } } } } if (c.getDataType() == Container::CONTOUREDOBJECTS) { if (m_contouredObjectsNode != NULL) { Lock l(m_rootMutex); ContouredObjects cos = c.getData<ContouredObjects>(); vector<ContouredObject> listOfContouredObjects = cos.getContouredObjects(); vector<ContouredObject>::iterator it = listOfContouredObjects.begin(); m_contouredObjectsNode->deleteAllChildren(); while (it != listOfContouredObjects.end()) { vector<Point3> contour = (*it).getContour(); vector<Point3>::iterator jt = contour.begin(); while (jt != contour.end()) { m_contouredObjectsNode->addChild(new hesperia::threeD::models::Point(NodeDescriptor("Point"), (*jt), Point3(1, 0, 0), 2)); jt++; } it++; } } } if (c.getDataType() == Container::ROUTE) { if (m_plannedRoute != NULL) { Lock l(m_rootMutex); Route r = c.getData<Route>(); vector<Point3> listOfVertices = r.getListOfPoints(); const uint32_t SIZE = listOfVertices.size(); if (SIZE > 0) { m_plannedRoute->deleteAllChildren(); for (uint32_t i = 0; i < SIZE - 1; i++) { Point3 posA = listOfVertices.at(i); posA.setZ(0.05); Point3 posB = listOfVertices.at(i+1); posB.setZ(0.05); m_plannedRoute->addChild(new hesperia::threeD::models::Line(NodeDescriptor(), posA, posB, Point3(0, 1, 0), 6)); } } } } if (c.getDataType() == Container::DRAW_LINE) { if (m_lines != NULL) { Lock l(m_rootMutex); hesperia::data::environment::Line line = c.getData<Line>(); Point3 posA = line.getA(); posA.setZ(0.05); Point3 posB = line.getB(); posB.setZ(0.05); m_lines->addChild(new hesperia::threeD::models::Line(NodeDescriptor(), posA, posB, Point3(1, 0, 0), 6)); } } if (c.getDataType() == Container::OBSTACLE) { if (m_obstaclesRoot != NULL) { Lock l(m_rootMutex); Obstacle obstacle = c.getData<Obstacle>(); switch (obstacle.getState()) { case Obstacle::REMOVE: { // Remove obstacle. map<uint32_t, Node*>::iterator result = m_mapOfObstacles.find(obstacle.getID()); if (result != m_mapOfObstacles.end()) { // Remove child from scene graph node. m_obstaclesRoot->removeChild(result->second); // Remove entry from map. m_mapOfObstacles.erase(result); } } break; case Obstacle::UPDATE: { map<uint32_t, Node*>::iterator result = m_mapOfObstacles.find(obstacle.getID()); if (result != m_mapOfObstacles.end()) { // Remove child from scene graph node. m_obstaclesRoot->removeChild(result->second); // Remove entry from map. m_mapOfObstacles.erase(result); } // Update obstacle. TransformGroup *contourTG = new TransformGroup(); vector<Point3> contour = obstacle.getPolygon().getVertices(); // Close polygons. Point3 p = contour.at(0); contour.push_back(p); for (uint32_t k = 0; k < contour.size() - 1; k++) { Point3 A = contour.at(k); A.setZ(0.5); Point3 B = contour.at(k+1); B.setZ(0.5); contourTG->addChild(new hesperia::threeD::models::Line(NodeDescriptor(), A, B, Point3(0, 1, 0), 2)); } m_mapOfObstacles[obstacle.getID()] = contourTG; m_obstaclesRoot->addChild(contourTG); } break; } } } }