#include "SPI.h"
#include "ILI9341_t3.h"
// For optimized ILI9341_t3 library
//!!!! EDIT THIS !!!!!!!!!!!
#define TFT_DC 15
#define TFT_CS 10
#define TFT_RST 255 // 255 = unused, connect to 3.3V
#define TFT_MOSI 11
#define TFT_SCLK 13
#define TFT_MISO 12
ILI9341_t3 tft = ILI9341_t3(TFT_CS, TFT_DC, TFT_RST, TFT_MOSI, TFT_SCLK, TFT_MISO);
const int stepsize = 15;
const int scale = 80;
const int center = 120;
float pitch = 30;
float roll = 0;
float heading = 0;
float zpoint(float longditude, float latitude) //returns depth information, used to mask rear of sphere
{
return 1 * cosf(longditude + heading ) * fabsf(cosf(latitude)) * cosf(pitch) + sinf(pitch) * -sinf(latitude);
}
float xpoint(float longditude, float latitude) //get x values from lat/long/heading
{
return scale * sinf(longditude + heading) * cosf(latitude);
}
float ypoint(float longditude, float latitude) //get y value from lat/long/heading and pitch
{
return scale * cosf(longditude + heading ) * sinf(pitch) * cosf(latitude) + scale * sinf(latitude) * cosf(pitch);
}
void placeLine(float startLong, float startLat, float endLong, float endLat, uint16_t color)
{ //applies roll to final co-ordinates via vector conversion
if (zpoint(startLong, startLat) > 0 && zpoint(endLong, endLat) > 0) {
float startDistanceFromCenter = sqrtf(sq(xpoint(startLong, startLat)) + sq(ypoint(startLong, startLat)));
float endDistanceFromCenter = sqrtf(sq(xpoint(endLong, endLat)) + sq(ypoint(endLong, endLat)));
float startAngle = atan2f(ypoint(startLong, startLat), xpoint(startLong, startLat));
float endAngle = atan2f(ypoint(endLong, endLat), xpoint(endLong, endLat));
int rotatedXStart = center + floorf(startDistanceFromCenter * cosf(roll + startAngle ));
int rotatedYStart = center + floorf(startDistanceFromCenter * sinf(roll + startAngle ));
int rotatedXEnd = center + floorf(endDistanceFromCenter * cosf(roll + endAngle));
int rotatedYEnd = center + floorf(endDistanceFromCenter * sinf(roll + endAngle));
tft.drawLine(rotatedXStart, rotatedYStart, rotatedXEnd, rotatedYEnd, color);
}
}
void drawBall(float pitchIn, float headingIn, float rollIn) //produces geometry
{
static float oldPitch, oldHeading, oldRoll;
if (oldPitch != pitchIn || oldHeading != headingIn || oldRoll != rollIn) {
for (int i = 0; i < floor(360 / stepsize); i++) {
float angle = float(i * stepsize) / 180 * PI;
int meridiansize = floor(90 / stepsize);
for (int l = -meridiansize; l < meridiansize + 1; l++) {
pitch = oldPitch;
heading = oldHeading;
roll = oldRoll;
float currantLat = float(l) * stepsize / 180 * PI;
placeLine(angle, currantLat, angle + PI / 180 * stepsize, currantLat, ILI9341_BLUE);
placeLine(angle, currantLat, angle, currantLat + PI / 180 * stepsize, ILI9341_BLUE);
pitch = pitchIn;
heading = headingIn;
roll = rollIn;
placeLine(angle, currantLat, angle + PI / 180 * stepsize, currantLat, ILI9341_BLACK);
placeLine(angle, currantLat, angle, currantLat + PI / 180 * stepsize, ILI9341_BLACK);
}
}
oldPitch = pitchIn;
oldHeading = headingIn;
oldRoll = rollIn;
}
}
void loop()
{
float m = millis();
drawBall(sinf(m / 4000), m / 2000, sinf(m / 2000) / 2);
}
void setup() {
SPI.begin();
tft.begin();
tft.fillScreen(ILI9341_BLUE);
delay(1000);
Serial.print(F_BUS);
}