TrafficSimulation::TrafficSimulation(MapCoordinator *mapCoordinator, const string& host) : driverVehicleRadius(5.0), loadedMaps(), collisionHandler(NULL), client(host), mapCoordinator(mapCoordinator), viewDistance(200), player(NULL), playerCreated(false), communicationThreadId(-1), networkDataMutex(), receivedData(), dataToSend(), meshes(), vehicles(), lightBulbs(), noConnection(false), communicationThreadMutex() { // Add a dummy model for unknown vehicle types VRGeometry* geo = new VRGeometry("vehicle_type_unknown"); geo->addAttachment("dynamicaly_generated", 0); geo->setPrimitive("Box", "1 1 2 1 1 1"); meshes[404] = geo; a_red = new VRMaterial("a_red"); a_orange = new VRMaterial("a_orange"); a_green = new VRMaterial("a_green"); a_red->setDiffuse(Vec3f(1,0,0)); a_orange->setDiffuse(Vec3f(1,0.8,0.1)); a_green->setDiffuse(Vec3f(0,1,0.1)); a_red->setLit(false); a_orange->setLit(false); a_green->setLit(false); //VRFunction<int>* fkt = new VRFunction<int>( "traffic update fkt", boost::bind(&TrafficSimulation::updateScene, this) ); //VRSceneManager::get()->getActiveScene()->addUpdateFkt(fkt); }
void TrafficSimulation::update() { networkDataMutex.lock(); // Update the position of the player to be send to the server if (player != NULL) { // Move the vehicle Value vehicle; vehicle["id"] = 0; Value pos; Vec3f worldPosition = player->getFrom(); pos[0u] = worldPosition[0]; pos[1] = worldPosition[1]; pos[2] = worldPosition[2]; vehicle["pos"] = pos; // Pack as the requested array entry dataToSend["moveVehicles"][0] = vehicle; } if (!receivedData.isNull()) { //cout << "Received data\n"; // New data received per network. Update the positions of vehicles and the states of traffic lights // A set which contains the IDs of the currently existing vehicles. // If data for one of those is received, the entry in the set will be removed. // All vehicles that are in the set after the loop finished are no longer in the // area and can be deleted. set<uint> vehicleIDs; for (auto iter : vehicles) vehicleIDs.insert(iter.first); if (!receivedData["vehicles"].isNull() && receivedData["vehicles"].isArray()) { // Update the vehicles //cout << "Received vehicles " << receivedData["vehicles"].size(); // A set for possible collisions // Add them all to the set and resolve them later on to avoid doubled checks set< pair<unsigned int, unsigned int> > collisions; // Sleeps for 10ms each tick, but the send deltas are for one second // Use only a part of them to avoid moving too fast static const float partDelta = 10 / 1000; for (auto vehicleIter : receivedData["vehicles"]) { // Check if the values have valid types if (!vehicleIter["id"].isConvertibleTo(uintValue) || !vehicleIter["pos"].isConvertibleTo(arrayValue) || !vehicleIter["pos"][0].isConvertibleTo(realValue) || !vehicleIter["pos"][1].isConvertibleTo(realValue) || !vehicleIter["pos"][2].isConvertibleTo(realValue) || !vehicleIter["dPos"].isConvertibleTo(arrayValue) || !vehicleIter["dPos"][0].isConvertibleTo(realValue) || !vehicleIter["dPos"][1].isConvertibleTo(realValue) || !vehicleIter["dPos"][2].isConvertibleTo(realValue) || !vehicleIter["angle"].isConvertibleTo(arrayValue) || !vehicleIter["angle"][0].isConvertibleTo(realValue) || !vehicleIter["angle"][1].isConvertibleTo(realValue) || !vehicleIter["angle"][2].isConvertibleTo(realValue) || !vehicleIter["dAngle"].isConvertibleTo(arrayValue) || !vehicleIter["dAngle"][0].isConvertibleTo(realValue) || !vehicleIter["dAngle"][1].isConvertibleTo(realValue) || !vehicleIter["dAngle"][2].isConvertibleTo(realValue)) { cout << "TrafficSimulation: Warning: Received invalid vehicle data.\n"; continue; } uint ID = vehicleIter["id"].asUInt(); if (vehicles.count(ID) == 0) { // The vehicle is new, create it if (!vehicleIter["vehicle"].isConvertibleTo(uintValue) || !vehicleIter["driver"].isConvertibleTo(uintValue)) { cout << "TrafficSimulation: Warning: Received invalid vehicle data.\n"; continue; } uint vID = vehicleIter["vehicle"].asUInt(); if (meshes.count(vID) == 0) { // If it is bigger than 500 it is our user-controlled vehicle if (vID < 500) cout << "TrafficSimulation: Warning: Received unknown vehicle type " << vID << ".\n"; continue; } Vehicle v; v.id = ID; v.vehicleTypeId = vID; v.driverTypeId = vehicleIter["driver"].asUInt(); if (meshes.count(v.vehicleTypeId) == 0) v.vehicleTypeId = 404; v.geometry = (VRGeometry*)meshes[v.vehicleTypeId]->duplicate(true); v.geometry->addAttachment("dynamicaly_generated", 0); // Add it to the map vehicles.insert(make_pair(v.id, v)); } else vehicleIDs.erase(ID); // Already (and still) exists, remove its ID from the vehicle-id-set // Now the vehicle exists, update its position and state Vehicle& v = vehicles[ID]; v.pos = Vec3f(vehicleIter["pos"][0].asFloat(), vehicleIter["pos"][1].asFloat(), vehicleIter["pos"][2].asFloat()); v.deltaPos = Vec3f(vehicleIter["dPos"][0].asFloat(), vehicleIter["dPos"][1].asFloat(), vehicleIter["dPos"][2].asFloat()); v.deltaPos *= partDelta; v.orientation = Vec3f(vehicleIter["angle"][0].asFloat(), vehicleIter["angle"][1].asFloat(), vehicleIter["angle"][2].asFloat()); v.deltaOrientation = Vec3f(vehicleIter["dAngle"][0].asFloat(), vehicleIter["dAngle"][1].asFloat(), vehicleIter["dAngle"][2].asFloat()); v.deltaOrientation *= partDelta; if (!vehicleIter["state"].isNull() && vehicleIter["state"].isArray()) { v.state = Vehicle::NONE; for (auto state : vehicleIter["state"]) { if (!state.isConvertibleTo(stringValue)) continue; if(state.asString() == "rightIndicator") { v.state |= Vehicle::RIGHT_INDICATOR; } else if(state.asString() == "leftIndicator") { v.state |= Vehicle::LEFT_INDICATOR; } else if(state.asString() == "accelerating") { v.state |= Vehicle::ACCELERATING; } else if(state.asString() == "braking") { v.state |= Vehicle::BRAKING; } else if(state.asString() == "waiting") { v.state |= Vehicle::WAITING; } else if(state.asString() == "blocked") { v.state |= Vehicle::BLOCKED; } else if(state.asString() == "collision") { v.state |= Vehicle::COLLIDED; } } } if (!vehicleIter["colliding"].isNull() && vehicleIter["colliding"].isArray()) { for (auto collisionIter : vehicleIter["colliding"]) { if (!collisionIter.isConvertibleTo(uintValue)) continue; uint other = collisionIter.asUInt(); if (other < v.id) collisions.insert(make_pair(other, v.id)); else collisions.insert(make_pair(v.id, other)); } } } // End vehicle iteration // Okay, all vehicles are updated now // Resolve collisions if (collisionHandler != NULL) { for (auto c : collisions) { if (collisionHandler(vehicles[c.first], vehicles[c.second])) { dataToSend["collision"].append(c.first); dataToSend["collision"].append(c.second); } } } } // End vehicle updates // Remove vehicles which are no longer on the map for (auto v : vehicleIDs) { vehicles[v].geometry->destroy(); vehicles.erase(v); } // Get traffic light updates if (!receivedData["trafficlights"].isNull() && receivedData["trafficlights"].isArray()) { // The light bulbs in the array will be moved around arbitrary whichever light posts are given // If there are not enough bulbs in the array, more are added // If there are too many bulbs, they are deleted size_t bulbIndex = 0; static const double postHeight = 2; static const double bulbSize = 1; // Note: If you change this value from 2, change the value further down in new VRGeometry(), too. for (auto lightpost : receivedData["trafficlights"]) { if (!lightpost.isObject()) continue; if (!lightpost["at"].isConvertibleTo(uintValue) || !lightpost["to"].isConvertibleTo(uintValue) || lightpost["state"].isNull() || !lightpost["state"].isArray()) { cout << "TrafficSimulation: Warning: Received invalid light post data.\n"; continue; } // Calculate the vector of the street // Get the node positions Vec2f atPos, toPos; string atId = lexical_cast<string>(lightpost["at"].asUInt()); string toId = lexical_cast<string>(lightpost["to"].asUInt()); bool foundAt = false, foundTo = false; for (auto mapIter : loadedMaps) { for (auto nodeIter : mapIter->osmNodes) { if (!foundAt && nodeIter->id == atId) { atPos = mapCoordinator->realToWorld(Vec2f(nodeIter->lat, nodeIter->lon)); foundAt = true; } if (!foundTo && nodeIter->id == toId) { toPos = mapCoordinator->realToWorld(Vec2f(nodeIter->lat, nodeIter->lon)); foundTo = true; } if (foundAt && foundTo) break; } if (foundAt && foundTo) break; } Vec2f streetOffset = toPos - atPos; const float prevLength = streetOffset.length(); streetOffset.normalize(); streetOffset *= min(prevLength / 2, Config::get()->STREET_WIDTH); Vec2f normal(-streetOffset[1], streetOffset[0]); normal.normalize(); normal *= Config::get()->STREET_WIDTH; streetOffset += atPos; // streetOffset now contains a position in the center of a street a bit away from the crossing // normal is a vector that is orthogonal to the street // Now iterate over the lanes and hang up the lights double lane = -0.5; for (auto light : lightpost["state"]) { lane += 1; if (!light.isConvertibleTo(stringValue)) continue; while (bulbIndex+1 >= lightBulbs.size()) { if (VRSceneManager::get()->getActiveScene() == NULL) break; // Create a new light VRGeometry* geo = new VRGeometry("ampel"); geo->addAttachment("dynamicaly_generated", 0); geo->setPrimitive("Sphere", "0.5 2"); // The first value has to be half of bulbSize geo->setMaterial(a_red); VRSceneManager::get()->getActiveScene()->add(geo); lightBulbs.push_back(geo); } // color switch VRGeometry* bulb = lightBulbs[bulbIndex++]; Vec3f p = Vec3f(streetOffset[0] + lane * normal[0], postHeight, streetOffset[1] + lane * normal[1]); string lcol = light.asString(); if (lcol == "red") { bulb->setWorldPosition(p+Vec3f(0,3 * bulbSize,0)); bulb->setMaterial(a_red); } else if (lcol == "redamber") { bulb->setWorldPosition(p+Vec3f(0,3 * bulbSize,0)); bulb->setMaterial(a_red); bulb = lightBulbs[bulbIndex++]; bulb->setWorldPosition(p+Vec3f(0,2 * bulbSize,0)); bulb->setMaterial(a_orange); } else if (lcol == "amber") { bulb->setWorldPosition(p+Vec3f(0,2 * bulbSize,0)); bulb->setMaterial(a_orange); } else if (lcol == "green") { bulb->setWorldPosition(p+Vec3f(0,bulbSize,0)); bulb->setMaterial(a_green); } } } // Remove unused lightbulbs while (bulbIndex < lightBulbs.size()) { lightBulbs.back()->destroy(); lightBulbs.pop_back(); } } } networkDataMutex.unlock(); // Advance the vehicles a bit //cout << "Update " << vehicles.size() << " vehicles\n"; for (auto v : vehicles) { Vec3f p = v.second.pos; p[1] = -1.2;//TODO: get right street height v.second.geometry->setFrom(p); v.second.geometry->setDir(v.second.pos - v.second.orientation); v.second.pos += v.second.deltaPos; v.second.orientation += v.second.deltaOrientation; } }