void distance_between2(float lat1, const float long1, float lat2,
const float long2, float * distance, float * bearing) {
//courtesy of http://www.movable-type.co.uk/scripts/latlong.html
float dLat = radians(lat1 - lat2);
float dLong = radians(long1 - long2);
float sindLong = mySin(dLong / 2);
float sindLat = mySin(dLat / 2);
lat1 = radians(lat1);
lat2 = radians(lat2);
float cosLat1 = myCos(lat1);
float cosLat2 = myCos(lat2);
float a = (sindLat * sindLat) + (sindLong * sindLong * cosLat1 * cosLat2);
float sa = mySqrt(a);
float c = 2 * fast_atan2f(sa, mySqrt(1 - a));
*distance = c * 6372.795;
}
double addsaw(double phase)
{
unsigned char j,k=1;
double out = 0;
for(j=0;j<MAX_N_HARMONICS;j++)
{
out += k*mySin(0,0,j*phase)/(j+1);
k = -k;
}
}
uint8_t Spiral::animate(COLOUR *arrayP, uint8_t fadeIn, uint8_t fadeOut, BUCK startBuck){
if(fadeIn){
begin();
memset(arrayP, 0, SIZE3*sizeof(COLOUR));
colourPos = 0;
}
memset(arrayP, 0, SIZE3*sizeof(COLOUR));
//Calculate frame
for(uint8_t z = bottom; z < top; z++){
for(uint8_t i = 0; i < 4; i++){
Y = myCos(phase + myMap(z, 0, SIZE-1, 0, 2*myPI) + i*myPI/8);
X = mySin(phase + myMap(z, 0, SIZE-1, 0, 2*myPI) + i*myPI/8);
Y = myMap(Y, -1.1, 0.9, narrow, (float)SIZE-1-narrow);
X = myMap(X, -1.1, 0.9, narrow, (float)SIZE-1-narrow);
arrayP[ar((uint8_t)X, (uint8_t)Y, z)] = colourWheel.get_colour(colourPos + 10*z);
}
}
increment_colour_pos(2);
//Count periods
phase += myPI/5*speed;
if(phase >= 2*myPI){
phase -= 2*myPI;
//Fade out to the top
if(fadeOut){
bottom<2?bottom++:bottom;
top>5?top--:top;
speed+=0.6;
narrow+=0.5;
if(narrow == 2){
endBuck.colour_pos = colourPos;
fadeOutDone = 1;
}
}
//Fade in from the bottom
if(!fadeInDone){
top++;
if(top >= SIZE){
fadeInDone = 1;
}
}
}
return fadeOutDone;
}
void distance_between(float lat1, const float long1, float lat2,
const float long2, float * distance, int * bearing) {
//courtesy of http://arduiniana.org/libraries/tinygps/
float delta = radians(long1 - long2);
float sdlong = mySin(delta);
float cdlong = myCos(delta);
lat1 = radians(lat1);
lat2 = radians(lat2);
float slat1 = mySin(lat1);
float clat1 = myCos(lat1);
float slat2 = mySin(lat2);
float clat2 = myCos(lat2);
delta = (clat1 * slat2) - (slat1 * clat2 * cdlong);
float x = delta ;
float y = sdlong * clat2;
delta = sq(delta);
delta += sq(clat2 * sdlong);
delta = mySqrt(delta);
float denom = (slat1 * slat2) + (clat1 * clat2 * cdlong);
delta = fast_atan2f(delta, denom);
*distance = delta * 6372.795;
x = (180.0 * (fast_atan2f(y, x)/PI)) ;
*bearing = ((int) -x + 360)%360 ;
}
void ColourWheel::fill_colour_wheel(void){
float red, green, blue;
float c, s;
int32_t phase = 0;
int16_t I = 0;
while(phase < COLOUR_WHEEL_LENGTH){
s = (1<<BAM_RESOLUTION)*mySin(myPI*(3*phase-I*COLOUR_WHEEL_LENGTH)/(2*COLOUR_WHEEL_LENGTH));
c = (1<<BAM_RESOLUTION)*myCos(myPI*(3*phase-I*COLOUR_WHEEL_LENGTH)/(2*COLOUR_WHEEL_LENGTH));
red = (I==0?1:0)*s + (I==1?1:0)*c;
green = (I==1?1:0)*s + (I==2?1:0)*c;
blue = (I==2?1:0)*s + (I==0?1:0)*c;
array[phase] = {red, green, blue};
if(++phase >= (1+I)*COLOUR_WHEEL_LENGTH/3){
I++;
}
}
}
float myTan(float x){
return mySin(x)/myCos(x);
}
//FAST COSINE APPROX
float myCos(float x){
return mySin(x+myPI/2);
}
