bool Circle::calcTangentIntersectionPoints(const Vector2D startPoint, Vector2D &point1, Vector2D &point2){
if(isInside(startPoint)){
// Startpoint is inside circle -> there are no tangent interception points
return(false);
}
//float d = posCenter.getLength()-startPoint.getLength();
float d = (posCenter-startPoint).getLength();
float r = radius;
float alphaRad = asin(r/d);
float p = sqrt(d*d-r*r);
point1.setX(cos(alphaRad)*p);
point1.setY(sin(alphaRad)*p);
point2.setX(cos(-alphaRad)*p);
point2.setY(sin(-alphaRad)*p);
point1=point1.rotate((posCenter-startPoint).getDirection());
point2=point2.rotate((posCenter-startPoint).getDirection());
point1+=startPoint;
point2+=startPoint;
return(true);
}
Vector2D ScenRectangle::calcShift(Point2D point,Vector2D lv,float radius)
{
Vector2D shift;
if(point.x<=0)
{
shift.x=-(radius);
shift.y=0;
shift.rotate(rotAngle);
}
if(point.x>=width)
{
shift.x=radius;
shift.y=0;
shift.rotate(rotAngle);
}
if(point.y<=0)
{
shift.y=-radius;
shift.x=0;
shift.rotate(rotAngle);
}
if(point.y>=height)
{
shift.y=radius;
shift.x=0;
shift.rotate(rotAngle);
}
return shift;
}//Fine calcShift
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
void Chassis::setWheelsSpeed() {
for (iterator i = wheels.begin(); i != wheels.end(); i++) {
Vector2D rot = (*i)->r;
rot.rotate(M_PI / 2);
rot.mul(angular_speed);
(*i)->v = v;
(*i)->v.add(rot);
}
}
Vector2D CameraController::worldToScreen( const Vector2D& worldPos )
{
Vector2D screenPos ( (worldPos.x - m_position.x),
(worldPos.y - m_position.y) );
screenPos.rotate( -m_rotation );
screenPos.x /= m_viewWidth;
screenPos.y /= -m_viewHeight;
screenPos.x += 0.5f;
screenPos.y += 0.5f;
return screenPos;
}
bool SkillCircle::execute(RobotCommand &rc)
{
if(_rid==-1) return false;
Vector2D vc;
vc = _wm->ourRobot[_rid].pos.loc;
vc.setLength(500);
vc.rotate(10);
rc.FinalPos.loc=vc;
rc.TargetPos.loc=vc;
rc.FinalPos.dir=(_wm->ball.pos.loc - _wm->ourRobot[_rid].pos.loc).dir().radian();
rc.TargetPos.dir=(_wm->ball.pos.loc - _wm->ourRobot[_rid].pos.loc).dir().radian();
rc.Speed=1;
return true;
}
void PositionManager::setPoint(PointOrient2D<float> &po, Vector2D<float> &v, float omega) {
if (simu) {
pos_cur = po;
return;
}
std::cout << "Pas simu" << std::endl;
// TODO add pid
// convert in robot frame
v.rotate(-offset_angle);
pnm.sendSetPointSpeed(v, omega);
}
void Cluster::predictRelativePosition(){
if(estimatedVelocity.x == 0.0f && estimatedVelocity.y == 0.0f) //virgin state
estimatedVelocity = velocity;
else{ // else estimate new velocity from acceleration...
if(velocity.normalizedDot(estimatedVelocity) <= -20.7){
acceleration.reset();
}
else{
// calculate new relative velocity
float angle = 0; //calc rotation angle to align velocity vector to x-axis
Vector2D xAxis(1,0);
if(velocity.norm() > 0)
angle = acos(velocity.normalizedDot(xAxis));
if(velocity.y > 0)
angle = 2*PI - angle;
Vector2D forward = estimatedVelocity.rotate(angle);
Vector2D target = velocity.rotate(angle);
float m = target.norm()-forward.norm();
//filter and set acceleration magnitude...
float a = 0;
if(target.norm() > 0)
a = 1-target.normalizedDot(xAxis);
if(target.y < 0)
a = -a;
//filter and set acceleration angle...
}
//estimate new position
float angle = 0;
Vector2D xAxis(1,0);
if(velocity.norm() > 0)
angle = acos(velocity.normalizedDot(xAxis));
if(velocity.y > 0)
angle = 2*PI - angle;
Vector2D forward = estimatedVelocity.rotate(angle);
float m = forward.norm() + acceleration.m * delta;
float a = acceleration.a * delta;
Vector2D newVelocity;
newVelocity.x = m * cos(abs(a));
int sgn = 0;
if(a != 0.0f)
sgn = a < 0 ? -1 : 1;
newVelocity.y = m * sin(abs(a)) * sgn;
newVelocity = newVelocity.rotate(-angle);
// filter - synthetic velocity
Vector2D measured;
Vector2D predicted;
if(measured.normalizedDot(predicted) < 0.5){
acceleration.reset();
estimatedVelocity = velocity;
}
}
// compute new forward velocity... => jaer code: SyntheticVelocityPredictor
}
/*!
*/
void
Bhv_GoalieChaseBall::doGoToCatchPoint( PlayerAgent * agent,
const Vector2D & target_point )
{
const ServerParam & SP = ServerParam::i();
const WorldModel & wm = agent->world();
double dash_power = 0.0;
Vector2D rel = target_point - wm.self().pos();
rel.rotate( - wm.self().body() );
AngleDeg rel_angle = rel.th();
const double angle_buf
= std::fabs( AngleDeg::atan2_deg( SP.catchableArea() * 0.9, rel.r() ) );
dlog.addText( Logger::TEAM,
__FILE__": GoToCatchPoint. (%.1f, %.1f). angle_diff=%.1f. angle_buf=%.1f",
target_point.x, target_point.y,
rel_angle.degree(), angle_buf );
agent->debugClient().setTarget( target_point );
// forward dash
if ( rel_angle.abs() < angle_buf )
{
dash_power = std::min( wm.self().stamina() + wm.self().playerType().extraStamina(),
SP.maxDashPower() );
dlog.addText( Logger::TEAM,
__FILE__": forward dash" );
agent->debugClient().addMessage( "GoToCatch:Forward" );
agent->doDash( dash_power );
}
// back dash
else if ( rel_angle.abs() > 180.0 - angle_buf )
{
dash_power = SP.minDashPower();
double required_stamina = ( SP.minDashPower() < 0.0
? SP.minDashPower() * -2.0
: SP.minDashPower() );
if ( wm.self().stamina() + wm.self().playerType().extraStamina()
< required_stamina )
{
dash_power = wm.self().stamina() + wm.self().playerType().extraStamina();
if ( SP.minDashPower() < 0.0 )
{
dash_power *= -0.5;
if ( dash_power < SP.minDashPower() )
{
dash_power = SP.minDashPower();
}
}
}
dlog.addText( Logger::TEAM,
__FILE__": back dash. power=%.1f",
dash_power );
agent->debugClient().addMessage( "GoToCatch:Back" );
agent->doDash( dash_power );
}
// forward dash turn
else if ( rel_angle.abs() < 90.0 )
{
dlog.addText( Logger::TEAM,
__FILE__": turn %.1f for forward dash",
rel_angle.degree() );
agent->debugClient().addMessage( "GoToCatch:F-Turn" );
agent->doTurn( rel_angle );
}
else
{
rel_angle -= 180.0;
dlog.addText( Logger::TEAM,
__FILE__": turn %.1f for back dash",
rel_angle.degree() );
agent->debugClient().addMessage( "GoToCatch:B-Turn" );
agent->doTurn( rel_angle );
}
agent->setNeckAction( new Neck_TurnToBall() );
}
