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;
}
