void StateFollowRoute::action() { updateDeltaTime(); // Get a path from the state owner's position to the first point in the route std::vector<irr::core::vector3df> path = pathUtil->returnPath(&stateOwner->getPosition(), &(route.at(pointToVisit).first)); // If there is a path to the next point if (!path.empty() && path.size() > 1) { // Move to it if (stateOwner->getPosition().getDistanceFrom(path.at(1)) > offset) { // Call callback method - boolean is false, not waiting at a point callbackFunction(&std::make_pair(false, &path.at(1))); } // If the point is reached else { // Callback method - boolean is true, waiting at a point callbackFunction(&std::make_pair(true, &path.at(1))); // Increase timer timer += frameDeltaTime; // Wait for an amount of time if specified if (timer >= route.at(pointToVisit).second) { if (++pointToVisit >= route.size()) pointToVisit = 0; timer = 0; } } } }
/** * @brief - Predict each of the filters given info on where robot has moved * Grab delta time from the system and then call the predict on each filter * */ void MMKalmanFilter::predictFilters(messages::RobotLocation odometry) { // Update the time passed updateDeltaTime(); // Update each filter for (std::vector<KalmanFilter *>::iterator it = filters.begin(); it != filters.end(); it++) (*it)->predict(odometry, deltaTime); }
void KalmanFilter::update(messages::VBall visionBall, messages::RobotLocation motion) { updated = true; //Get passed time updateDeltaTime(); predict(motion); //Note: Only update if we have an observation... if(visionBall.on()) updateWithObservation(visionBall); }
void testCube() { /* Vertices of a cube */ GLfloat vertices[] = { // Positions // Normals // Texture Coords -0.5f, -0.5f, -0.5f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.5f, -0.5f, -0.5f, 0.0f, 0.0f, -1.0f, 1.0f, 0.0f, 0.5f, 0.5f, -0.5f, 0.0f, 0.0f, -1.0f, 1.0f, 1.0f, 0.5f, 0.5f, -0.5f, 0.0f, 0.0f, -1.0f, 1.0f, 1.0f, -0.5f, 0.5f, -0.5f, 0.0f, 0.0f, -1.0f, 0.0f, 1.0f, -0.5f, -0.5f, -0.5f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, -0.5f, -0.5f, 0.5f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.5f, -0.5f, 0.5f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.5f, 0.5f, 0.5f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 0.5f, 0.5f, 0.5f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, -0.5f, 0.5f, 0.5f, 0.0f, 0.0f, 1.0f, 0.0f, 1.0f, -0.5f, -0.5f, 0.5f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, -0.5f, 0.5f, 0.5f, -1.0f, 0.0f, 0.0f, 1.0f, 0.0f, -0.5f, 0.5f, -0.5f, -1.0f, 0.0f, 0.0f, 1.0f, 1.0f, -0.5f, -0.5f, -0.5f, -1.0f, 0.0f, 0.0f, 0.0f, 1.0f, -0.5f, -0.5f, -0.5f, -1.0f, 0.0f, 0.0f, 0.0f, 1.0f, -0.5f, -0.5f, 0.5f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -0.5f, 0.5f, 0.5f, -1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.5f, 0.5f, 0.5f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.5f, 0.5f, -0.5f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.5f, -0.5f, -0.5f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.5f, -0.5f, -0.5f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.5f, -0.5f, 0.5f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.5f, 0.5f, 0.5f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, -0.5f, -0.5f, -0.5f, 0.0f, -1.0f, 0.0f, 0.0f, 1.0f, 0.5f, -0.5f, -0.5f, 0.0f, -1.0f, 0.0f, 1.0f, 1.0f, 0.5f, -0.5f, 0.5f, 0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.5f, -0.5f, 0.5f, 0.0f, -1.0f, 0.0f, 1.0f, 0.0f, -0.5f, -0.5f, 0.5f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, -0.5f, -0.5f, -0.5f, 0.0f, -1.0f, 0.0f, 0.0f, 1.0f, -0.5f, 0.5f, -0.5f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.5f, 0.5f, -0.5f, 0.0f, 1.0f, 0.0f, 1.0f, 1.0f, 0.5f, 0.5f, 0.5f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 0.5f, 0.5f, 0.5f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, -0.5f, 0.5f, 0.5f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, -0.5f, 0.5f, -0.5f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f }; glm::vec3 cubePositions[] = { glm::vec3( 0.0f, 0.0f, 0.0f), glm::vec3( 2.0f, 5.0f, -15.0f), glm::vec3(-1.5f, -2.2f, -2.5f), glm::vec3(-3.8f, -2.0f, -12.3f), glm::vec3( 2.4f, -0.4f, -3.5f), glm::vec3(-1.7f, 3.0f, -7.5f), glm::vec3( 1.3f, -2.0f, -2.5f), glm::vec3( 1.5f, 2.0f, -2.5f), glm::vec3( 1.5f, 0.2f, -1.5f), glm::vec3(-1.3f, 1.0f, -1.5f) }; GLuint VBO; glGenBuffers(1, &VBO); GLuint VAO; glGenVertexArrays(1, &VAO); glBindVertexArray(VAO); /* Copy vertice array into a vertex buffer */ glBindBuffer(GL_ARRAY_BUFFER, VBO); glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW); /* Specify Format of Vertex Attributes */ glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 8 * sizeof(GLfloat), (GLvoid*)0); glEnableVertexAttribArray(0); glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, 8 * sizeof(GLfloat), (GLvoid*)(6 * sizeof(GLfloat))); glEnableVertexAttribArray(1); glBindVertexArray(0); glm::mat4 projection; projection = glm::perspective(glm::radians(45.0f), screenWidth / screenHeight, 0.1f, 100.0f); cubeShader.use(); glUniform1i(glGetUniformLocation(cubeShader.Program, "ourTexture1"), 0); while(!glfwWindowShouldClose(window)) { glfwPollEvents(); updateDeltaTime(); do_movement(); glClearColor(0.2f, 0.3f, 0.3f, 0.1f); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); /* Rendering */ cubeShader.use(); glm::mat4 view; view = viewCamera.getViewMatrix(); GLint viewLoc = glGetUniformLocation(cubeShader.Program, "view"); glUniformMatrix4fv(viewLoc, 1, GL_FALSE, glm::value_ptr(view)); GLint projectionLoc = glGetUniformLocation(cubeShader.Program, "projection"); glUniformMatrix4fv(projectionLoc, 1, GL_FALSE, glm::value_ptr(projection)); /* Load textures */ glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, a); glBindVertexArray(VAO); GLint modelLoc = glGetUniformLocation(cubeShader.Program, "model"); // glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_INT, 0); for(GLuint i = 0; i < 10; ++i) { glm::mat4 model; model = glm::translate(model, cubePositions[i]); GLfloat angle = glm::radians(20.0f * (i+1) * (GLfloat)glfwGetTime()); model = glm::rotate(model, angle, glm::vec3(1.0f, 0.3f, 0.5f)); glUniformMatrix4fv(modelLoc, 1, GL_FALSE, glm::value_ptr(model)); glDrawArrays(GL_TRIANGLES, 0, 36); } glBindVertexArray(0); //unbind vertex array /* Display the buffer */ glfwSwapBuffers(window); } }
void testLighting() { glm::vec3 lightPos(1.2f, 1.0f, 2.0f); /* Vertices of a cube */ GLfloat vertices[] = { // Positions // Normals // Texture Coords -0.5f, -0.5f, -0.5f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.5f, -0.5f, -0.5f, 0.0f, 0.0f, -1.0f, 1.0f, 0.0f, 0.5f, 0.5f, -0.5f, 0.0f, 0.0f, -1.0f, 1.0f, 1.0f, 0.5f, 0.5f, -0.5f, 0.0f, 0.0f, -1.0f, 1.0f, 1.0f, -0.5f, 0.5f, -0.5f, 0.0f, 0.0f, -1.0f, 0.0f, 1.0f, -0.5f, -0.5f, -0.5f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, -0.5f, -0.5f, 0.5f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.5f, -0.5f, 0.5f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.5f, 0.5f, 0.5f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 0.5f, 0.5f, 0.5f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, -0.5f, 0.5f, 0.5f, 0.0f, 0.0f, 1.0f, 0.0f, 1.0f, -0.5f, -0.5f, 0.5f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, -0.5f, 0.5f, 0.5f, -1.0f, 0.0f, 0.0f, 1.0f, 0.0f, -0.5f, 0.5f, -0.5f, -1.0f, 0.0f, 0.0f, 1.0f, 1.0f, -0.5f, -0.5f, -0.5f, -1.0f, 0.0f, 0.0f, 0.0f, 1.0f, -0.5f, -0.5f, -0.5f, -1.0f, 0.0f, 0.0f, 0.0f, 1.0f, -0.5f, -0.5f, 0.5f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -0.5f, 0.5f, 0.5f, -1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.5f, 0.5f, 0.5f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.5f, 0.5f, -0.5f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.5f, -0.5f, -0.5f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.5f, -0.5f, -0.5f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.5f, -0.5f, 0.5f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.5f, 0.5f, 0.5f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, -0.5f, -0.5f, -0.5f, 0.0f, -1.0f, 0.0f, 0.0f, 1.0f, 0.5f, -0.5f, -0.5f, 0.0f, -1.0f, 0.0f, 1.0f, 1.0f, 0.5f, -0.5f, 0.5f, 0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.5f, -0.5f, 0.5f, 0.0f, -1.0f, 0.0f, 1.0f, 0.0f, -0.5f, -0.5f, 0.5f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, -0.5f, -0.5f, -0.5f, 0.0f, -1.0f, 0.0f, 0.0f, 1.0f, -0.5f, 0.5f, -0.5f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.5f, 0.5f, -0.5f, 0.0f, 1.0f, 0.0f, 1.0f, 1.0f, 0.5f, 0.5f, 0.5f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 0.5f, 0.5f, 0.5f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, -0.5f, 0.5f, 0.5f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, -0.5f, 0.5f, -0.5f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f }; GLuint VBO, VAO; glGenBuffers(1, &VBO); glGenVertexArrays(1, &VAO); glBindVertexArray(VAO); glBindBuffer(GL_ARRAY_BUFFER, VBO); glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW); /* Specify Format of Vertex Attributes */ glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 8 * sizeof(GLfloat), (GLvoid*)0); glEnableVertexAttribArray(0); glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 8 * sizeof(GLfloat), (GLvoid*)(3 * sizeof(GLfloat))); glEnableVertexAttribArray(1); glVertexAttribPointer(2, 2, GL_FLOAT, GL_FALSE, 8 * sizeof(GLfloat), (GLvoid*)(6 * sizeof(GLfloat))); glEnableVertexAttribArray(2); glBindVertexArray(0); GLuint lightVAO; glGenVertexArrays(1, &lightVAO); glBindVertexArray(lightVAO); glBindBuffer(GL_ARRAY_BUFFER, VBO); glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 8 * sizeof(GLfloat), (GLvoid*)0); glEnableVertexAttribArray(0); glBindVertexArray(0); cubeShader.use(); glUniform1i(glGetUniformLocation(cubeShader.Program, "material.diffuse"), 0); glUniform1i(glGetUniformLocation(cubeShader.Program, "material.specular"), 0); glm::mat4 projection, view, model; while(!glfwWindowShouldClose(window)) { glfwPollEvents(); updateDeltaTime(); do_movement(); glClearColor(0.0f, 0.0f, 0.0f, 0.1f); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); /* Cube */ cubeShader.use(); GLint modelLoc = glGetUniformLocation(cubeShader.Program, "model"); GLint viewLoc = glGetUniformLocation(cubeShader.Program, "view"); GLint projectionLoc = glGetUniformLocation(cubeShader.Program, "projection"); model = glm::mat4(); view = viewCamera.getViewMatrix(); projection = glm::perspective(glm::radians(45.0f), screenWidth / screenHeight, 0.1f, 100.0f); glUniformMatrix4fv(modelLoc, 1, GL_FALSE, glm::value_ptr(model)); glUniformMatrix4fv(viewLoc, 1, GL_FALSE, glm::value_ptr(view)); glUniformMatrix4fv(projectionLoc, 1, GL_FALSE, glm::value_ptr(projection)); GLint viewPosLoc = glGetUniformLocation(cubeShader.Program, "viewPos"); glm::vec3 cameraPos = viewCamera.getPositionVec(); glUniform3f(viewPosLoc, cameraPos.x, cameraPos.y, cameraPos.z); GLint matSpecularLoc = glGetUniformLocation(cubeShader.Program, "material.specular"); GLint matShineLoc = glGetUniformLocation(cubeShader.Program, "material.shininess"); glUniform3f(matSpecularLoc, 0.5f, 0.5f, 0.5f); glUniform1f(matShineLoc, 32.0f); GLint lightAmbientLoc = glGetUniformLocation(cubeShader.Program, "light.ambient"); GLint lightDiffuseLoc = glGetUniformLocation(cubeShader.Program, "light.diffuse"); GLint lightSpecularLoc = glGetUniformLocation(cubeShader.Program, "light.specular"); GLint lightPosLoc = glGetUniformLocation(cubeShader.Program, "light.position"); glUniform3f(lightAmbientLoc, 0.2f, 0.2f, 0.2f); glUniform3f(lightDiffuseLoc, 0.5f, 0.5f, 0.5f); glUniform3f(lightSpecularLoc, 1.0f, 1.0f, 1.0f); glUniform3f(lightPosLoc, lightPos.x, lightPos.y, lightPos.z); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, textures["container2_diffuseMap"]); glActiveTexture(GL_TEXTURE1); glBindTexture(GL_TEXTURE_2D, textures["container2_specularMap"]); glBindVertexArray(VAO); glDrawArrays(GL_TRIANGLES, 0, 36); glBindVertexArray(0); /* Light Source */ lightShader.use(); view = viewCamera.getViewMatrix(); viewLoc = glGetUniformLocation(lightShader.Program, "view"); glUniformMatrix4fv(viewLoc, 1, GL_FALSE, glm::value_ptr(view)); projection = glm::perspective(glm::radians(45.0f), screenWidth / screenHeight, 0.1f, 100.0f); projectionLoc = glGetUniformLocation(lightShader.Program, "projection"); glUniformMatrix4fv(projectionLoc, 1, GL_FALSE, glm::value_ptr(projection)); glm::mat4 newModel; newModel = glm::mat4(); newModel = glm::translate(newModel, lightPos); newModel = glm::scale(newModel, glm::vec3(0.2f)); modelLoc = glGetUniformLocation(lightShader.Program, "model"); glUniformMatrix4fv(modelLoc, 1, GL_FALSE, glm::value_ptr(newModel)); glBindVertexArray(lightVAO); glDrawArrays(GL_TRIANGLES, 0, 36); glBindVertexArray(0); glfwSwapBuffers(window); } }