bool Barrier::detactCollision() { float innerR,outerR; innerR = _parameter.innerR - GameScene::sharedInstance()->movedLength; outerR = _parameter.outerR - GameScene::sharedInstance()->movedLength; if(innerR < 0.1) innerR = 0.1; Sprite* s = GameScene::sharedInstance()->getSprite(); CCSize size = CCDirector::sharedDirector()->getWinSize(); if( s->getRadius() >= outerR*size.width/2 || s->getRadius() + s->getHeight() <= innerR*size.width/2 ) return false; float fs = formatAngle(s->getDirectAngle()); if( 60*_parameter.start <= fs && 60*_parameter.end >= fs ) return true; return false; }
/*int last_ts = (int) (ros::Time::now().toNSec() / 1000000); int time = (int) (ros::Time::now().toNSec() / 1000000); time_counter += (time - last_ts); last_ts = time;*/ void FlightController::turnDegrees(double degrees) { double orientation = navData->getRotation(); double target = formatAngle(orientation + degrees); turnTowardsAngle(target, 2); }
/** * Formats the given angle with the given format. * * @param angle The angle (always in rad). * @param format Format of the string. * @param prec Precision of the value (e.g. 0.001 or 1/128 = 0.0078125) * * @ret String with the formatted angle. */ QString RS_Units::formatAngle(double angle, RS2::AngleFormat format, int prec) { QString ret; double value; switch (format) { case RS2::Surveyors: case RS2::DegreesDecimal: case RS2::DegreesMinutesSeconds: value = RS_Math::rad2deg(angle); break; case RS2::Radians: value = angle; break; case RS2::Gradians: value = RS_Math::rad2gra(angle); break; default: RS_DEBUG->print(RS_Debug::D_WARNING, "RS_Units::formatAngle: Unknown Angle Unit"); return ""; break; } switch (format) { case RS2::DegreesDecimal: case RS2::Radians: case RS2::Gradians: ret = RS_Math::doubleToString(value, prec); if (format==RS2::DegreesDecimal) ret+=QChar(0xB0); if (format==RS2::Radians) ret+="r"; if (format==RS2::Gradians) ret+="g"; break; case RS2::DegreesMinutesSeconds: { int vDegrees, vMinutes; double vSeconds; QString degrees, minutes, seconds; vDegrees = (int)floor(value); vMinutes = (int)floor((value - vDegrees) * 60.0); vSeconds = (value - vDegrees - (vMinutes/60.0)) * 3600.0; seconds = RS_Math::doubleToString(vSeconds, (prec>1 ? prec-2 : 0)); if(seconds=="60") { seconds="0"; ++vMinutes; if(vMinutes==60) { vMinutes=0; ++vDegrees; } } if (prec==0 && vMinutes>=30.0) { vDegrees++; } else if (prec==1 && vSeconds>=30.0) { vMinutes++; } degrees.setNum(vDegrees); minutes.setNum(vMinutes); switch (prec) { case 0: ret = degrees + QChar(0xB0); break; case 1: ret = degrees + QChar(0xB0) + " " + minutes + "'"; break; default: ret = degrees + QChar(0xB0) + " " + minutes + "' " + seconds + "\""; break; } } break; case RS2::Surveyors: { QString prefix,suffix; int quadrant; quadrant = ((int)floor(value)/90); switch(quadrant){ case 0: prefix="N"; suffix="E"; break; case 1: prefix="S"; suffix="E"; value=180. - value; break; case 2: prefix="S"; suffix="W"; value=value - 180.; break; case 3: prefix="N"; suffix="W"; value=360. - value; break; } ret = prefix+formatAngle(RS_Math::deg2rad(value),RS2::DegreesMinutesSeconds,prec)+suffix; ret.replace(QChar(0xB0),"d"); ret.replace(" ",""); } break; default: break; } return ret; }