Point Robot::getFrontLeft() const
{
// x and y are pointing to top left now
int x = position.x - (size.x / 2);
int y = position.y - (size.y / 2);
Point originalFrontLeft( x, y);
double angle = Shape2DUtils::getAngle( front) + 0.5 * PI;
Point frontLeft( (originalFrontLeft.x - position.x) * std::cos( angle) - (originalFrontLeft.y - position.y) * std::sin( angle) + position.x, (originalFrontLeft.y - position.y) * std::cos( angle)
+ (originalFrontLeft.x - position.x) * std::sin( angle) + position.y);
return frontLeft;
}
void Pyramid::createPyramid()
{
//Vertices
Vect apex(0.0f, 7.07f, 0.0f);
Vect frontLeft(-5.0f, 0.0f, 5.0f);
Vect frontRight(5.0f, 0.0f, 5.0f);
Vect backLeft(-5.0f, 0.0f, -5.0f);
Vect backRight(5.0f, 0.0f, -5.0f);
//Normals
Vect frontNormal = (frontRight - frontLeft).cross(apex - frontRight);
Vect rightNormal = (backRight - frontRight).cross(apex - backRight);
Vect backNormal = (backLeft - backRight).cross(apex - backLeft);
Vect leftNormal = (frontLeft - backLeft).cross(apex - frontLeft);
Vect bottomNormal1 = (frontLeft - frontRight).cross(backLeft - frontLeft);
Vect bottomNormal2 = (backRight - backLeft).cross(frontRight - backRight);
pyramidBatch.Begin(GL_TRIANGLES, 18, 1);
//Front side of pyramid
//Front left
pyramidBatch.Normal3f(frontNormal[x], frontNormal[y], frontNormal[z]);
pyramidBatch.MultiTexCoord2f(0, 0.0f, 0.0f);
pyramidBatch.Vertex3f(frontLeft[x], frontLeft[y], frontLeft[z]);
//Front right
pyramidBatch.Normal3f(frontNormal[x], frontNormal[y], frontNormal[z]);
pyramidBatch.MultiTexCoord2f(0, 1.0f, 0.0f);
pyramidBatch.Vertex3f(frontRight[x], frontRight[y], frontRight[z]);
//Apex
pyramidBatch.Normal3f(frontNormal[x], frontNormal[y], frontNormal[z]);
pyramidBatch.MultiTexCoord2f(0, 0.5f, 1.0f);
pyramidBatch.Vertex3f(apex[x], apex[y], apex[z]);
//Right side of pyramid
//Front right
pyramidBatch.Normal3f(rightNormal[x], rightNormal[y], rightNormal[z]);
pyramidBatch.MultiTexCoord2f(0, 0.0f, 0.0f);
pyramidBatch.Vertex3f(frontRight[x], frontRight[y], frontRight[z]);
//Back right
pyramidBatch.Normal3f(rightNormal[x], rightNormal[y], rightNormal[z]);
pyramidBatch.MultiTexCoord2f(0, 1.0f, 0.0f);
pyramidBatch.Vertex3f(backRight[x], backRight[y], backRight[z]);
//Apex
pyramidBatch.Normal3f(rightNormal[x], rightNormal[y], rightNormal[z]);
pyramidBatch.MultiTexCoord2f(0, 0.5f, 1.0f);
pyramidBatch.Vertex3f(apex[x], apex[y], apex[z]);
//Back side of pyramid
//Back right
pyramidBatch.Normal3f(backNormal[x], backNormal[y], backNormal[z]);
pyramidBatch.MultiTexCoord2f(0, 0.0f, 0.0f);
pyramidBatch.Vertex3f(backRight[x], backRight[y], backRight[z]);
//Back left
pyramidBatch.Normal3f(backNormal[x], backNormal[y], backNormal[z]);
pyramidBatch.MultiTexCoord2f(0, 1.0f, 0.0f);
pyramidBatch.Vertex3f(backLeft[x], backLeft[y], backLeft[z]);
//Apex
pyramidBatch.Normal3f(backNormal[x], backNormal[y], backNormal[z]);
pyramidBatch.MultiTexCoord2f(0, 0.5f, 1.0f);
pyramidBatch.Vertex3f(apex[x], apex[y], apex[z]);
//Left side of pyramid
//Back left
pyramidBatch.Normal3f(leftNormal[x], leftNormal[y], leftNormal[z]);
pyramidBatch.MultiTexCoord2f(0, 0.0f, 0.0f);
pyramidBatch.Vertex3f(backLeft[x], backLeft[y], backLeft[z]);
//Front left
pyramidBatch.Normal3f(leftNormal[x], leftNormal[y], leftNormal[z]);
pyramidBatch.MultiTexCoord2f(0, 1.0f, 0.0f);
pyramidBatch.Vertex3f(frontLeft[x], frontLeft[y], frontLeft[z]);
//Apex
pyramidBatch.Normal3f(backNormal[x], backNormal[y], backNormal[z]);
pyramidBatch.MultiTexCoord2f(0, 0.5f, 1.0f);
pyramidBatch.Vertex3f(apex[x], apex[y], apex[z]);
//Bottom 1 of pyramid
//Front right
pyramidBatch.Normal3f(bottomNormal1[x], bottomNormal1[y], bottomNormal1[z]);
pyramidBatch.MultiTexCoord2f(0, 1.0f, 0.0f);
pyramidBatch.Vertex3f(frontRight[x], frontRight[y], frontRight[z]);
//Front left
pyramidBatch.Normal3f(bottomNormal1[x], bottomNormal1[y], bottomNormal1[z]);
pyramidBatch.MultiTexCoord2f(0, 0.0f, 0.0f);
pyramidBatch.Vertex3f(frontLeft[x], frontLeft[y], frontLeft[z]);
//Back left
pyramidBatch.Normal3f(bottomNormal1[x], bottomNormal1[y], bottomNormal1[z]);
pyramidBatch.MultiTexCoord2f(0, 0.0f, 1.0f);
pyramidBatch.Vertex3f(backLeft[x], backLeft[y], backLeft[z]);
//Bottom 2 of pyramid
//Back left
pyramidBatch.Normal3f(bottomNormal2[x], bottomNormal2[y], bottomNormal2[z]);
pyramidBatch.MultiTexCoord2f(0, 0.0f, 1.0f);
pyramidBatch.Vertex3f(backLeft[x], backLeft[y], backLeft[z]);
//Back right
pyramidBatch.Normal3f(bottomNormal2[x], bottomNormal2[y], bottomNormal2[z]);
pyramidBatch.MultiTexCoord2f(0, 1.0f, 1.0f);
pyramidBatch.Vertex3f(backRight[x], backRight[y], backRight[z]);
//Front right
pyramidBatch.Normal3f(bottomNormal2[x], bottomNormal2[y], bottomNormal2[z]);
pyramidBatch.MultiTexCoord2f(0, 1.0f, 0.0f);
pyramidBatch.Vertex3f(frontRight[x], frontRight[y], frontRight[z]);
pyramidBatch.End();
}
/**
* Task to handle the display of LEDs
*/
void LEDs::LEDRunner() {
// This is kind of ugly :(
auto ds = DriverStation::GetInstance();
int mode = 0;
//float hueOutput = 0;
auto leds = LEDController::GetInstance();
leds->SetBrightness(30);
LEDStrip frontLeft(0, 8);
LEDStrip rearLeft(8, 16);
LEDStrip rearRight(24, 16);
LEDStrip frontRight(40, 8);
std::vector<Util::Color> tmp;
while (taskRunning.test_and_set()) {
Wait(0.2);
if (ds->IsDisabled()) {
tmp.clear(); tmp.resize(16, 0xffff00);
if (!ds->IsDSAttached()) {
// Not attached to the DS, so, alternate yellow/off
for (int i = 0; i < 8; i++) {
tmp[i * 2] = 0x000000;
}
}
rearLeft.Set(tmp);
rearRight.Set(tmp);
tmp.resize(8);
frontRight.Set(tmp);
frontLeft.Set(tmp);
mode = Auton::GetInstance()->GetMode();
tmp.clear();
tmp.resize(3);
tmp[0] = mode & 1 ? 0xffff00 : 0x00;
tmp[1] = mode & 2 ? 0xffff00 : 0x00;
tmp[2] = mode & 4 ? 0xffff00 : 0x00;
rearRight.Set(tmp, 0);
tmp.clear(); tmp.resize(6);
tmp[0] = ds->IsDSAttached() ? 0x00ff00 : 0x00;
tmp[1] = ds->IsFMSAttached() ? 0x00ff00 : 0x00;
float hueOutput = Interpolate(ds->GetBatteryVoltage(), 12.0f, 13.0f, 0.0f, 120.0f);
//hueOutput = ((int)hueOutput + 5) % 360;
//float hueOutput
//printf("Voltage: %f (%f) ", ds->GetBatteryVoltage(), hueOutput);
tmp[2] = Util::Color(hueOutput, 1.0, 0.75);
//Util::RGB c = tmp[2];
//printf("(%d, %d, %d)\n", c.r, c.g, c.b);
auto position = ds->GetLocation();
auto alliance = ds->GetAlliance();
Util::Color allianceColor = alliance == DriverStation::kRed ? 0xff0000 : (alliance == DriverStation::kBlue ? 0x0000ff : 0xffff00);
if (position) {
tmp[2 + position] = allianceColor;
//} else {
//position += 1;
//tmp[2 + position] = allianceColor;
//tmp[2 + position + 1] = allianceColor;
//tmp[2 + position + 2] = allianceColor;
}
rearRight.Set(tmp, 3);
rearRight.Show();
} else {
if (ds->IsAutonomous()) {
auto alliance = ds->GetAlliance();
Util::Color allianceColor = alliance == DriverStation::kRed ? 0xff0000 : (alliance == DriverStation::kBlue ? 0x0000ff : 0xffff00);
tmp.clear();
tmp.resize(16, allianceColor);
rearLeft.Set(tmp);
rearRight.Set(tmp);
tmp.resize(8);
frontLeft.Set(tmp);
frontRight.Set(tmp);
} else if (ds->IsOperatorControl()) {
double timeLeft = ds->GetMatchTime();
tmp.clear();
tmp.resize(16, 0xffff00);
if (timeLeft < 0 || timeLeft > 30) {
// All yellow
} else if (timeLeft > 20) {
for (int i = 0; i < 8; i++) {
tmp[i * 2] = 0x00ff00;
}
} else if (timeLeft > 5) {
// off and green->red
float hueOutput = Interpolate((float)timeLeft, 5.0f, 20.f, 0.0f, 120.0f);
for (int i = 0; i < 8; i++) {
tmp[i * 2 + 1] = 0x00;
tmp[i * 2] = Util::Color(hueOutput, 1.0, 0.75);
//tmp[i * 2] = 0x00ff00;
}
} else {
// Just red
tmp.clear();
tmp.resize(16, 0xff0000);
}
if (Grabber::GetInstance()->Get(Grabber::kMini)) {
tmp[0] = 0xff00ff;
tmp[15] = 0xff00ff;
}
if (!Grabber::GetInstance()->Get(Grabber::kMain)) {
tmp[1] = 0x00ffff;
tmp[14] = 0x00ffff;
}
rearLeft.Set(tmp);
rearRight.Set(tmp);
tmp.resize(8);
if (Grabber::GetInstance()->Get(Grabber::kMini)) {
tmp[7] = 0xff00ff;
}
if (!Grabber::GetInstance()->Get(Grabber::kMain)) {
tmp[6] = 0x00ffff;
}
frontLeft.Set(tmp);
frontRight.Set(tmp);
//printf("Match Time: %f\n", ds->GetMatchTime());
}
}
frontLeft.Show();
rearLeft.Show();
frontRight.Show();
rearRight.Show();
}
}
int main()
{
//Create window, and limit frame rate
sf::RenderWindow window (sf::VideoMode(800, 600, 32), "Game", sf::Style::Default);
window.setFramerateLimit(60);
//------------------------TEXTURES------------------------------
//Declare textures
sf::Texture texture;
sf::Texture texture1;
sf::Texture texture2;
//Load image
if(!texture.loadFromFile("Sprites/main.png"))
{
return 1;
}
if(!texture1.loadFromFile("Sprites/background.png"))
{
return 1;
}
if(!texture2.loadFromFile("Sprites/house.png"))
{
return 1;
}
//------------------------SPRITES--------------------------
//Creates and places the sprites
sf::Sprite sprite;
sf::Sprite background;
sf::Sprite house;
sprite.setPosition(400, 300);
background.setPosition(0, 0);
house.setPosition(440, 300);
//Loads texture into sprite
sprite.setTexture(texture);
background.setTexture(texture1);
house.setTexture(texture2);
//-------------------------RECTANGLES--------------------------------
//Declares the rectangles
sf::IntRect front(1, 1, 18, 24);
sf::IntRect back (20, 1, 18, 24);
sf::IntRect left (20, 26, 18, 24);
sf::IntRect right (1, 26, 18, 24);
//Steps
sf::IntRect frontLeft(39, 1, 18, 24);
sf::IntRect frontRight(39, 26, 18, 24);
sf::IntRect backLeft();
sf::IntRect backRight();
sf::IntRect leftLeft();
sf::IntRect leftRight();
sf::IntRect rightLeft();
sf::IntRect rightRight();
sf::IntRect backgroundRect (0, 0, 800, 600);
sf::IntRect houseRect (0, 0, 17, 22);
//Crop sprites using rectangles defined above
sprite.setTextureRect(front);
background.setTextureRect(backgroundRect);
house.setTextureRect(houseRect);
//-----------------------SOUND------------------------------------------------------
//Declare the Sound Buffer
sf::SoundBuffer footstepsBuffer;
sf::SoundBuffer bumpBuffer;
//Loads the sound file
if(!footstepsBuffer.loadFromFile("Sounds/footsteps.wav"))
{
return 1;
}
if(!bumpBuffer.loadFromFile("Sounds/bump.wav"))
{
return 1;
}
//Declare sound
sf::Sound footsteps;
sf::Sound bump;
//Load Buffer into Sound
footsteps.setBuffer(footstepsBuffer);
bump.setBuffer(bumpBuffer);
//-------------------------------MAIN-----------------------------
//Main window loop
while(window.isOpen())
{
sf::Event event;
//Vectors used for collision
sf::Vector2i spritePosition(sprite.getPosition());
sf::Vector2i backgroundPosition(background.getPosition());
sf::Vector2i housePosition(house.getPosition());
//Sprite Vectors
sf::Vector2i backVector(back.width, back.height);
sf::Vector2i frontVector(front.width, front.height);
sf::Vector2i rightVector(right.width, right.height);
sf::Vector2i leftVector(left.width, left.height);
//House Vectors
sf::Vector2i houseVector(houseRect.width, houseRect.height);
while(window.pollEvent(event))
{
if(event.type == sf::Event::Closed)
{
window.close();
}
if(event.key.code == sf::Keyboard::Insert)
{
sf::Image screenshot = window.capture();
screenshot.saveToFile("Screenshot.png");
}
}
//-----------------------------------MOVEMENT----------------------------------------
if(sf::Keyboard::isKeyPressed(sf::Keyboard::Up))
{
//Change to stepping sprite
Sleep(250);
sprite.setTextureRect(back);
sprite.move(0, -24);
//Redeclaring the collision textures
sf::Vector2i spritePosition(sprite.getPosition());
sf::Vector2i housePosition(house.getPosition());
if(collision(spritePosition, backVector, housePosition, houseVector) == true)
{
sprite.move(0, 24);
bump.play();
}
else
{
footsteps.play();
}
}
else if(sf::Keyboard::isKeyPressed(sf::Keyboard::Down))
{
//Change to stepping sprite
Sleep(250);
sprite.setTextureRect(front);
sprite.move(0, 24);
//Redeclaring the collision textures
sf::Vector2i spritePosition(sprite.getPosition());
sf::Vector2i housePosition(house.getPosition());
if(collision(spritePosition, frontVector, housePosition, houseVector) == true)
{
sprite.move(0, -24);
bump.play();
}
else
{
footsteps.play();
}
}
else if(sf::Keyboard::isKeyPressed(sf::Keyboard::Right))
{
//Change to stepping sprite
Sleep(250);
sprite.setTextureRect(right);
sprite.move(19, 0);
//Redeclaring the collision textures
sf::Vector2i spritePosition(sprite.getPosition());
sf::Vector2i housePosition(house.getPosition());
if(collision(spritePosition, leftVector, housePosition, houseVector) == true)
{
sprite.move(-19, 0);
bump.play();
}
else
{
footsteps.play();
}
}
else if(sf::Keyboard::isKeyPressed(sf::Keyboard::Left))
{
//Change to stepping sprite
Sleep(250);
sprite.setTextureRect(left);
sprite.move(-19, 0);
//Redeclaring the collision textures
sf::Vector2i spritePosition(sprite.getPosition());
sf::Vector2i housePosition(house.getPosition());
if(collision(spritePosition, rightVector, housePosition, houseVector) == true)
{
sprite.move(19, 0);
bump.play();
}
else
{
footsteps.play();
}
}
//Draw sequence
window.clear(); //(Red, Green, Blue, (optional) Alpha) Alpha is transperency
//Draw....
window.draw(background);
window.draw(house);
window.draw(sprite);
window.display();
std::cout << x << std::endl;
std::cout << y << std::endl;
}
return 0;
}
