inline uint16_t getGpsDistance() { uint32_t lat = (((uint32_t)gpsLatitude_bp / 100) * 1000000) + (((gpsLatitude_bp % 100) * 10000 + gpsLatitude_ap)*5)/3; uint32_t lon = (((uint32_t)gpsLongitude_bp / 100) * 1000000) + (((gpsLongitude_bp % 100) * 10000 + gpsLongitude_ap)*5)/3; // lat=50247528; //50247528 1000 = ca 111 m // lon=11387561; //11397561 1000 = ca 70 m uint32_t lat_len = (lat > pilotLatitude) ? lat - pilotLatitude : pilotLatitude - lat; uint32_t lon_len = (lon > pilotLongitude) ? lon - pilotLongitude : pilotLongitude - lon; uint32_t alt_len = (abs_alt > pilotAltitude) ? abs_alt - pilotAltitude : pilotAltitude - abs_alt; lat_len = (lat_len * metersPerDegreeLat) / 1000; // * 1113 lon_len = (lon_len * metersPerDegreeLon) / 10000; // * 1113 * cos(lat) uint32_t temp_len = (sqrt32(lat_len*lat_len + lon_len*lon_len)); //um den Gesamtabstand mit der Höhe zu rechnen muß erneut quadriert und Wurzel gezogen werden lon_len = sqrt32(temp_len*temp_len + alt_len*alt_len); // Ergebnis in 1/10tel Meter return lon_len; }
bool InputManager::moveToPosition(Vector3<s16>* currentPosition, float32 speed, Vector3<s16>& destinationOut) { s16 from_x = currentPosition->x(); s16 from_y = currentPosition->y(); touchPosition heldPosition; if(keysHeld() & KEY_TOUCH){ touchRead(&heldPosition); s16 diff_x = (heldPosition.px - from_x); s16 diff_y = (heldPosition.py - from_y); u32 magnitude = sqrt32(diff_x * diff_x + diff_y * diff_y); if (magnitude < 2) return NULL; diff_x = round(diff_x * speed / magnitude); diff_y = round(diff_y * speed / magnitude); destinationOut.setX((u16)(from_x + diff_x)); destinationOut.setY((u16)(from_y + diff_y)); return true; } else { destinationOut = *currentPosition; return false; } }
uint16_t acc_magnitude(int8_t* angle) { int16_t acc[3], a, alpha; /* briefly turn on accelerometer */ acc_write(ACC_REG_CTRL_REG1, 0x97); timer_wait(MILLISECONDS(2)); acc_read(ACC_REG_OUT_X, sizeof(acc), (uint8_t*)&acc); acc_write(ACC_REG_CTRL_REG1, 0x00); /* get acceleration vector magnitude */ a = sqrt32( ((int)acc[0])*acc[0] + ((int)acc[1])*acc[1] + ((int)acc[2])*acc[2] )/64; /* calculate tag angle */ if(angle) { if(!a) alpha = 127; else { alpha = (acc[2]*2)/a; if(alpha>127) alpha=127; else if(alpha<-127) alpha=-127; } *angle = asin7deg(alpha); } return a; }
/* Triangle median */ static int triangle_median(int f1, int f2, int f3) { // Median to a = sqrt((2*b*b + 2*c*c - *a*a) / 4) // Median to b = sqrt((2*a*a + 2*c*c - *b*b) / 4) // Median to c = sqrt((2*a*a + 2*b*b - *c*c) / 4) ret = ( 2*f1*f1 + 2*f2*f2 - f3*f3 ) / 4; return sqrt32(ret); }
dm_time_t dm_mech_g1_seek_3pt_curve(struct dm_disk_if *d, struct dm_mech_state *begin, struct dm_mech_state *end, int rw) { dm_time_t result; int cyls; struct dm_mech_g1 *m = (struct dm_mech_g1 *)d->mech; cyls = abs(end->cyl - begin->cyl); if (cyls == 0) { result = 0; } else { result = m->seekone; result += m->seekavg * (cyls - 1); result += m->seekfull * sqrt32(cyls - 1); } return result; }
void genTextures() { // int pal_addr = gluTexLoadPal( &paldata[0], 8, GL_RGB8_A5 ); u16 paldata[8]; for(int i = 0; i <8; i++) paldata[i] = 0xFFFF; u8 texdata[64*64]; for(int x = 0; x < 64; x++) for(int y = 0; y < 64; y++) { texdata[x+y*64] = (u8) 1; } glBindTexture(0, textureIDS[0]); glTexImage2D(0, 0, GL_RGB256, TEXTURE_SIZE_64, TEXTURE_SIZE_64, 0, TEXGEN_TEXCOORD, &texdata); glColorTableEXT( 0, 0, 8, 0, 0, paldata ); genTextureFromImg(1, 0); genTextureFromImg(2, 1); genTextureFromImg(3, 2); u8* texdata2 = new u8[256*64]; u8* text = (u8*) introtextBitmap; for(int x = 0; x < 256; x++) for(int y = 0; y < 64; y++) { u8 val = 0; if(text[x+y*256] == 0x01) val = 31<<3; texdata2[x+y*256] = val; } glBindTexture(4, textureIDS[4]); glTexImage2D(0, 0, GL_RGB8_A5, TEXTURE_SIZE_256, TEXTURE_SIZE_64, 0, TEXGEN_TEXCOORD, texdata2); glColorTableEXT( 0, 0, 8, 0, 0, paldata ); genTextureFromImg(6, 3); genTextureFromImg(8, 4); for(int x = 0; x < 64; x++) for(int y = 0; y < 64; y++) texdata[x+y*64] = (u8) (31)<< 3 | 1; glBindTexture(7, textureIDS[7]); glTexImage2D(0, 0, GL_RGB8_A5, TEXTURE_SIZE_64, TEXTURE_SIZE_64, 0, TEXGEN_TEXCOORD, &texdata); glColorTableEXT( 0, 0, 8, 0, 0, paldata ); for(int x = 0; x < 64; x++) for(int y = 0; y < 64; y++) { int dist = (x-32)*(x-32)+(y-32)*(y-32); dist = sqrt32(dist); // dist /= 32; if(dist>31) dist = 31; if(dist<0) dist = 0; if(dist !=31) texdata[x+y*64] = (u8) 31<< 3 | 1; else texdata[x+y*64] = (u8) 0; } glBindTexture(9, textureIDS[9]); glTexImage2D(0, 0, GL_RGB8_A5, TEXTURE_SIZE_64, TEXTURE_SIZE_64, 0, TEXGEN_TEXCOORD, &texdata); glColorTableEXT( 0, 0, 9, 0, 0, paldata ); genTextureFromImg(10, 5); genTextureFromImg(11, 6); genTextureFromImg(12, 7); genTextureFromImg(13, 8); delete[] texdata2; }
void_t vt_inclined_plane(virtualtouch_s* vt) { dbg_function(); int tmp; int_t alpha = mm_alpha(vt->countalpha); int_t ahpla = 1000 - mm_alpha(vt->countalpha); tmp = (vt->distance.dx + vt->distance.sx + vt->distance.dw + vt->distance.up) / 4; if ( vt->eamflags & VT_EAMF_PLANE ) vt->p3d.z = (alpha * tmp + ahpla * vt->p3d.z)/1000; else vt->p3d.z = tmp; dbg_info("plane: [z=%3d]", vt->p3d.z); int_t h, f, l; int_t b = vt->vxdistance.x; if (vt->distance.sx > vt->distance.dx) { h = vt->distance.sx - vt->distance.dx; f = 1; } else { h = vt->distance.dx - vt->distance.sx; f = -1; } l = sqrt32(b*b + h*h); tmp = ((vt->peso.x * h) / l) * f; if ( vt->eamflags & VT_EAMF_PLANE ) vt->p3d.x = (alpha * tmp + ahpla * vt->p3d.x)/1000; else vt->p3d.x = tmp; dbg_info("plane: [p=%3d z=%3d h=%3d b=%3d l=%3d f=%3d][px=%3d]", vt->peso.x, vt->p3d.z, h, b, l, f, vt->p3d.x); b = vt->vxdistance.y; if (vt->distance.up > vt->distance.dw) { h = vt->distance.up - vt->distance.dw; f = -1; } else { h = vt->distance.dw - vt->distance.up; f = 1; } l = sqrt32(b*b + h*h); tmp = ((vt->peso.y * h) / l) * f; if ( vt->eamflags & VT_EAMF_PLANE ) vt->p3d.y = (alpha * tmp + ahpla * vt->p3d.y)/1000; else vt->p3d.y = tmp; dbg_info("plane: [p=%3d z=%3d h=%3d b=%3d l=%3d f=%3d][px=%3d]", vt->peso.y, vt->p3d.z, h, b, l, f, vt->p3d.y); dbg_return(0); }
static int calc_distance(point_2d* a, point_2d* b) { ret = (a->x - b->x) * (a->x - b->x) + (a->y - b->y) * (a->y - b->y); return sqrt32(ret); }