/*!
*/
Circle2D
Circle2D::circumcircle( const Vector2D & p0,
const Vector2D & p1,
const Vector2D & p2 )
{
Vector2D center = Triangle2D::circumcenter( p0, p1, p2 );
if ( ! center.isValid() )
{
std::cerr << "Circle2D::circumcircle()"
<< " ***ERROR*** failed to get circumcenter from "
<< p0 << p1 << p2
<< std::endl;
return Circle2D();
}
return Circle2D( center, center.dist( p0 ) );
}
// Helper function to compute the minimal circle that contains the given three points.
// To avoid extra Sqrt() operations in hot inner loops, this function returns a Circle2D structure that has its radius squared (caller needs to compute circle.r = Sqrt(circle.r) to use)
// This is essentially a fast version of Circle2D::OptimalEnclosingCircle(a, b, c)
static Circle2D MakeCircleSq(float AB, float AC, const float2 &ab, const float2 &ac)
{
const float AB_AC = Dot(ab,ac);
float denom = AB*AC - AB_AC*AB_AC;
if (Abs(denom) < 1e-5f) // Each of a, b and c lie on a straight line?
{
if (AB_AC > 0.f) return AB > AC ? Circle2D(ab*0.5f, AB*0.25f) : Circle2D(ac*0.5f, AC*0.25f);
else return Circle2D((ab+ac)*0.5f, (AB + AC - 2.f*AB_AC)*0.25f);
}
denom = 0.5f / denom;
float s = (AC * AB - AB_AC * AC) * denom;
if (s < 0.f) return Circle2D(ac * 0.5f, AC * 0.25f);
else
{
float t = (AC * AB - AB_AC * AB) * denom;
if (t < 0.f) return Circle2D(ab * 0.5f, AB * 0.25f);
else if (s + t > 1.f) return Circle2D((ab + ac) * 0.5f, (AB + AC - 2.f*AB_AC)*0.25f);
else
{
const float2 center = s * ab + t * ac;
return Circle2D(center, center.LengthSq());
}
}
}
Circle2D Circle2D::OptimalEnclosingCircle(const float2 *pointArray, int numPoints)
{
assert(pointArray || numPoints == 0);
// Special case handling for 0-3 points.
switch(numPoints)
{
case 0: return Circle2D(float2::nan, -FLOAT_INF);
case 1: return Circle2D(pointArray[0], 0.f);
case 2:
{
float2 center = (pointArray[0] + pointArray[1]) * 0.5f;
float r = Sqrt(Max(center.DistanceSq(pointArray[0]), center.DistanceSq(pointArray[1]))) + 1e-5f;
return Circle2D(center, r);
}
case 3: return Circle2D::OptimalEnclosingCircle(pointArray[0], pointArray[1], pointArray[2]);
}
// Start off by computing the convex hull of the points, which prunes many points off from the problem space.
float2 *pts = new float2[numPoints];
memcpy(pts, pointArray, sizeof(float2)*numPoints);
numPoints = float2_ConvexHullInPlace(pts, numPoints);
// Use initial bounding box extents (min/max x and y) as fast guesses for the optimal
// bounding sphere extents.
for(int i = 0; i < numPoints; ++i)
{
if (pts[0].x < pts[i].x) Swap(pts[0], pts[i]);
if (pts[1].x > pts[i].x) Swap(pts[1], pts[i]);
if (pts[2].y < pts[i].y) Swap(pts[2], pts[i]);
if (pts[3].y > pts[i].y) Swap(pts[3], pts[i]);
}
// Compute the minimal enclosing circle for the first three points.
Circle2D minCircle = OptimalEnclosingCircle(pts[0], pts[1], pts[2]);
float r2 = minCircle.r*minCircle.r;
// Iteratively include the remaining points to the minimal circle.
for(int i = 3; i < numPoints; ++i)
{
// If the new point is already inside the current bounding circle, it can be skipped
float d2 = (pts[i] - minCircle.pos).LengthSq();
if (d2 <= r2)
continue;
// The new point is outside the current bounding circle defined by pts[0]-pts[2].
// Compute a new bounding circle that encloses pts[0]-pts[2] and the new point pts[i].
// A circle is defined by at most three points, so one of the resulting points is redundant.
// Swap points around so that pts[0]-pts[2] define the new minimum circle, and pts[i] will
// have the redundant point.
int redundantIndex;
minCircle = SmallestCircleSqEnclosing4Points(pts[i], pts[0], pts[1], pts[2], redundantIndex);
minCircle.pos += pts[i];
Swap(pts[i], pts[redundantIndex]);
// For robustness, apply epsilon after square root.
minCircle.r = Sqrt(minCircle.r) + 1e-3f;
r2 = minCircle.r*minCircle.r;
// Start again from scratch: pts[0]-pts[2] now has the new candidate.
i = 2;
mathassert1(minCircle.Contains(pts[0], 1e-3f), minCircle.SignedDistance(pts[0]));
mathassert1(minCircle.Contains(pts[1], 1e-3f), minCircle.SignedDistance(pts[1]));
mathassert1(minCircle.Contains(pts[2], 1e-3f), minCircle.SignedDistance(pts[2]));
}
delete[] pts;
return minCircle;
}
void Shape2D::SubtractCircle(Vector2D center, double r)
{
_ncircles.append(Circle2D(center, r));
}
void Shape2D::AddCircle(Vector2D center, double r)
{
_pcircles.append(Circle2D(center, r));
}
RobotCommand TacticTransferObject::getCommand()
{
AllInMargin=true;
RobotCommand rc;
if(!wm->ourRobot[id].isValid) return rc;
rc.useNav=true;
rc.maxSpeed = 1;
rc.fin_pos.loc=wm->endPoint;//Vector2D(300,0);
int object;
addData();
mergeData();
sortData();
// if(wm->balls.size() > 0)
// {
// qDebug()<< " BALLL . X = " << wm->balls.at(0)->pos.loc.x << " BALLL . Y = " << wm->balls.at(0)->pos.loc.y;
// qDebug() << " MAX x : " << region[1].maxX() << " MIN X : " << region[1].minX() ;
// qDebug() << " MAX y : " << region[1].maxY() << " MIN y : " << region[1].minY() ;
// if(region[0].IsInside(wm->balls.at(0)->pos.loc)) qDebug() << " THE BALLLLL ISSS INNNNN SIDE !!!!!!!!!!!!!!!!!!!!!!1";
// }
index = -1;
for(int i=0;i<mergedList.size();i++)
{
// qDebug() << i << " AT : (" << mergedList.at(i).pos.x << "," << mergedList.at(i).pos.y << ")";
temp=0;
if(!region[mergedList.at(i).goalRegion].IsInside(mergedList.at(i).pos) && !IsInmargins(mergedList.at(i).pos,300))
{
//qDebug() <<" OBJECT : " << mergedList.at(i).pos.x << " ------ Y = " << mergedList.at(i).pos.y;// TOOOOOOOOOOOOOOOOOOOSHE !!!!!!!" << index ;
// AllInMargin=false;
index=i;
goalRegion=mergedList.at(i).goalRegion;
temp=1;
break;
}
}
for(int i=0; i<mergedList.size(); i++)
{
if(!IsInmargins(mergedList.at(i).pos,300))
{
AllInMargin=false;
}
}
if(AllInMargin)
{
for(int i=0;i<mergedList.size();i++)
{
if(!region[mergedList.at(i).goalRegion].IsInside(mergedList.at(i).pos))
{
index=i;
goalRegion=mergedList.at(i).goalRegion;
break;
}
}
}
// if(index ==-1)
// {
// for(int i=0;i<wm->Chasbideh.size(); i++)
// {
// if(!region[0].IsInside(wm->Chasbideh.at(i).position) && !region[1].IsInside(wm->Chasbideh.at(i).position))
// {
// //qDebug() <<" OBJECT : " << mergedList.at(i).pos.x << " ------ Y = " << mergedList.at(i).pos.y;// TOOOOOOOOOOOOOOOOOOOSHE !!!!!!!" << index ;
// index=i;
// goalRegion=0;//mergedList.at(i).goalRegion;
// temp=1;
// break;
// }
// }
// }
// qDebug() << mergedList.size() << " MERGED SIZE " ;
if(index != -1)
{
Vector2D point2 = mergedList.at(index).pos;
Vector2D diff2 = region[goalRegion].center() - point2;
bool reach=false;
if(temp!=0)
{
switch(state)
{
case 0:{ //Go Behind the Object
Vector2D space2=diff2;
space2.setLength(300);
rc.maxSpeed=1.4;
rc.useNav = true;
rc.fin_pos.loc=point2 - space2;
rc.fin_pos.dir=diff2.dir().radian();
object=findnearestObject(mergedShapeList,wm->ourRobot[id].pos.loc);
if(object!=-1) ObsC=Circle2D(mergedShapeList.at(object).position,(mergedShapeList.at(object).roundedRadios+ROBOT_RADIUS+150));
rc.fin_pos.loc=AvoidtoEnterCircle(ObsC,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,150);
if(reach) state = 1;
}
break;
case 1:{//Ready to Push
rc.useNav = false;
rc.maxSpeed=1.2;
rc.fin_pos.loc.x=point2.x - (100 + ROBOT_RADIUS)*(diff2.x)/(diff2.length()); // 100 >> Rounded Radius
rc.fin_pos.loc.y=point2.y - (100 + ROBOT_RADIUS)*(diff2.y)/(diff2.length());
rc.fin_pos.dir=diff2.dir().radian();
if(((wm->ourRobot[id].pos.loc-point2).length())>400) state=0;
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,100);
if(reach)
state = 2;
}
break;
case 2:{//Push
//Vector2D diff2 = region2.center() - wm->ourRobot[id].pos.loc ;
rc.useNav = false;
rc.maxSpeed=1;
//if(diff2.length() > 1500) diff2.setLength(1500);
// if(((wm->ourRobot[id].pos.loc-point2).length())>400) state=0;
diff2.setLength(300);
if(((wm->ourRobot[id].pos.loc-point2).length())>600) state=0;
if(((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length())<50) state=0;
Vector2D o2r = ( point2 - wm->ourRobot[id].pos.loc );
if(fabs(wm->ourRobot[id].pos.dir - o2r.dir().radian()) > AngleDeg::deg2rad(40))
{
qDebug() << " !!!! Out OF Direction !!!! " ;
state=1;//4;
}
rc.fin_pos.loc=point2 + diff2;//5;
rc.fin_pos.dir=diff2.dir().radian();
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,10);
if(reach)
state = 3;
}
break;
case 3:{//Release
if(region[goalRegion].IsInside(point2))
{
//qDebug() << " INNNNNNNNNNNN SIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIDE !!!";
//if(index==agentsR1.size()) rc.fin_pos.loc=Vector2D(0,0);
if(temp==0)
{
rc.fin_pos.loc=Vector2D(0,0);
break;
}
//agentsR1.takeFirst();
//index++;
}
//if(reach)
state = 0;
}
break;
case 4:{// back behind the object
}
break;
}
}
Vector2D dlta;
double mrgn=300;
if(IsInmargins(point2,mrgn))
{
// qDebug() << " IS IN MARGIN !!!!!!!!!";
int side = ((point2.x-mean_x)/abs(point2.x-mean_x))*((point2.y-mean_y)/abs(point2.y-mean_y));
if(point2.x > MAX_X-mrgn || point2.x < MIN_X+mrgn) {
side *= ((point2.y-mean_y)/abs(point2.y-mean_y));
dlta=Vector2D(side*10,side*(ROBOT_RADIUS+/*mergedShapeList.at(index).roundedRadios+*/50));}
else if(point2.y > MAX_Y-mrgn || point2.y < MIN_Y+mrgn) {
side *=((point2.x-mean_x)/abs(point2.x-mean_x));
dlta=Vector2D(side*(ROBOT_RADIUS+/*mergedShapeList.at(index).roundedRadios+*/50),side*10);}
switch(statemargin)
{
case 0:{
rc.fin_pos.loc=point2+dlta;
// object=findnearestObject(mergedShapeList,wm->ourRobot[id].pos.loc);
// if(object!=-1) ObsC=Circle2D(mergedShapeList.at(object).position,(mergedShapeList.at(object).roundedRadios+ROBOT_RADIUS));
// rc.fin_pos.loc=AvoidtoEnterCircle(ObsC,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
// int rad = mergedShapeList.at(index).roundedRadios+ROBOT_RADIUS;
// Circle2D c(point2,rad);
// rc.fin_pos.loc=AvoidtoEnterCircle(c,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
// qDebug()<< "In Margins Pos : ball = ( " << point2.x << ","<< point2.y << ")";
// qDebug()<< "In Margins Pos : delta = ( " << dlta.x << ","<< dlta.y << ")";
// qDebug()<< "In Margins Pos : fin_pos = ( " << rc.fin_pos.loc.x << ","<<rc.fin_pos.loc.y << ")";
// qDebug()<< "In Margins Pos : Robot = ( " << wm->ourRobot[id].pos.loc.x << ","<<wm->ourRobot[id].pos.loc.y << ")";
rc.fin_pos.dir=dlta.dir().radian()-side*M_PI/2;
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos,rc.fin_pos,20,7);
// wm->ourRobot[id].pos.loc,rc.fin_pos.loc,200);
// qDebug() << "dist To final Pos : " << (wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length();
// qDebug() << " Avoided : " << Avoided << " reach" << reach;
if(reach) statemargin = 1;
}
break;
case 1:{
rc.fin_pos.dir = dlta.dir().radian() - side*0.9*M_PI ;
rc.fin_pos.loc=point2-dlta;
// qDebug() << "Fin_POS . dir = " << AngleDeg::rad2deg(rc.fin_pos.dir) << " ROBOT . dir = " << AngleDeg::rad2deg(wm->ourRobot[id].pos.dir);
if(((wm->ourRobot[id].pos.loc-point2).length())>300) statemargin=0;
double delta_ang=wm->ourRobot[id].pos.dir-rc.fin_pos.dir;
if (delta_ang > M_PI) delta_ang -= (M_PI * 2);
if (delta_ang < -M_PI) delta_ang += (M_PI * 2);
if(fabs(delta_ang) < AngleDeg::deg2rad(10)) statemargin=0;
rc.maxSpeed=1.7;
// bool chighz=wm->kn->ReachedToPos(wm->ourRobot[id].pos,rc.fin_pos,20,10);
// if(chighz) statemargin=0;
// if((wm->ourRobot[id].pos.loc.dir()-rc.fin_pos.dir)<AngleDeg::deg2rad(10)) statemargin=0;
// if(((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length())<250) state=0;
}
break;
}
}
// qDebug() << rc.fin_pos.loc.x << " ------- Y = " << rc.fin_pos.loc.y << " STATE = " << state;
// qDebug() << "STATE = " << state;
}
//rc.maxSpeed = 1.2;//rc.maxSpeed;
// rc.fin_pos.loc.x=rcpast.x + 0.1*(rc.fin_pos.loc.x-rcpast.x);
// rc.fin_pos.loc.y=rcpast.y + 0.1*(rc.fin_pos.loc.y-rcpast.y);
// rcpast=rc.fin_pos.loc;
// qDebug() << " INDEX = " << index ;
rc.maxSpeed/=1.4;
if(IsInmargins(wm->ourRobot[id].pos.loc,500)) rc.maxSpeed /= 1.5 ;
rc.fin_pos.loc=KeepInField(rc);
// qDebug() << " This Object Is For Region " << goalRegion ;
rc.useNav=false;
rc.isBallObs = false;
rc.isKickObs = true;
return rc;
}
