float ScenRectangle::intersect(Node *node)
{
////////////////
if(nodeOverlap(*node))
{
Point2D pt(node->getPosition());
//Trasformo il punto nelle coordinate locali del rettangolo
pt.translate(-this->p.x,-this->p.y);
pt.rotate(-rotAngle);
Vector2D lv;
lv=node->getVelocity();
lv.rotate(-rotAngle);
Vector2D shift=calcShift(pt,lv,node->getRadius()+0.01f);
node->setPosition(shift);
//return 0;
}
////////////////////////////
float radius=node->getRadius();
ScenRectangle boundRect(Point2D(p.x-radius,p.y-radius),
width+radius*2,height+radius*2);
//Creo il raggio e lo trasformo nelle coodinate locali del rettangolo
Ray2D ray=transformRay(Ray2D(node->getPosition(),node->getVelocity()));
//Lo devo traslare perchè calcolo
//l'intersezione con un rettangolo allargato
ray.o.x+=radius; ray.o.y+=radius;
//Calcolo l'intersezione
return boundRect.rayLocalIntersect(ray);
}//Fine intersect
GhostCellImmersedBoundary::BcStencil::BcStencil(const Cell &cell,
const ImmersedBoundaryObject &ibObj,
const FiniteVolumeGrid2D &grid,
const Vector2D &r)
:
cell_(std::cref(cell))
{
bp_ = ibObj.shape().intersections(Ray2D(cell.centroid(), r))[0];
ip_ = ibObj.isInIb(cell) ? 2. * bp_ - cell.centroid() : 2. * cell.centroid() - bp_;
nw_ = ibObj.nearestEdgeUnitNormal(bp_);
initCoeffMatrix(grid);
}
bool SkillGoalie::execute(RobotCommand &rc)
{
if(_rid==-1) return false;
Ray2D ballRay(_wm->ball.pos.loc,AngleDeg(_wm->ball.vel.loc.dir().degree()));
Line2D lineGoal(Vector2D(-3025,-2025),Vector2D(-3025,2025));
Vector2D gdir, gloc;
gdir=Vector2D();
gloc=Vector2D();
int kickerID=-1;
float minDist=9000000,temp;
Vector2D ballContact = ballRay.intersection(lineGoal);
if(_wm->ball.pos.loc.dist2(Vector2D(-3025,175))<=640000 || _wm->ball.pos.loc.dist2(Vector2D(-3025,-175))<=640000 || (_wm->ball.pos.loc.x<=-2225 && _wm->ball.pos.loc.y<=175 && _wm->ball.pos.loc.y>=-175))
{
if(ballContact.y>=-350 && ballContact.y<=350)
{
gloc=_wm->ball.pos.loc+_wm->ball.vel.loc.setLengthVector(90);
}
else
{
gloc=_wm->ball.pos.loc;
gloc.x-=90;
}
gdir.x=1;
gdir.y=0;
}
else
{
if(ballContact.y>=-350 && ballContact.y<=350)
{
gloc=ballContact-_wm->ball.vel.loc.setLengthVector(350);
gdir=-_wm->ball.vel.loc;
}
else
{
for(int i=0; i<PLAYERS_MAX_NUM;i++)
{
if(_wm->oppRobot[i].isValid)
{
temp=_wm->oppRobot[i].pos.loc.dist2(_wm->ball.pos.loc);
if(temp<minDist)
{
minDist=temp;
kickerID=i;
}
}
}
if(kickerID!=-1)
{
if(_wm->oppRobot[kickerID].pos.dir>=-M_PI_2 && _wm->oppRobot[kickerID].pos.dir<=M_PI_2)
{
Vector2D rTarget,rTemp;
Ray2D tRay(_wm->oppRobot[kickerID].pos.loc,AngleDeg(_wm->oppRobot[kickerID].pos.dir*AngleDeg::RAD2DEG));
minDist=100000000;
for(int i=0;i<PLAYERS_MAX_NUM;i++)
{
rTemp=tRay.intersection(Ray2D(_wm->oppRobot[i].pos.loc,AngleDeg(_wm->oppRobot[i].vel.loc.dir().degree())));
temp=_wm->ball.pos.loc.dist2(rTemp);
if(temp<minDist)
{
rTarget=rTemp;
minDist=temp;
}
}
gdir=(rTarget-Vector2D(-3025,0)).setLengthVector(500);
gloc=Vector2D(-3025,0)+gdir;
}
else
{
Vector2D ins;
Line2D l1(_wm->oppRobot[kickerID].pos.loc,AngleDeg(_wm->oppRobot[kickerID].pos.dir*AngleDeg::RAD2DEG));
ins = l1.intersection(lineGoal);
if(ins.y>350)
{
ins.y=350;
}
else if(ins.y<-350)
{
ins.y=-350;
}
ins.x=-3025;
gdir=(_wm->ball.pos.loc-ins).setLengthVector(500);
gloc=gdir+ins;
}
}
else
{
gdir=(_wm->ball.pos.loc-Vector2D(-3025,0)).setLengthVector(500);
gloc=Vector2D(-3025,0)+gdir;
}
}
}
if(gloc.x<-2980)
gloc.x=-2980;
rc.FinalPos.loc=gloc;
rc.TargetPos.loc=gloc;
rc.FinalPos.dir=gdir.dir().radian();
rc.TargetPos.dir=gdir.dir().radian();
rc.Speed=1;
return true;
}
