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);
}
}
