static bool orderTRange(skiatest::Reporter* reporter, const SkDQuad& quad1, const SkDQuad& quad2,
double r, TRange* result) {
SkTArray<double, false> t1Array, t2Array;
orderQuads(reporter, quad1, r, &t1Array);
orderQuads(reporter,quad2, r, &t2Array);
if (!t1Array.count() || !t2Array.count()) {
return false;
}
SkTQSort<double>(t1Array.begin(), t1Array.end() - 1);
SkTQSort<double>(t2Array.begin(), t2Array.end() - 1);
double t1 = result->tMin1 = t1Array[0];
double t2 = result->tMin2 = t2Array[0];
double a1 = quadAngle(reporter,quad1, t1);
double a2 = quadAngle(reporter,quad2, t2);
if (approximately_equal(a1, a2)) {
return false;
}
bool refCCW = angleDirection(a1, a2);
result->t1 = t1;
result->t2 = t2;
result->tMin = SkTMin(t1, t2);
result->a1 = a1;
result->a2 = a2;
result->ccw = refCCW;
return true;
}
void FlightController::turnTowardsAngle(double target, double hoverTime) {
if (hoverTime == 0)
hoverTime = 5;
double orientation = navData->getRotation();
double difference = angleDifference(orientation, target);
int direction = angleDirection(orientation, target);
ROS_INFO("Orientation: %3.1f\ttarget: %3.1f\tdifference: %3.1f\tdirection: %d\t", orientation,target, difference, direction );
int last_ts = (int) (ros::Time::now().toNSec() / 1000000);
int time_counter = 0;
//rotateDrone(direction * 1.0);
int debug_counter = 0;
while (true) {
int time = (int) (ros::Time::now().toNSec() / 1000000);
time_counter += (time - last_ts);
last_ts = time;
debug_counter++;
if(debug_counter > 50) {
ROS_INFO("LOOP orientation: %3.1f \ttarget: %3.0f \tdifference: %3.1f \tdirection: %d \ttime_counter: %d", orientation, target, difference, direction, time_counter);
debug_counter = 0;
}
if (time_counter > 150) {
hoverDuration(1);
time_counter = 0;
last_ts = (int) (ros::Time::now().toNSec() / 1000000);
}
if( difference < (time_counter*time_counter)/3000+4){
break;
}
orientation = navData->getRotation();
difference = angleDifference(orientation, target);
direction = angleDirection(orientation, target);
rotateDrone(direction * 1.0);
}
ROS_INFO("VICTORY! orientation: %6.1f\ttarget: %6.1f", orientation, target);
hoverDuration(hoverTime);
orientation = navData->getRotation();
ROS_INFO("VICTORY AFTERMATH! orientation: %6.1f\ttarget: %6.1f", orientation, target);
}
