/** Navigates around (x, y). Clockwise iff radius > 0 */
void nav_circle_XY(float x, float y, float radius)
{
struct EnuCoor_f *pos = stateGetPositionEnu_f();
float last_trigo_qdr = nav_circle_trigo_qdr;
nav_circle_trigo_qdr = atan2f(pos->y - y, pos->x - x);
float sign_radius = radius > 0 ? 1 : -1;
if (nav_in_circle) {
float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr;
NormRadAngle(trigo_diff);
nav_circle_radians += trigo_diff;
trigo_diff *= - sign_radius;
if (trigo_diff > 0) { // do not rewind if the change in angle is in the opposite sense than nav_radius
nav_circle_radians_no_rewind += trigo_diff;
}
}
float dist2_center = DistanceSquare(pos->x, pos->y, x, y);
float dist_carrot = CARROT * NOMINAL_AIRSPEED;
radius += -nav_shift;
float abs_radius = fabs(radius);
/** Computes a prebank. Go straight if inside or outside the circle */
circle_bank =
(dist2_center > Square(abs_radius + dist_carrot)
|| dist2_center < Square(abs_radius - dist_carrot)) ?
0 :
atanf(stateGetHorizontalSpeedNorm_f() * stateGetHorizontalSpeedNorm_f() / (NAV_GRAVITY * radius));
float carrot_angle = dist_carrot / abs_radius;
carrot_angle = Min(carrot_angle, M_PI / 4);
carrot_angle = Max(carrot_angle, M_PI / 16);
float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle;
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
float radius_carrot = abs_radius;
if (nav_mode == NAV_MODE_COURSE) {
radius_carrot += (abs_radius / cosf(carrot_angle) - abs_radius);
}
fly_to_xy(x + cosf(alpha_carrot)*radius_carrot,
y + sinf(alpha_carrot)*radius_carrot);
nav_in_circle = true;
nav_circle_x = x;
nav_circle_y = y;
nav_circle_radius = radius;
}
bool CConvoy::IsAtCurrentWaypoint()
{
bool Result = false;
CVector3f Waypt = *(m_Waypoints[m_CurrentWaypointIndex].m_Frame.GetPosition());
CVector3f UnitPos = GetSphere()->Center;
Waypt.y = 0.0f;
UnitPos.y = 0.0f;
float Dist = DistanceSquare(&Waypt, &UnitPos);
if (Dist < 30.0f * 30.0f)
Result = true;
return Result;
}
/** Navigates around (x, y). Clockwise iff radius > 0 */
void nav_circle_XY(float x, float y, float radius) {
float last_trigo_qdr = nav_circle_trigo_qdr;
nav_circle_trigo_qdr = atan2(estimator_y - y, estimator_x - x);
if (nav_in_circle) {
float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr;
NormRadAngle(trigo_diff);
nav_circle_radians += trigo_diff;
}
float dist2_center = DistanceSquare(estimator_x, estimator_y, x, y);
float dist_carrot = CARROT*NOMINAL_AIRSPEED;
float sign_radius = radius > 0 ? 1 : -1;
radius += -nav_shift;
float abs_radius = fabs(radius);
/** Computes a prebank. Go straight if inside or outside the circle */
circle_bank =
(dist2_center > Square(abs_radius + dist_carrot)
|| dist2_center < Square(abs_radius - dist_carrot)) ?
0 :
atan(estimator_hspeed_mod*estimator_hspeed_mod / (G*radius));
float carrot_angle = dist_carrot / abs_radius;
carrot_angle = Min(carrot_angle, M_PI/4);
carrot_angle = Max(carrot_angle, M_PI/16);
float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle;
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
float radius_carrot = abs_radius;
if (nav_mode == NAV_MODE_COURSE)
radius_carrot += (abs_radius / cos(carrot_angle) - abs_radius);
fly_to_xy(x+cos(alpha_carrot)*radius_carrot,
y+sin(alpha_carrot)*radius_carrot);
nav_in_circle = TRUE;
nav_circle_x = x;
nav_circle_y = y;
nav_circle_radius = radius;
}
int CConvoy::TargetRelativePosition()
{
int TargetPosition = 0;
const Sphere* pMySphere = GetSphere();
CVector3f TargetOrigin;
CVector3f MyOrigin;
CVector3f PToP;
CVector3f MyLookAt;
CVector3f Result;
TargetOrigin = *(m_Waypoints[m_CurrentWaypointIndex].m_Frame.GetPosition());
MyOrigin = pMySphere->Center;
const CObjFrame* Frame = GetFrame(0);
Ray ShotTrajectory;
Sphere TargetSphere;
TargetSphere.Center = TargetOrigin;
float Scale = 1.0f;
float Dist = DistanceSquare(&MyOrigin, &TargetOrigin);
/*
if (Dist > 100.0f * 100.0f)
{
Scale = Dist / 100.0f * 100.0f ;
if (Scale > 5.0f)
Scale = 5.0f;
}
*/
TargetSphere.Radius = 40.0f * Scale;
Frame->GetPosition(&(ShotTrajectory.Origin));
Frame->GetForward(&(ShotTrajectory.Direction));
Vec3fNormalize(&(ShotTrajectory.Direction), &(ShotTrajectory.Direction));
if (RayToSphere(&ShotTrajectory, &TargetSphere))
return TargetPosition;
Vec3fSubtract(&PToP, &TargetOrigin, &MyOrigin);
Frame->GetForward(&MyLookAt);
MyLookAt.y = 0.0f;
PToP.y = 0.0f;
Vec3fCrossProduct(&Result, &MyLookAt, &PToP);
if (Result.y > 0.0f)
TargetPosition = 1;
else if (Result.y < 0.0f)
TargetPosition = -1;
return TargetPosition;
}
void CEnemy::TargetEnemy()
{
CGameObject* pCurrentObj = GetWhoIsAroundMe2(400.0f);
CGameObject* pFirstGuy = pCurrentObj;
m_CurrentTarget = &g_Player;
float Dist = DistanceSquare(&(GetSphere()->Center), &(g_Player.GetSphere()->Center));
float TempDist;
if (pCurrentObj != NULL)
{
do
{
switch(pCurrentObj->GetType())
{
case OBJ_BUILDING:
case OBJ_BUILDING2:
case OBJ_BUILDING3:
{
TempDist = DistanceSquare(&(GetSphere()->Center), &(pCurrentObj->GetSphere()->Center));
if (TempDist < Dist)
{
m_CurrentTarget = pCurrentObj;
Dist = TempDist;
}
break;
};
case OBJ_FTURRET:
{
TempDist = DistanceSquare(&(GetSphere()->Center), &(pCurrentObj->GetSphere()->Center));
if (TempDist < Dist)
{
m_CurrentTarget = pCurrentObj;
Dist = TempDist;
}
break;
};
case OBJ_CONVOY:
{
TempDist = DistanceSquare(&(GetSphere()->Center), &(pCurrentObj->GetSphere()->Center));
if (TempDist < Dist)
{
m_CurrentTarget = pCurrentObj;
Dist = TempDist;
}
break;
};
default:
break;
};
pCurrentObj = pCurrentObj->m_GuyNextToMe;
} while (pCurrentObj != NULL);
}
ClearWhoIsAroundMe(pFirstGuy);
}
Type Distance(const Vector& b) const {
return std::sqrt(DistanceSquare(b));
}
double Distance(const Vector2D& vecA, const Vector2D& vecB) {
return sqrt( DistanceSquare(vecA, vecB) );
}
