/*!
* \brief Creates a new arc in an element.
*/
ArcType *
CreateNewArcInElement (ElementType *Element,
Coord X, Coord Y,
Coord Width, Coord Height,
Angle angle, Angle delta, Coord Thickness)
{
ArcType *arc;
arc = g_slice_new0 (ArcType);
Element->Arc = g_list_append (Element->Arc, arc);
Element->ArcN ++;
/* set Delta (0,360], StartAngle in [0,360) */
if (delta < 0)
{
delta = -delta;
angle -= delta;
}
angle = NormalizeAngle (angle);
delta = NormalizeAngle (delta);
if (delta == 0)
delta = 360;
/* copy values */
arc->X = X;
arc->Y = Y;
arc->Width = Width;
arc->Height = Height;
arc->StartAngle = angle;
arc->Delta = delta;
arc->Thickness = Thickness;
arc->ID = ID++;
return arc;
}
// convert to human-friendly format
void CMappingDefinition::ToUI(CMappingDefinitionUI &mdui) const
{
// make a copy of parameters
FLOAT fUoS = md_fUoS;
FLOAT fUoT = md_fUoT;
FLOAT fVoS = md_fVoS;
FLOAT fVoT = md_fVoT;
// find size of mapping vectors
FLOAT fUSize = FLOAT(sqrt(fUoS*fUoS+fUoT*fUoT));
FLOAT fVSize = FLOAT(sqrt(fVoS*fVoS+fVoT*fVoT));
// find rotation of both vectors
ANGLE aURot = -(ATan2(fVoT, fVoS)-90.0f);
ANGLE aVRot = -(ATan2(fUoT, fUoS));
// use the found values
Snap(aURot, 0.001f);
Snap(aVRot, 0.001f);
mdui.mdui_aURotation= NormalizeAngle(aURot);
mdui.mdui_aVRotation= NormalizeAngle(aVRot);
mdui.mdui_fUStretch = 1/fUSize;
mdui.mdui_fVStretch = 1/fVSize;
mdui.mdui_fUOffset = md_fUOffset;
mdui.mdui_fVOffset = md_fVOffset;
}
UVector RotationHelper::NormalizeAngles(UVector angles)
{
angles.x = NormalizeAngle(angles.x);
angles.y = NormalizeAngle(angles.y);
angles.z = NormalizeAngle(angles.z);
return angles;
}
bool Blackguard::Utility::InsideAngle(float angle, float minAngle, float maxAngle)
{
angle = NormalizeAngle(angle);
minAngle = NormalizeAngle(minAngle);
maxAngle = NormalizeAngle(maxAngle);
if(minAngle < maxAngle)
return minAngle <= angle && maxAngle >= angle;
return minAngle <= angle || maxAngle >= angle;
}
/* TODO: this code is BROKEN in the case of non-circular arcs,
* and in the case that the arc thickness is greater than
* the radius.
*/
bool
IsPointOnArc (Coord X, Coord Y, Coord Radius, ArcTypePtr Arc)
{
/* Calculate angle of point from arc center */
double p_dist = Distance (X, Y, Arc->X, Arc->Y);
double p_cos = (X - Arc->X) / p_dist;
Angle p_ang = acos (p_cos) * RAD_TO_DEG;
Angle ang1, ang2;
/* Convert StartAngle, Delta into bounding angles in [0, 720) */
if (Arc->Delta > 0)
{
ang1 = NormalizeAngle (Arc->StartAngle);
ang2 = NormalizeAngle (Arc->StartAngle + Arc->Delta);
}
else
{
ang1 = NormalizeAngle (Arc->StartAngle + Arc->Delta);
ang2 = NormalizeAngle (Arc->StartAngle);
}
if (ang1 > ang2)
ang2 += 360;
/* Make sure full circles aren't treated as zero-length arcs */
if (Arc->Delta == 360 || Arc->Delta == -360)
ang2 = ang1 + 360;
if (Y > Arc->Y)
p_ang = -p_ang;
p_ang += 180;
/* Check point is outside arc range, check distance from endpoints */
if (ang1 >= p_ang || ang2 <= p_ang)
{
Coord ArcX, ArcY;
ArcX = Arc->X + Arc->Width *
cos ((Arc->StartAngle + 180) / RAD_TO_DEG);
ArcY = Arc->Y - Arc->Width *
sin ((Arc->StartAngle + 180) / RAD_TO_DEG);
if (Distance (X, Y, ArcX, ArcY) < Radius + Arc->Thickness / 2)
return true;
ArcX = Arc->X + Arc->Width *
cos ((Arc->StartAngle + Arc->Delta + 180) / RAD_TO_DEG);
ArcY = Arc->Y - Arc->Width *
sin ((Arc->StartAngle + Arc->Delta + 180) / RAD_TO_DEG);
if (Distance (X, Y, ArcX, ArcY) < Radius + Arc->Thickness / 2)
return true;
return false;
}
/* If point is inside the arc range, just compare it to the arc */
return fabs (Distance (X, Y, Arc->X, Arc->Y) - Arc->Width) < Radius + Arc->Thickness / 2;
}
double ShortestAngleDiff(double a1_rad, double a2_rad)
{
double a1_norm = NormalizeAngle(a1_rad, 0.0, 2.0 * M_PI);
double a2_norm = NormalizeAngle(a2_rad, 0.0, 2.0 * M_PI);
double dist = ShortestAngleDist(a1_rad, a2_rad);
if (ShortestAngleDist(a1_norm + dist, a2_norm) < ShortestAngleDist(a1_norm - dist, a2_norm)) {
return -dist;
}
else {
return dist;
}
}
Facing NounShip::getFacing( float direction, bool bTowards /*= true*/ )
{
// determine the facing from the direction
if ( bTowards )
direction += PI;
float angle = NormalizeAngle( direction );
//// build a vector from the direction of the impact
//Vector3 local( sinf(direction), 0, cosf(direction) );
//// check the facing
//float angle = atan2( local.x, local.z ); // value is always from -PI to PI
if ( angle > -PI4 )
{
if ( angle > PI4 )
{
if ( angle < (PI - PI4) )
return FACING_RIGHT;
return FACING_BACK;
}
return FACING_FRONT;
}
if ( angle > -(PI - PI4) )
return FACING_LEFT;
return FACING_BACK;
}
AnimateDir AnimGetDirFromAngle(float ang)
{
ang = NormalizeAngle(ang);
// rotate angle by 22.5:
ang += 22.5;
size_t quadrant = ang/45.0;
switch (quadrant)
{
case 0:
return ANIMDIR_N;
case 1:
return ANIMDIR_NW;
case 2:
return ANIMDIR_W;
case 3:
return ANIMDIR_SW;
case 4:
return ANIMDIR_S;
case 5:
return ANIMDIR_SE;
case 6:
return ANIMDIR_E;
case 7:
return ANIMDIR_NE;
case 8:
return ANIMDIR_N;
}
return ANIMDIR_N;
}
void Unit::updateAngle(float angle)
{
//update angle
m_positionAngle = NormalizeAngle(angle);
//update player position based on angle
m_position = getPositionByAngle(m_positionAngle);
}
float CBUtils::RandomAngle(float From, float To)
{
while(To < From)
{
To += 360;
}
return NormalizeAngle(RandomFloat(From, To));
}
void
FreeRotateElementLowLevel (DataType *Data, ElementType *Element,
Coord X, Coord Y,
double cosa, double sina, Angle angle)
{
/* solder side objects need a different orientation */
/* the text subroutine decides by itself if the direction
* is to be corrected
*/
#if 0
ELEMENTTEXT_LOOP (Element);
{
if (Data && Data->name_tree[n])
r_delete_entry (Data->name_tree[n], (BoxType *)text);
RotateTextLowLevel (text, X, Y, Number);
}
END_LOOP;
#endif
ELEMENTLINE_LOOP (Element);
{
free_rotate (&line->Point1.X, &line->Point1.Y, X, Y, cosa, sina);
free_rotate (&line->Point2.X, &line->Point2.Y, X, Y, cosa, sina);
SetLineBoundingBox (line);
}
END_LOOP;
PIN_LOOP (Element);
{
/* pre-delete the pins from the pin-tree before their coordinates change */
if (Data)
r_delete_entry (Data->pin_tree, (BoxType *)pin);
RestoreToPolygon (Data, PIN_TYPE, Element, pin);
free_rotate (&pin->X, &pin->Y, X, Y, cosa, sina);
SetPinBoundingBox (pin);
}
END_LOOP;
PAD_LOOP (Element);
{
/* pre-delete the pads before their coordinates change */
if (Data)
r_delete_entry (Data->pad_tree, (BoxType *)pad);
RestoreToPolygon (Data, PAD_TYPE, Element, pad);
free_rotate (&pad->Point1.X, &pad->Point1.Y, X, Y, cosa, sina);
free_rotate (&pad->Point2.X, &pad->Point2.Y, X, Y, cosa, sina);
SetLineBoundingBox ((LineType *) pad);
}
END_LOOP;
ARC_LOOP (Element);
{
free_rotate (&arc->X, &arc->Y, X, Y, cosa, sina);
arc->StartAngle = NormalizeAngle (arc->StartAngle + angle);
}
END_LOOP;
free_rotate (&Element->MarkX, &Element->MarkY, X, Y, cosa, sina);
SetElementBoundingBox (Data, Element, &PCB->Font);
ClearFromPolygon (Data, ELEMENT_TYPE, Element, Element);
}
int cPositioner::CalcLongitude(int HourAngle)
{
double Lat = RAD(Setup.SiteLat);
double Lon = RAD(Setup.SiteLon);
double Delta = RAD(HourAngle);
double Alpha = Delta - asin(sin(M_PI - Delta) * cos(Lat) * SAT_EARTH_RATIO);
int Sign = Setup.SiteLat >= 0 ? 1 : -1;
return NormalizeAngle(round(DEG(Lon - Sign * Alpha)));
}
bool AnyAngleAlgorithm::IsTautCornerTurn(const double x1, const double y1, const int x2, const int y2, const double x3, const double y3) const
{
// std::cout<<x2<<"\t"<<y2<<std::endl;
// Compute the angle of the vector (x2, y2) -> (x1, y1)
double theta_1 = GetAngle(x2, y2, x1, y1);
// std::cout<<"Theta1 = "<<theta_1<<std::endl;
// Compute the angle of the vector (x2, y2) -> (x3, y3)
double theta_2 = GetAngle(x2, y2, x3, y3);
// std::cout<<"Theta2 = "<<theta_2<<std::endl;
double theta_diff = NormalizeAngle(theta_2 - theta_1);
// std::cout<<"Theta diff = "<<theta_diff<<std::endl;
double theta_bisector;
if (theta_diff < 180)
theta_bisector = NormalizeAngle(theta_1 + theta_diff/2);
else
theta_bisector = NormalizeAngle(theta_1 + theta_diff/2 + 180);
// std::cout<<"Bisector = "<<theta_bisector<<std::endl;
switch ((int) theta_bisector/90) {
case 0:
return !IsTraversable(NorthEastCell(x2,y2));
case 1:
return !IsTraversable(NorthWestCell(x2,y2));
case 2:
return !IsTraversable(SouthWestCell(x2,y2));
case 3:
return !IsTraversable(SouthEastCell(x2,y2));
default:
return false;
}
return false;
}
POINT CalculateRandomPosition(const POINT& ptOrigin, int nRadiusMin, int nRadiusMax, int nAngleMin/*=0*/, int nAngleMax/*=360*/)
{
// Data validation
nRadiusMin = max(0, nRadiusMin);
nRadiusMax = max(0, nRadiusMax);
NormalizeAngle(nAngleMin);
NormalizeAngle(nAngleMax);
const int R1 = min(nRadiusMin, nRadiusMax);
const int R2 = max(nRadiusMin, nRadiusMax);
const int A1 = min(nAngleMin, nAngleMax);
const int A2 = max(nAngleMin, nAngleMax);
const int R = (R1 == R2) ? R1 : (R1 + (::rand() % (R2 - R1))); // Final radius
const int A = (A1 == A2) ? A1 : (A1 + (::rand() % (A2 - A1))); // Final angle
return CalculatePointOnTrack(ptOrigin, R, A);
}
// Update the rotation around Z if necessary
void ObjectOpenGL::SetZRotation(int angle)
{
NormalizeAngle(&angle);
if (angle != zRot)
{
zRot = angle;
emit zRotationChanged(angle);
updateGL();
}
}
int cPositioner::HorizonLongitude(ePositionerDirection Direction)
{
double Delta;
if (abs(Setup.SiteLat) <= SAT_VISIBILITY_LAT)
Delta = acos(SAT_EARTH_RATIO / cos(RAD(Setup.SiteLat)));
else
Delta = 0;
if ((Setup.SiteLat >= 0) != (Direction == pdLeft))
Delta = -Delta;
return NormalizeAngle(round(DEG(RAD(Setup.SiteLon) + Delta)));
}
double AnyAngleAlgorithm::GetAngle(const double x1, const double y1, const double x2, const double y2) const
{
// Normalize so that the vector is from (0,0) to (x,y)
double x = x2 - x1;
double y = y2 - y1;
// -y because the y coordinate for the map grows as we move South
double theta = atan2(-y,x) * 180 / 3.14159265358979323846;
return NormalizeAngle(theta);
}
POINT CalculatePointOnTrack(const POINT& ptOrigin, int nRadius, int nAngle)
{
if (nRadius == 0)
return ptOrigin;
NormalizeAngle(nAngle);
POINT pt;
pt.x = long(double(ptOrigin.x) + ::cos((double)nAngle * PI / 180.0) * (double)nRadius);
pt.y = long(double(ptOrigin.y) - ::sin((double)nAngle * PI / 180.0) * (double)nRadius);
return pt;
}
void RotateBeginAnim::animate(QTime cur_time){
AnimatorBase::animate(cur_time);
float time_part = this->start_time.msecsTo(cur_time) / anim_time;
if(time_part > 1){
time_part = 1.0;
}
this->animated->rotate[0] = this->rotate_start[0] + ((this->rotate_end[0] - this->rotate_start[0]) * time_part);
this->animated->rotate[1] = this->rotate_start[1] + ((this->rotate_end[1] - this->rotate_start[1]) * time_part);
NormalizeAngle(this->animated->rotate);
memcpy(this->animated->rotate_backup, this->animated->rotate, sizeof(float) * 3);
this->animated->zoom = this->zoom_start + ((this->zoom_end - this->zoom_start) * time_part);
}
/*!
* \brief Creates a new arc on a layer.
*/
ArcType *
CreateNewArcOnLayer (LayerType *Layer,
Coord X1, Coord Y1,
Coord width,
Coord height,
Angle sa,
Angle dir, Coord Thickness,
Coord Clearance, FlagType Flags)
{
ArcType *Arc;
ARC_LOOP (Layer);
{
if (arc->X == X1 && arc->Y == Y1 && arc->Width == width &&
NormalizeAngle (arc->StartAngle) == NormalizeAngle (sa) &&
arc->Delta == dir)
return (NULL); /* prevent stacked arcs */
}
END_LOOP;
Arc = GetArcMemory (Layer);
if (!Arc)
return (Arc);
Arc->ID = ID++;
Arc->Flags = Flags;
Arc->Thickness = Thickness;
Arc->Clearance = Clearance;
Arc->X = X1;
Arc->Y = Y1;
Arc->Width = width;
Arc->Height = height;
Arc->StartAngle = sa;
Arc->Delta = dir;
SetArcBoundingBox (Arc);
if (!Layer->arc_tree)
Layer->arc_tree = r_create_tree (NULL, 0, 0);
r_insert_entry (Layer->arc_tree, (BoxType *) Arc, 0);
return (Arc);
}
void Unit::setTargetAngle(float targetAngle)
{
if (targetAngle != 2 * PI && targetAngle != m_targetAngle)
{
m_targetAngle = NormalizeAngle(targetAngle);
m_state = UNIT_STATE::MOVING;
}
else if (isPlayer() && targetAngle != m_targetAngle)
{
m_targetAngle = m_positionAngle;
m_state = UNIT_STATE::WAITING;
}
}
void
ghid_draw_arc (hidGC gc, Coord cx, Coord cy,
Coord xradius, Coord yradius, Angle start_angle, Angle delta_angle)
{
gint vrx, vry;
gint w, h, radius;
render_priv *priv = gport->render_priv;
w = gport->width * gport->view.coord_per_px;
h = gport->height * gport->view.coord_per_px;
radius = (xradius > yradius) ? xradius : yradius;
if (SIDE_X (cx) < gport->view.x0 - radius
|| SIDE_X (cx) > gport->view.x0 + w + radius
|| SIDE_Y (cy) < gport->view.y0 - radius
|| SIDE_Y (cy) > gport->view.y0 + h + radius)
return;
USE_GC (gc);
vrx = Vz (xradius);
vry = Vz (yradius);
if (gport->view.flip_x)
{
start_angle = 180 - start_angle;
delta_angle = -delta_angle;
}
if (gport->view.flip_y)
{
start_angle = -start_angle;
delta_angle = -delta_angle;
}
/* make sure we fall in the -180 to +180 range */
start_angle = NormalizeAngle (start_angle);
if (start_angle >= 180) start_angle -= 360;
gdk_draw_arc (gport->drawable, priv->u_gc, 0,
Vx (cx) - vrx, Vy (cy) - vry,
vrx * 2, vry * 2, (start_angle + 180) * 64, delta_angle * 64);
}
static STATUS_T CmdSplitTrack( wAction_t action, coOrd pos )
{
track_p trk0, trk1;
EPINX_T ep0;
int oldTrackCount;
int inx, mode, quad;
ANGLE_T angle;
switch (action) {
case C_START:
InfoMessage( _("Select track to split") );
case C_DOWN:
case C_MOVE:
return C_CONTINUE;
break;
case C_UP:
onTrackInSplit = TRUE;
trk0 = OnTrack( &pos, TRUE, TRUE );
if ( trk0 != NULL) {
if (!CheckTrackLayer( trk0 ) ) {
onTrackInSplit = FALSE;
return C_TERMINATE;
}
ep0 = PickEndPoint( pos, trk0 );
onTrackInSplit = FALSE;
if (ep0 < 0) {
return C_CONTINUE;
}
UndoStart( _("Split Track"), "SplitTrack( T%d[%d] )", GetTrkIndex(trk0), ep0 );
oldTrackCount = trackCount;
SplitTrack( trk0, pos, ep0, &trk1, FALSE );
UndoEnd();
return C_TERMINATE;
}
onTrackInSplit = FALSE;
return C_TERMINATE;
break;
case C_CMDMENU:
splitTrkTrk[0] = OnTrack( &pos, TRUE, TRUE );
if ( splitTrkTrk[0] == NULL )
return C_CONTINUE;
if ( splitPopupM[0] == NULL ) {
splitPopupM[0] = MenuRegister( "End Point Mode R-L" );
splitPopupMI[0][0] = wMenuToggleCreate( splitPopupM[0], "", _("None"), 0, TRUE, ChangeSplitEPMode, (void*)0 );
splitPopupMI[0][1] = wMenuToggleCreate( splitPopupM[0], "", _("Left"), 0, FALSE, ChangeSplitEPMode, (void*)1 );
splitPopupMI[0][2] = wMenuToggleCreate( splitPopupM[0], "", _("Right"), 0, FALSE, ChangeSplitEPMode, (void*)2 );
splitPopupMI[0][3] = wMenuToggleCreate( splitPopupM[0], "", _("Both"), 0, FALSE, ChangeSplitEPMode, (void*)3 );
splitPopupM[1] = MenuRegister( "End Point Mode T-B" );
splitPopupMI[1][0] = wMenuToggleCreate( splitPopupM[1], "", _("None"), 0, TRUE, ChangeSplitEPMode, (void*)0 );
splitPopupMI[1][1] = wMenuToggleCreate( splitPopupM[1], "", _("Top"), 0, FALSE, ChangeSplitEPMode, (void*)1 );
splitPopupMI[1][2] = wMenuToggleCreate( splitPopupM[1], "", _("Bottom"), 0, FALSE, ChangeSplitEPMode, (void*)2 );
splitPopupMI[1][3] = wMenuToggleCreate( splitPopupM[1], "", _("Both"), 0, FALSE, ChangeSplitEPMode, (void*)3 );
}
splitTrkEP[0] = PickEndPoint( pos, splitTrkTrk[0] );
angle = NormalizeAngle(GetTrkEndAngle( splitTrkTrk[0], splitTrkEP[0] ));
if ( angle <= 45.0 )
quad = 0;
else if ( angle <= 135.0 )
quad = 1;
else if ( angle <= 225.0 )
quad = 2;
else if ( angle <= 315.0 )
quad = 3;
else
quad = 0;
splitTrkFlip = (quad<2);
if ( (splitTrkTrk[1] = GetTrkEndTrk( splitTrkTrk[0], splitTrkEP[0] ) ) == NULL ) {
ErrorMessage( MSG_BAD_BLOCKGAP );
return C_CONTINUE;
}
splitTrkEP[1] = GetEndPtConnectedToMe( splitTrkTrk[1], splitTrkTrk[0] );
mode = 0;
if ( GetTrkEndOption( splitTrkTrk[1-splitTrkFlip], splitTrkEP[1-splitTrkFlip] ) & EPOPT_GAPPED )
mode |= 2;
if ( GetTrkEndOption( splitTrkTrk[splitTrkFlip], splitTrkEP[splitTrkFlip] ) & EPOPT_GAPPED )
mode |= 1;
for ( inx=0; inx<4; inx++ )
wMenuToggleSet( splitPopupMI[quad&1][inx], mode == inx );
wMenuPopupShow( splitPopupM[quad&1] );
break;
}
return C_CONTINUE;
}
double ShortestAngleDist(double a1_rad, double a2_rad)
{
double a1_norm = NormalizeAngle(a1_rad, 0.0, 2.0 * M_PI);
double a2_norm = NormalizeAngle(a2_rad, 0.0, 2.0 * M_PI);
return std::min(fabs(a1_norm - a2_norm), 2.0 * M_PI - fabs(a2_norm - a1_norm));
}
void ai09::NormalPlayAtt ( void )
{
ManageAttRoles ( );
debugDraw=true;
recievePass(dmf,PointOnConnectingLine(ball.Position, Vec2(side*field_width, 0), 2500));
debugDraw=false;
if (oneTouchType[attack]==allaf) {
ERRTSetObstacles ( attack , false , true , true , true );
OwnRobot[attack].face(Vec2(-side*field_width, 0));
//OwnRobot[robot_num].target.Angle=-90;
ERRTNavigate2Point ( attack , allafPos[attack] ,0 , 100,&VELOCITY_PROFILE_MAMOOLI);
if (timer.time()>2.5) {
oneTouchType[attack] = oneTouch;
}
activeShootTimer.start();
}
else
{
float ballReachTimeTmp = calculateBallRobotReachTime(attack, &VELOCITY_PROFILE_MAMOOLI) * 1.5;
TVec2 ballReachPlace = predictBallForwardAI(ballReachTimeTmp);
float ballGoalDot = (Dot(Normalize(Vec2(ball.velocity.x, ball.velocity.y)), Normalize(Vec2(-side*field_width, 0)-ballReachPlace)));
if ( 0)//ballGoalDot > -0.6 && ballGoalDot < 0.7 && ball.velocity.magnitude > 900 )
{
float passAngle = ball.velocity.direction;
tech_circle(attack, passAngle, 1, 0, 1, 0, 0, 1);
}
else
{
TVec2 openAngle = calculateOpenAngleToGoal(ball.Position, attack);
bool mid1Reached = OwnRobot[mid1].State.velocity.magnitude < 50;
bool mid2Reached = OwnRobot[mid2].State.velocity.magnitude < 50;
bool mid1DisOk = DIS(OwnRobot[mid1].State.Position, ball.Position) > 2000;
bool mid2DisOk = DIS(OwnRobot[mid2].State.Position, ball.Position) > 2000;
bool mid1Seen = OwnRobot[mid1].State.seenState != CompletelyOut;
bool mid2Seen = OwnRobot[mid2].State.seenState != CompletelyOut;
bool mid1Suitable = mid1Seen && mid1Reached && mid1DisOk;
bool mid2Suitable = mid2Seen && mid2Reached && mid2DisOk;
if (mid1Suitable && mid2Suitable)
{
if(-side*OwnRobot[mid1].State.Position.X > -side*OwnRobot[mid2].State.Position.X)
mid2Suitable = false;
else
mid1Suitable = false;
}
if ( openAngle.Y < 2 && (mid1Suitable||mid2Suitable) && (findKickerOpp(-1)==-1) )//&& ( ball.Position.X * side < -2300 ) && ( fabs ( ball.Position.Y ) > 1800 ) )
{
//float passAngle = AngleWith ( OwnRobot[randomParam<0.3?dmf:(randomParam<0.6?rmf:lmf)].State.Position , ball.Position );
float passAngle = AngleWith ( Vec2 ( -side*1700 , -sgn ( ball.Position.Y ) * 1700 ) , ball.Position );
float chip_pow = 40;
if ( mid1Suitable )
{
passAngle = AngleWith ( OwnRobot[mid1].State.Position , ball.Position );
chip_pow = 50.0 * DIS(OwnRobot[mid1].State.Position, ball.Position) / 2000.0f;
}
else if ( mid2Suitable )
{
passAngle = AngleWith ( OwnRobot[mid2].State.Position , ball.Position );
chip_pow = 50.0 * DIS(OwnRobot[mid2].State.Position, ball.Position) / 2000.0f;
}
else
{
passAngle = AngleWith ( Vec2 ( -side*1700 , -sgn ( ball.Position.Y ) * 1700 ) , ball.Position );
chip_pow = 1;
}
tech_circle(attack, passAngle, 0, chip_pow, 1, 0, 0, 1);
}
else {
float shootAngle;
if ( openAngle.Y > 10 )
shootAngle = NormalizeAngle( 180+openAngle.X);
else
shootAngle = AngleWith ( Vec2 ( -side*field_width , 0 ) , ball.Position );
float shoot_pow = 80 - OwnRobot[attack].State.velocity.magnitude * 0.01;
//if ( openAngle.Y < 2 )
// shoot_pow = 0;
if (DIS(OwnRobot[attack].State.Position,ball.Position) > 400 ) {
shoot_pow = 1;
activeShootTimer.start();
}
else if (goal_blocked(ball.Position, 200, 90)) {
shoot_pow = 1;
}
else
{
//if ( activeShootTimer.time()<0.3 )
//{
// shoot_pow = 1;
//}
}
//if (attackFuckingAngle()) {
// shootAngle = AngleWith(ball.Position, Vec2(side*field_width, 0));
// shoot_pow = 1;
//}
debugDraw = true;
tech_circle(attack, shootAngle, shoot_pow, 0, 1, 0, 0, 0);
//circle_ball(attack, 90, 80, 0, 1.0f);
debugDraw = false;
}
}
}
if (ball.Position.Y>600) {
recievePass(mid1, Vec2 ( -side*250 , 0 ));
}
else {
recievePass(mid1, Vec2 ( -side*(field_width-800) , field_height-800 ));
}
if (ball.Position.Y<-600) {
recievePass(mid2, Vec2 ( -side*250 , 0 ));
}
else {
recievePass(mid2, Vec2 ( -side*(field_width-800) ,-field_height+800 ));
}
}
void ai09::circle_ball ( int robot_num , float tagret_angle , int shoot_pow , int chip_pow , float precision , float near_dis_override )
{
//tagret_angle -= 5;
const float very_far_ball_dis = 600.0f;
const float far_ball_dis = 160.0f;
const int far_to_near_hys = 5;
float near_ball_dis = 140.0f;
if ( near_dis_override > 0 )
near_ball_dis = near_dis_override;
const float near_angle_tol = 4.0f;
const int near_to_kick_hys = 3;
const float shmit_coeff = 1.2f;
static ball_circling_state state = very_far;
static float last_change_t = 0.0f;
static int hys_bank[4]={0,0,0,0};
if (timer.time()<0.1) {
state = very_far;
last_change_t = timer.time();
hys_bank[0]=hys_bank[1]=hys_bank[2]=hys_bank[3]=0;
Halt(robot_num);
return;
}
if (state == very_far) {
OwnRobot[robot_num].face(ball.Position);
ERRTSetObstacles(robot_num, 0, 1, 1, 1, 0, 1);
ERRTNavigate2Point(robot_num, ball.Position, 1, 30, &VELOCITY_PROFILE_AROOM);
AddDebugCircle(ball.Position,very_far_ball_dis-90.0f,Red);
if (DIS(OwnRobot[robot_num].State.Position, ball.Position) < very_far_ball_dis) {
state = far;
last_change_t = timer.time();
}
}
else if (state == far) {
OwnRobot[robot_num].face(ball.Position);
ERRTSetObstacles(robot_num, 0, 1, 1, 1, 0, 1);
TVec2 target_point = CircleAroundPoint(ball.Position, AngleWith(ball.Position, OwnRobot[robot_num].State.Position), near_ball_dis);
ERRTNavigate2Point(robot_num, target_point, 1, 20, &VELOCITY_PROFILE_AROOM);
AddDebugCircle(ball.Position,far_ball_dis-90.0f,Pink);
if (DIS(OwnRobot[robot_num].State.Position, ball.Position) < far_ball_dis) {
hys_bank[0]++;
}
else {
hys_bank[0]=0;
}
if (hys_bank[0] > far_to_near_hys ) {
state = near;
last_change_t = timer.time();
}
else if (DIS(OwnRobot[robot_num].State.Position, ball.Position) > very_far_ball_dis * shmit_coeff) {
state = very_far;
last_change_t = timer.time();
}
}
else if (state == near) {
float toRobot = AngleWith(ball.Position, OwnRobot[robot_num].State.Position);
float newToRobot = NormalizeAngle(toRobot-tagret_angle);
float deltaAngle = min(fabs(newToRobot), 30.0f);
newToRobot = max(0.0f,fabs(newToRobot)-deltaAngle)*sgn(newToRobot);
newToRobot = NormalizeAngle(newToRobot+tagret_angle);
OwnRobot[robot_num].face(ball.Position);
OwnRobot[robot_num].target.Angle += NormalizeAngle(newToRobot+180.0f-OwnRobot[robot_num].target.Angle)/2.0f;
//OwnRobot[robot_num].target.Angle = NormalizeAngle(OwnRobot[robot_num].target.Angle);
ERRTSetObstacles(robot_num, 0, 1, 1, 1, 0, 1);
TVec2 target_point = CircleAroundPoint(ball.Position, newToRobot, near_ball_dis/cosDeg(deltaAngle));
if ( near_dis_override > 0 )
ERRTNavigate2Point(robot_num, target_point, 1, 20, &VELOCITY_PROFILE_AROOM);
else
ERRTNavigate2Point(robot_num, target_point, 1, 20, &VELOCITY_PROFILE_MAMOOLI);
if (DIS(OwnRobot[robot_num].State.Position, ball.Position) > far_ball_dis*shmit_coeff) {
state = far;
last_change_t = timer.time();
}
if (fabs(deltaAngle) < near_angle_tol) {
hys_bank[0]++;
}
else {
hys_bank[0]=0;
}
if ((hys_bank[0]>near_to_kick_hys)&&((shoot_pow>0)||(chip_pow>0))) {
state = kick;
last_change_t = timer.time();
}
}
else if (state == kick) {
if (chip_pow>0) {
chip_head = OwnRobot[robot_num].State.Angle;
}
//OwnRobot[robot_num].face(ball.Position);
OwnRobot[robot_num].target.Angle=NormalizeAngle(tagret_angle+180.0f);
ERRTSetObstacles(robot_num, 0, 1, 1, 1, 0, 1);
ERRTNavigate2Point(robot_num, ball.Position, 1, 100, &VELOCITY_PROFILE_AROOM);
if ( shoot_pow > 0 )
OwnRobot[robot_num].Shoot(shoot_pow);
if ( chip_pow > 0 )
OwnRobot[robot_num].Chip(chip_pow);
AddDebugCircle(ball.Position,very_far_ball_dis-90.0f,Red);
//tech_circle(robot_num, tagret_angle, shoot_pow, chip_pow, 1, 1, 0, 0);
if (DIS(OwnRobot[robot_num].State.Position, ball.Position) > near_ball_dis*shmit_coeff) {
state = far;
last_change_t = timer.time();
}
}
AddDebugLine(OwnRobot[robot_num].State.Position,OwnRobot[robot_num].target.Position,Black);
}
// Move actual view angles towards desired ones.
// This is the only place v_angle is altered.
// TODO: Make stiffness and turn rate constants timestep invariant.
void CCSBot::UpdateLookAngles()
{
const float deltaT = g_flBotCommandInterval;
float maxAccel;
float stiffness;
float damping;
// springs are stiffer when attacking, so we can track and move between targets better
if (IsAttacking())
{
stiffness = 300.0f;
damping = 30.0f;
maxAccel = 3000.0f;
}
else
{
stiffness = 200.0f;
damping = 25.0f;
maxAccel = 3000.0f;
}
// these may be overridden by ladder logic
float useYaw = m_lookYaw;
float usePitch = m_lookPitch;
// Ladders require precise movement, therefore we need to look at the
// ladder as we approach and ascend/descend it.
// If we are on a ladder, we need to look up or down to traverse it - override pitch in this case.
// If we're trying to break something, though, we actually need to look at it before we can
// look at the ladder
if (IsUsingLadder())
{
// set yaw to aim at ladder
Vector to = m_pathLadder->m_top - pev->origin;
float idealYaw = UTIL_VecToYaw(to);
NavDirType faceDir = m_pathLadder->m_dir;
if (m_pathLadderFaceIn)
{
faceDir = OppositeDirection(faceDir);
}
const float lookAlongLadderRange = 100.0f;
const float ladderPitch = 60.0f;
// adjust pitch to look up/down ladder as we ascend/descend
switch (m_pathLadderState)
{
case APPROACH_ASCENDING_LADDER:
{
Vector to = m_goalPosition - pev->origin;
useYaw = idealYaw;
if (to.IsLengthLessThan(lookAlongLadderRange))
usePitch = -ladderPitch;
break;
}
case APPROACH_DESCENDING_LADDER:
{
Vector to = m_goalPosition - pev->origin;
useYaw = idealYaw;
if (to.IsLengthLessThan(lookAlongLadderRange))
usePitch = ladderPitch;
break;
}
case FACE_ASCENDING_LADDER:
{
useYaw = idealYaw;
usePitch = -ladderPitch;
break;
}
case FACE_DESCENDING_LADDER:
{
useYaw = idealYaw;
usePitch = ladderPitch;
break;
}
case MOUNT_ASCENDING_LADDER:
case ASCEND_LADDER:
{
useYaw = DirectionToAngle(faceDir) + StayOnLadderLine(this, m_pathLadder);
usePitch = -ladderPitch;
break;
}
case MOUNT_DESCENDING_LADDER:
case DESCEND_LADDER:
{
useYaw = DirectionToAngle(faceDir) + StayOnLadderLine(this, m_pathLadder);
usePitch = ladderPitch;
break;
}
case DISMOUNT_ASCENDING_LADDER:
case DISMOUNT_DESCENDING_LADDER:
{
useYaw = DirectionToAngle(faceDir);
break;
}
}
}
// Yaw
float angleDiff = NormalizeAngle(useYaw - pev->v_angle.y);
// if almost at target angle, snap to it
const float onTargetTolerance = 1.0f;
if (angleDiff < onTargetTolerance && angleDiff > -onTargetTolerance)
{
m_lookYawVel = 0.0f;
pev->v_angle.y = useYaw;
}
else
{
// simple angular spring/damper
float accel = stiffness * angleDiff - damping * m_lookYawVel;
// limit rate
if (accel > maxAccel)
accel = maxAccel;
else if (accel < -maxAccel)
accel = -maxAccel;
m_lookYawVel += deltaT * accel;
pev->v_angle.y += deltaT * m_lookYawVel;
}
// Pitch
// Actually, this is negative pitch.
angleDiff = usePitch - pev->v_angle.x;
angleDiff = NormalizeAngle(angleDiff);
if (false && angleDiff < onTargetTolerance && angleDiff > -onTargetTolerance)
{
m_lookPitchVel = 0.0f;
pev->v_angle.x = usePitch;
}
else
{
// simple angular spring/damper
// double the stiffness since pitch is only +/- 90 and yaw is +/- 180
float accel = 2.0f * stiffness * angleDiff - damping * m_lookPitchVel;
// limit rate
if (accel > maxAccel)
accel = maxAccel;
else if (accel < -maxAccel)
accel = -maxAccel;
m_lookPitchVel += deltaT * accel;
pev->v_angle.x += deltaT * m_lookPitchVel;
}
// limit range - avoid gimbal lock
if (pev->v_angle.x < -89.0f)
pev->v_angle.x = -89.0f;
else if (pev->v_angle.x > 89.0f)
pev->v_angle.x = 89.0f;
pev->v_angle.z = 0.0f;
}
void ai09::kickoff_their_one_wall ( void )
{
//swap(dmf, lmf);
GKHi ( gk , 1 );
DefHi ( def );
ERRTSetObstacles ( dmf , true , true , true , true );
OwnRobot[dmf].face(ball.Position);
ERRTNavigate2Point ( dmf , PointOnConnectingLine(ball.Position, Vec2(side*field_width, 0), DIS(ball.Position, Vec2(side*field_width, 0))/2.0f) ,0 , 40,&VELOCITY_PROFILE_MAMOOLI);
int indexP = -1;
int indexN = -1;
for ( int i = 0 ; i < 12 ; i ++ )
{
if ( ( fabs ( OppRobot[i].Position.X ) < 600 ) && ( fabs ( OppRobot[i].Position.Y ) > 600 ) && ( OppRobot[i].seenState != CompletelyOut ) )
{
if ( OppRobot[i].Position.Y > 0 )
indexP = i;
if ( OppRobot[i].Position.Y < 0 )
indexN = i;
}
}
cout << indexN << " " << indexP << endl;
if ( indexN != -1 )
{
if ( side == -1 )
{
ERRTSetObstacles ( mid1 , true , true , true , false );
OwnRobot[mid1].face(Vec2(-side*field_width,0));
ERRTNavigate2Point ( mid1 , PointOnConnectingLine ( OppRobot[indexN].Position , Vec2(side*field_width,0) , (fabs(OppRobot[indexN].Position.X)+14)*1.5 ) );
markMap[&mid1] = indexN;
}
else
{
ERRTSetObstacles ( mid2 , true , true , true , false );
OwnRobot[mid2].face(Vec2(-side*field_width,0));
ERRTNavigate2Point ( mid2 , PointOnConnectingLine ( OppRobot[indexN].Position , Vec2(side*field_width,0) , (fabs(OppRobot[indexN].Position.X)+14)*1.5 ) );
markMap[&mid2] = indexN;
}
}
else
{
if ( side == -1 )
{
ERRTSetObstacles ( mid1 , true , true , true , true );
OwnRobot[mid1].face(Vec2(ball.Position.X,ball.Position.Y));
ERRTNavigate2Point ( mid1 , CircleAroundPoint(Vec2(ball.Position.X,ball.Position.Y),NormalizeAngle(20+AngleWith(ball.Position , Vec2(side*field_width,0))),790) ,0 , 100);
markMap[&mid1] = -1;
}
else
{
ERRTSetObstacles ( mid2 , true , true , true , true );
OwnRobot[mid2].face(Vec2(ball.Position.X,ball.Position.Y));
ERRTNavigate2Point ( mid2 , CircleAroundPoint(Vec2(ball.Position.X,ball.Position.Y),NormalizeAngle(-20+AngleWith(ball.Position , Vec2(side*field_width,0))),790) ,0 , 100);
markMap[&mid2] = -1;
}
}
if ( indexP != -1 )
{
if ( side == 1 )
{
ERRTSetObstacles ( mid1 , true , true , true , false );
OwnRobot[mid1].face(Vec2(-side*field_width,0));
ERRTNavigate2Point ( mid1 , PointOnConnectingLine ( OppRobot[indexP].Position , Vec2(side*field_width,0) , (fabs(OppRobot[indexP].Position.X)+14)*1.5 ) );
markMap[&mid1] = indexP;
}
else
{
ERRTSetObstacles ( mid2 , true , true , true , false );
OwnRobot[mid2].face(Vec2(-side*field_width,0));
ERRTNavigate2Point ( mid2 , PointOnConnectingLine ( OppRobot[indexP].Position , Vec2(side*2995,0) , (fabs(OppRobot[indexP].Position.X)+14)*1.5 ) );
markMap[&mid2] = indexP;
}
}
else
{
if ( side == 1 )
{
ERRTSetObstacles ( mid1 , true , true , true , true );
OwnRobot[mid1].face(Vec2(ball.Position.X,ball.Position.Y));
ERRTNavigate2Point ( mid1 , CircleAroundPoint(Vec2(ball.Position.X,ball.Position.Y),NormalizeAngle(20+AngleWith(ball.Position , Vec2(side*field_width,0))),790) ,0 , 100);
markMap[&mid1] = -1;
}
else
{
ERRTSetObstacles ( mid2 , true , true , true , true );
OwnRobot[mid2].face(Vec2(ball.Position.X,ball.Position.Y));
ERRTNavigate2Point ( mid2 , CircleAroundPoint(Vec2(ball.Position.X,ball.Position.Y),NormalizeAngle(-20+AngleWith(ball.Position , Vec2(side*field_width,0))),790) ,0 , 100);
markMap[&mid2] = -1;
}
}
DefenceWall(attack,true);
//swap(dmf, lmf);
}
/**
Controls the angle of the X and Y axis. Only one axis will be tilted from the
zero position at a time.
@param xyCurPos_px The current position of the ball in (x, y) (in pixels)
@return cv::Point A vector containing the desired angle for each axis
*/
cv::Point DualAxisController::AngleControl(cv::Point xyCurPos_px)
{
//NOTE: AngleControl is called once per frame
this->xyCurPos_px = xyCurPos_px;
ComputeError();
int xTiltAngle = outputZero;
int yTiltAngle = outputZero;
/* Like the single axis tilt controller, we want to tilt and wait for
specified periods. However, we also only want to control a single axis
at a time. */
// If we're already controlling X, keep controlling it.
if (controllingX)
{
// If X is within the minimum position error, stop controlling it,
// reset the X timers and do nothing else this frame.
if (abs(error.x) <= minimumPositionError)
{
controllingX = false;
this->tiltStartTick = 0;
}
else
{
// We're controlling X, it's outside of the minimum position error,
// let's move it.
xTiltAngle = GetTilt();
}
}
// else if we're controlling Y, keep controlling it.
else if (controllingY) {
// If Y is within min pos, stop controlling and reset Y timers.
if (abs(error.y) <= minimumPositionError)
{
controllingY = false;
this->tiltStartTick = 0;
}
else
{
// Move Y!
yTiltAngle = GetTilt();
}
}
// else if we're controlling neither, see which one (if any) is above the
// minimum position error, and start controlling it.
else if (abs(error.x) > minimumPositionError)
{
controllingX = true;
xTiltAngle = GetTilt();
}
else if (abs(error.y) > minimumPositionError)
{
controllingY = true;
yTiltAngle = GetTilt();
}
// else, we're at our desired position within +- the minimum error
xTiltAngle = NormalizeAngle(xTiltAngle);
yTiltAngle = NormalizeAngle(yTiltAngle);
return cv::Point(xTiltAngle, yTiltAngle);
}
static STATUS_T CmdHandLaidTurnout( wAction_t action, coOrd pos )
{
ANGLE_T angle, angle2, angle3, reverseR, pointA, reverseA1, angle0;
EPINX_T ep, ep1, ep2, ep2a=-1, ep2b=-1, pointEp0, pointEp1;
DIST_T dist, reverseD, pointD;
coOrd off, intersectP;
coOrd pointP, pointC, pointP1, reverseC, point0;
track_p trk, trk1, trk2, trk2a=NULL, trk2b=NULL, pointT;
trkSeg_p segP;
BOOL_T right;
track_p trks[4], *trkpp;
switch (action) {
case C_START:
InfoMessage( _("Place frog and drag angle") );
DYNARR_SET( trkSeg_t, tempSegs_da, 1 );
Dhlt.state = 0;
Dhlt.normalT = NULL;
tempSegs_da.cnt = 0;
DYNARR_SET( trkSeg_t, tempSegs_da, 2 );
tempSegs(0).color = drawColorBlack;
tempSegs(0).width = 0;
tempSegs(1).color = drawColorBlack;
tempSegs(1).width = 0;
return C_CONTINUE;
case C_DOWN:
if (Dhlt.state == 0) {
if ((Dhlt.normalT = OnTrack( &pos, TRUE, TRUE )) == NULL)
break;
if ( QueryTrack( Dhlt.normalT, Q_NOT_PLACE_FROGPOINTS ) ) {
ErrorMessage( MSG_CANT_PLACE_FROGPOINTS, _("frog") );
Dhlt.normalT = NULL;
break;
}
Dhlt.normalP = Dhlt.reverseP = Dhlt.reverseP1 = pos;
Dhlt.normalA = GetAngleAtPoint( Dhlt.normalT, Dhlt.normalP, NULL, NULL );
InfoMessage( _("Drag to set angle") );
DrawLine( &tempD, Dhlt.reverseP, Dhlt.reverseP1, 0, wDrawColorBlack );
Dhlt.state = 1;
pointC = pointP = pointP1 = reverseC = zero;
return C_CONTINUE;
}
case C_MOVE:
case C_UP:
if (Dhlt.normalT == NULL)
break;
if (Dhlt.state == 1) {
DrawLine( &tempD, Dhlt.reverseP, Dhlt.reverseP1, 0, wDrawColorBlack );
Dhlt.reverseP1 = pos;
Dhlt.reverseA = FindAngle( Dhlt.reverseP, Dhlt.reverseP1 );
Dhlt.frogA = NormalizeAngle( Dhlt.reverseA - Dhlt.normalA );
/*printf( "RA=%0.3f FA=%0.3f ", Dhlt.reverseA, Dhlt.frogA );*/
if (Dhlt.frogA > 270.0) {
Dhlt.frogA = 360.0-Dhlt.frogA;
right = FALSE;
} else if (Dhlt.frogA > 180) {
Dhlt.frogA = Dhlt.frogA - 180.0;
Dhlt.normalA = NormalizeAngle( Dhlt.normalA + 180.0 );
/*ep = Dhlt.normalEp0; Dhlt.normalEp0 = Dhlt.normalEp1; Dhlt.normalEp1 = ep;*/
right = TRUE;
} else if (Dhlt.frogA > 90.0) {
Dhlt.frogA = 180.0 - Dhlt.frogA;
Dhlt.normalA = NormalizeAngle( Dhlt.normalA + 180.0 );
/*ep = Dhlt.normalEp0; Dhlt.normalEp0 = Dhlt.normalEp1; Dhlt.normalEp1 = ep;*/
right = FALSE;
} else {
right = TRUE;
}
/*printf( "NA=%0.3f FA=%0.3f R=%d\n", Dhlt.normalA, Dhlt.frogA, right );*/
Dhlt.frogNo = tan(D2R(Dhlt.frogA));
if (Dhlt.frogNo > 0.01)
Dhlt.frogNo = 1.0/Dhlt.frogNo;
else
Dhlt.frogNo = 0.0;
if (action == C_MOVE) {
if (Dhlt.frogNo != 0) {
InfoMessage( _("Angle = %0.2f Frog# = %0.2f"), Dhlt.frogA, Dhlt.frogNo );
} else {
InfoMessage( _("Frog angle is too close to 0") );
}
} else {
InfoMessage( _("Select point position") );
Dhlt.state = 2;
Translate( &Dhlt.reverseP, Dhlt.reverseP, Dhlt.normalA+(right?+90:-90), trackGauge );
Translate( &Dhlt.reverseP1, Dhlt.reverseP1, Dhlt.normalA+(right?+90:-90), trackGauge );
}
DrawLine( &tempD, Dhlt.reverseP, Dhlt.reverseP1, 0, wDrawColorBlack );
return C_CONTINUE;
} else if ( Dhlt.state == 2 ) {
DrawSegs( &tempD, zero, 0.0, &tempSegs(0), tempSegs_da.cnt, trackGauge, wDrawColorBlack );
tempSegs_da.cnt = 0;
pointP = pos;
if ((pointT = OnTrack( &pointP, TRUE, TRUE )) == NULL)
break;
if ( QueryTrack( pointT, Q_NOT_PLACE_FROGPOINTS ) ) {
ErrorMessage( MSG_CANT_PLACE_FROGPOINTS, _("points") );
break;
}
dist = FindDistance( Dhlt.normalP, pointP );
pointA = GetAngleAtPoint( pointT, pointP, &pointEp0, &pointEp1 );
angle = NormalizeAngle( pointA + 180.0 - Dhlt.reverseA );
PTRACE(( "rA=%0.1f pA=%0.1f a=%0.1f ", Dhlt.reverseA, pointA, angle ))
if ( angle > 90.0 && angle < 270.0 ) {
pointA = NormalizeAngle( pointA + 180.0 );
angle = NormalizeAngle( angle + 180.0 );
PTRACE(( " {pA=%0.1f a=%0.1f} ", pointA, angle ))
} else {
